平方根情報フィルタを用いた時不変カルマンフィルタの観測更新と時間更新

ホーム > 製品 > nAG数値計算ライブラリ > サンプルソースコード集 > 平方根情報フィルタを用いた時不変カルマンフィルタの観測更新と時間更新 (C言語/C++)

Keyword: 平方根情報フィルタ, カルマンフィルタ, 観測更新, 時間更新

概要

本サンプルは平方根情報フィルタを用いた時不変カルマンフィルタの観測更新と時間更新を行うC言語によるサンプルプログラムです。本サンプルでは２つの例についてプログラムを実行しています。例１では上コントローラヘッセンベルグ形で与えられた時不変システムに対し、カルマンフィルタを３回繰り返し実行しています。例２では一般の時不変システムに対しカルマンフィルタを３回繰り返し実行しています。

カルマンフィルタのデータ　

※本サンプルはnAG Cライブラリに含まれる関数 nag_kalman_sqrt_filt_info_invar() のExampleコードです。本サンプル及び関数の詳細情報は nag_kalman_sqrt_filt_info_invar のマニュアルページをご参照ください。
ご相談やお問い合わせはこちらまで

入力データ

（本関数の詳細はnag_kalman_sqrt_filt_info_invar のマニュアルページを参照）

このデータをダウンロード

nag_kalman_sqrt_filt_info_invar (g13edc) Example Program Data
Example 1
   4     2     2   0.0
   0.2113  0.7560  0.0002  0.3303
   0.8497  0.6857  0.8782  0.0683
   0.7263  0.1985  0.5442  0.2320
   0.0000  0.6525  0.3076  0.9329
   0.3616  0.5664  0.5015  0.2693
   0.2922  0.4826  0.4368  0.6325
   1.0000  0.0000
   0.0000  1.0000
  -0.8805  1.3257
   0.0000  0.5207
   0.0000  0.0000
   0.0000  0.0000
   1.1159  0.2305
   0.0000  0.6597
   1.0000  0.0000  0.0000  0.0000
   0.0000  1.0000  0.0000  0.0000
   0.0000  0.0000  1.0000  0.0000
   0.0000  0.0000  0.0000  1.0000
   0.0019
   0.5075
   0.4076
   0.8408
   0.5017
   0.9128
   0.2129
   0.5591
Example 2
   4     2     2    0.0
   0.2113  0.7560  0.0002  0.3303
   0.8497  0.6857  0.8782  0.0683
   0.7263  0.1985  0.5442  0.2320
   0.8833  0.6525  0.3076  0.9329
   0.3616  0.5664  0.5015  0.2693
   0.2922  0.4826  0.4368  0.6325
   1.0000  0.0000
   0.0000  1.0000
  -0.8805  1.3257
   2.1039  0.5207
  -0.6075  1.0386
  -0.8531  1.1688
   1.1159  0.2305
   0.0000  0.6597
   1.0000  2.1000  0.1400   0.0000
   0.0000  0.6010  2.8000  -1.3400
   0.0000  0.0000  1.3000  -0.8000
   0.0000  0.0000  0.0000   1.4100
   0.0019
   0.5075
   0.4076
   0.8408
   0.5017
   0.9128
   0.2129
   0.5591

１行目はタイトル行で読み飛ばされます。
２～２９行目に例１の入力データを指定しています。
３行目に状態の次元(n)、入力の次元(m)、出力の次元(p)を指定しています。４番目の値(tol)は特異点近傍の検証に使用されます。
４～７行目は上コントローラヘッセンベルグ行列UA^-1U^T（A^-1 は状態遷移行列の逆行列、U はユニタリ行列）を指定しています。
８～９行目は出力重み行列とユニタリ行列の転置行列との積(c)を指定しています。
１０～１１行目は観測ノイズ共分散行列の逆行列の上三角コレスキー因子(rinv)を指定しています。
１２～１５行目は上コントローラヘッセンベルグ行列UA^-1B（A^-1 は状態遷移行列の逆行列、B は入力重み行列、U はユニタリ行列）を指定しています。
１６～１７行目はプロセスノイズ共分散行列の逆行列の上三角コレスキー因子(qinv)を指定しています。
１８～２１行目は状態共分散行列の逆行列の平方根(t)を指定しています。
２２～２３行目は状態プロセスノイズの平均値(z)を指定しています。
２４～２７行目は状態推定(x)を指定しています。
２８～２９行目は上三角行列と観測出力ベクトルの積(rinvy)を指定しています。
３０～４１行目に例２の入力データを指定しています。
３１行目に状態の次元(n)、入力の次元(m)、出力の次元(p)を指定しています。４番目の値(tol)は特異点近傍の検証に使用されます。
３２～３５行目は上コントローラヘッセンベルグ行列UA^-1U^T（A^-1 は状態遷移行列の逆行列、U はユニタリ行列）を指定しています。
３６～３７行目は出力重み行列とユニタリ行列の転置行列との積(c)を指定しています。
３８～３９行目は観測ノイズ共分散行列の逆行列の上三角コレスキー因子(rinv)を指定しています。
４０～４３行目は上コントローラヘッセンベルグ行列UA^-1B（A^-1 は状態遷移行列の逆行列、B は入力重み行列、U はユニタリ行列）を指定しています。
４４～４５行目はプロセスノイズ共分散行列の逆行列の上三角コレスキー因子(qinv)を指定しています。
４６～４９行目は状態共分散行列の逆行列の平方根(t)を指定しています。
５０～５１行目は状態プロセスノイズの平均値(z)を指定しています。
５２～５５行目は状態推定(x)を指定しています。
５６～５７行目は上三角行列と観測出力ベクトルの積(rinvy)を指定しています。

出力結果

（本関数の詳細はnag_kalman_sqrt_filt_info_invar のマニュアルページを参照）

この出力例をダウンロード

nag_kalman_sqrt_filt_info_invar (g13edc) Example Program Results

Example 1

The inverse of the square root of the state covariance matrix is 

 -0.8731  -1.1461  -1.0260  -0.8901 
  0.0000  -0.2763  -0.1929  -0.3763 
  0.0000   0.0000  -0.1110  -0.1051 
  0.0000   0.0000   0.0000   0.3120 

The components of the estimated filtered state are

   k       x(k)  
   0     -2.0688  
   1     -0.7814  
   2      2.2181  
   3      0.9298  


Example 2

Results from nag_kalman_sqrt_filt_info_var (g13ecc) 

The information matrix ig is
 
  0.4661   0.5290   0.4826   0.4134 
  0.5290   0.7196   0.6158   0.5657 
  0.4826   0.6158   0.5781   0.4776 
  0.4134   0.5657   0.4776   0.5825 

The components of the estimated filtered state are

  k       x(k)  
  0    -0.8369 
  1    -1.4649 
  2     1.4877 
  3     1.5276 

Results from nag_kalman_sqrt_filt_info_invar (g13edc) 

The information matrix ih is
 
  0.0399  -0.0805  -0.0137  -0.0174 
 -0.0805   2.1143   0.2453   0.0406 
 -0.0137   0.2453   0.0770  -0.0294 
 -0.0174   0.0406  -0.0294   0.1151 

The matrix u' * ih * u is
 
  0.4661   0.5290   0.4826   0.4134 
  0.5290   0.7196   0.6158   0.5657 
  0.4826   0.6158   0.5781   0.4776 
  0.4134   0.5657   0.4776   0.5825 

The components of the estimated filtered state are 

  k       x(k)  
  0    -0.8369 
  1    -1.4649 
  2     1.4877 
  3     1.5276

３～１８行目までは例１の出力結果です。
５～１０行目は状態共分散行列の平方根の逆数が出力されています。
１２～１８行目はフィルター処理された状態推定の構成要素が出力されています。
２１～６２行目までは例２の出力結果です。
２３行目にnag_kalman_sqrt_filt_info_var (g13ecc)からの出力結果であることが示されています。
２５～３０行目は情報行列 ig が出力されています。
３２～３８行目はフィルター処理された状態推定の構成要素が出力されています。
２３行目にnag_kalman_sqrt_filt_info_invar (g13edc)からの出力結果であることが示されています。
４２～４７行目は情報行列 ih が出力されています。
４９～５４行目は行列U'x ih x Uが出力されています。
５６～６２行目はフィルター処理された状態推定の構成要素が出力されています。

ソースコード

（本関数の詳細はnag_kalman_sqrt_filt_info_invar のマニュアルページを参照）

※本サンプルソースコードはnAG数値計算ライブラリ（Windows, Linux, MAC等に対応）の関数を呼び出します。
サンプルのコンパイル及び実行方法

このソースコードをダウンロード

/* nag_kalman_sqrt_filt_info_invar (g13edc) Example Program.
 *
 * CLL6I261D/CLL6I261DL Version.
 *
 * Copyright 2017 Numerical Algorithms Group.
 *
 * Mark 26.1, 2017.
 */

#include <nag.h>
#include <stdio.h>
#include <nag_stdlib.h>
#include <nagf07.h>
#include <nagf16.h>
#include <nagg13.h>

typedef enum
{ read, print } ioflag;

static int ex1(void);
static int ex2(void);

int main(void)
{
  Integer exit_status_ex1 = 0;
  Integer exit_status_ex2 = 0;

  /* Skip the heading in the data file   */
  scanf("%*[^\n] ");

  printf("nag_kalman_sqrt_filt_info_invar (g13edc) Example Program "
         "Results\n\n");

  exit_status_ex1 = ex1();
  exit_status_ex2 = ex2();

  return (exit_status_ex1 == 0 && exit_status_ex2 == 0) ? 0 : 1;
}

#define AINV(I, J)  ainv[(I) *tdainv + J]
#define QINV(I, J)  qinv[(I) *tdqinv + J]
#define RINV(I, J)  rinv[(I) *tdrinv + J]
#define T(I, J)     t[(I) *tdt + J]
#define AINVB(I, J) ainvb[(I) *tdainvb + J]
#define C(I, J)     c[(I) *tdc + J]

static int ex1(void)
{
  Integer exit_status = 0, i, istep, j, m, n, p, tdainv, tdainvb, tdc, tdqinv;
  Integer tdrinv, tdt;
  double *ainv = 0, *ainvb = 0, *c = 0, *qinv = 0, *rinv = 0, *rinvy = 0;
  double *t = 0, tol, *x = 0, *z = 0;

  /* Nag Types */
  NagError fail;

  INIT_FAIL(fail);

  printf("Example 1\n");

  /* Skip the heading in the data file   */
  scanf("%*[^\n]");

  scanf("%ld%ld%ld%lf", &n, &m, &p, &tol);
  if (n >= 1 || m >= 1 || p >= 1) {
    if (!(ainv = nAG_ALLOC(n * n, double)) ||
        !(qinv = nAG_ALLOC(m * m, double)) ||
        !(rinv = nAG_ALLOC(p * p, double)) ||
        !(t = nAG_ALLOC(n * n, double)) ||
        !(ainvb = nAG_ALLOC(n * m, double)) ||
        !(c = nAG_ALLOC(p * n, double)) ||
        !(x = nAG_ALLOC(n, double)) ||
        !(z = nAG_ALLOC(m, double)) || !(rinvy = nAG_ALLOC(p, double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
    tdainv = n;
    tdqinv = m;
    tdrinv = p;
    tdt = n;
    tdainvb = m;
    tdc = n;
  }
  else {
    printf("Invalid n or m or p.\n");
    exit_status = 1;
    return exit_status;
  }

  /* Read data */
  for (i = 0; i < n; ++i)
    for (j = 0; j < n; ++j)
      scanf("%lf", &AINV(i, j));
  for (i = 0; i < p; ++i)
    for (j = 0; j < n; ++j)
      scanf("%lf", &C(i, j));
  if (rinv)
    for (i = 0; i < p; ++i)
      for (j = 0; j < p; ++j)
        scanf("%lf", &RINV(i, j));
  for (i = 0; i < n; ++i)
    for (j = 0; j < m; ++j)
      scanf("%lf", &AINVB(i, j));
  for (i = 0; i < m; ++i)
    for (j = 0; j < m; ++j)
      scanf("%lf", &QINV(i, j));
  for (i = 0; i < n; ++i)
    for (j = 0; j < n; ++j)
      scanf("%lf", &T(i, j));
  for (j = 0; j < m; ++j)
    scanf("%lf", &z[j]);
  for (j = 0; j < n; ++j)
    scanf("%lf", &x[j]);
  for (j = 0; j < p; ++j)
    scanf("%lf", &rinvy[j]);

  /* Perform three iterations of the Kalman filter recursion  */

  for (istep = 1; istep <= 3; ++istep)
    /* nag_kalman_sqrt_filt_info_invar (g13edc).
     * One iteration step of the time-invariant Kalman filter
     * recursion using the square root information
     * implementation with (A^(-1)(A^(-1)B)) in upper
     * controller Hessenberg form
     */
    nag_kalman_sqrt_filt_info_invar(n, m, p, t, tdt, ainv,
                                    tdainv, ainvb, tdainvb, rinv,
                                    tdrinv, c, tdc, qinv,
                                    tdqinv, x, rinvy, z, tol, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_kalman_sqrt_filt_info_invar (g13edc).\n%s\n",
           fail.message);
    exit_status = 1;
    goto END;
  }
  printf("\nThe inverse of the square root of the state covariance "
         "matrix is \n\n");

  for (i = 0; i < n; ++i) {
    for (j = 0; j < n; ++j)
      printf("%8.4f ", T(i, j));
    printf("\n");
  }
  printf("\nThe components of the estimated filtered state are\n\n");
  printf("   k       x(k)  \n");
  for (i = 0; i < n; ++i) {
    printf("   %ld  ", i);
    printf("  %8.4f  \n", x[i]);
  }

END:
  nAG_FREE(ainv);
  nAG_FREE(qinv);
  nAG_FREE(rinv);
  nAG_FREE(t);
  nAG_FREE(ainvb);
  nAG_FREE(c);
  nAG_FREE(x);
  nAG_FREE(z);
  nAG_FREE(rinvy);

  return exit_status;
}

static void mat_io(Integer n, Integer m, double mat[], Integer tdmat,
                   ioflag flag, const char *message);

#define AINV(I, J)   ainv[(I) *tdainv + J]
#define AINVB(I, J)  ainvb[(I) *tdainvb + J]
#define AINVU(I, J)  ainvu[(I) *tdainvu + J]
#define AINVBU(I, J) ainvbu[(I) *tdainvbu + J]
#define TU(I, J)     tu[(I) *tdtu + J]
#define IH(I, J)     ih[(I) *tdih + J]
#define U(I, J)      u[(I) *tdu + J]

static int ex2(void)
{
  Integer exit_status = 0, i, ione = 1, istep, j, m, n, p, tdainv, tdainvb;
  Integer tdainvbu, tdainvu, tdc, tdcu, tdig, tdih, tdqinv, tdrinv, tdrwork;
  Integer tdt, tdtu, tdu;
  Nag_ControllerForm reduceto = Nag_UH_Controller;
  Nag_ab_input inp_ab = Nag_ab_prod;
  double *ainv = 0, *ainvb = 0, *ainvbu = 0, *ainvu = 0, *c = 0;
  double *cu = 0, *ig = 0, *ih = 0, one = 1.0;
  double *qinv = 0, *rinv = 0, *rinvy = 0, *rwork = 0, *t = 0;
  double tol, *tu = 0, *u = 0, *ux = 0, *x = 0, *z = 0, zero = 0.0;

  /* Nag Types */
  NagError fail;

  INIT_FAIL(fail);

  printf("\n\nExample 2\n");

  /* skip the heading in the data file */
  scanf(" %*[^\n]");

  scanf("%ld%ld%ld%lf", &n, &m, &p, &tol);
  if (n >= 1 || m >= 1 || p >= 1) {
    if (!(ainv = nAG_ALLOC(n * n, double)) ||
        !(ainvb = nAG_ALLOC(n * m, double)) ||
        !(c = nAG_ALLOC(p * n, double)) ||
        !(ainvu = nAG_ALLOC(n * n, double)) ||
        !(ainvbu = nAG_ALLOC(n * m, double)) ||
        !(qinv = nAG_ALLOC(m * m, double)) ||
        !(rinv = nAG_ALLOC(p * p, double)) ||
        !(t = nAG_ALLOC(n * n, double)) ||
        !(x = nAG_ALLOC(n, double)) ||
        !(z = nAG_ALLOC(n, double)) ||
        !(rwork = nAG_ALLOC(n * n, double)) ||
        !(tu = nAG_ALLOC(n * n, double)) ||
        !(rinvy = nAG_ALLOC(p, double)) ||
        !(ig = nAG_ALLOC(n * n, double)) ||
        !(ih = nAG_ALLOC(n * n, double)) ||
        !(cu = nAG_ALLOC(p * n, double)) ||
        !(u = nAG_ALLOC(n * n, double)) || !(ux = nAG_ALLOC(n, double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
    tdainv = n;
    tdainvb = m;
    tdc = n;
    tdainvu = n;
    tdainvbu = m;
    tdqinv = m;
    tdrinv = p;
    tdt = n;
    tdrwork = n;
    tdtu = n;
    tdig = n;
    tdih = n;
    tdcu = n;
    tdu = n;
  }
  else {
    printf("Invalid n or m or p.\n");
    exit_status = 1;
    return exit_status;
  }

  /* Read data */
  mat_io(n, n, ainv, tdainv, read, "");
  mat_io(p, n, c, tdc, read, "");
  if (rinv)
    mat_io(p, p, rinv, tdrinv, read, "");
  mat_io(n, m, ainvb, tdainvb, read, "");
  mat_io(m, m, qinv, tdqinv, read, "");
  mat_io(n, n, t, tdt, read, "");
  for (j = 0; j < m; ++j)
    scanf("%lf", &z[j]);
  for (j = 0; j < n; ++j)
    scanf("%lf", &x[j]);
  for (j = 0; j < p; ++j)
    scanf("%lf", &rinvy[j]);

  for (i = 0; i < n; ++i) { /* Initialize the identity matrix u */
    for (j = 0; j < n; ++j)
      U(i, j) = zero;
    U(i, i) = one;
  }

  /* Copy the arrays ainv[] and ainvb[] into ainvu[] and ainvbu[] */
  for (i = 0; i < n; ++i)
    for (j = 0; j < n; ++j)
      AINVU(j, i) = AINV(j, i);
  for (j = 0; j < m; ++j)
    for (i = 0; i < n; ++i)
      AINVBU(i, j) = AINVB(i, j);

  /* Transform (ainvu[],ainvbu[]) to reduceto controller Hessenberg form */
  /* nag_trans_hessenberg_controller (g13exc).
   * Unitary state-space transformation to reduce (BA) to
   * lower or upper controller Hessenberg form
   */
  nag_trans_hessenberg_controller(n, m, reduceto, ainvu, tdainvu, ainvbu,
                                  tdainvbu, u, tdu, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_trans_hessenberg_controller (g13exc).\n%s\n",
           fail.message);
    exit_status = 1;
    goto END;
  }

  /* Calculate the matrix cu = c*u'    */
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_Trans, p, n, n, one, c, tdc,
            u, tdu, zero, cu, tdcu, &fail);

  /* Calculate the vector ux = u*x     */
  nag_dgemv(Nag_RowMajor, Nag_NoTrans, n, n, one, u, tdu, x, ione,
            zero, ux, ione, &fail);

  /* Form the information matrices ih = u*ig*u' and ig = t'*t     */
  nag_dgemm(Nag_RowMajor, Nag_Trans, Nag_NoTrans, n, n, n, one, t, tdt,
            t, tdt, zero, ig, tdig, &fail);
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_Trans, n, n, n, one, ig, tdig,
            u, tdu, zero, rwork, tdrwork, &fail);
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_NoTrans, n, n, n, one, u, tdu,
            rwork, tdrwork, zero, ih, tdih, &fail);

  /* Now find the triangular (right) Cholesky factor of ih */
  /* nag_dpotrf (f07fdc).
   * Cholesky factorization of real symmetric positive definite matrix.
   */
  nag_dpotrf(Nag_RowMajor, Nag_Lower, n, ih, tdih, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_dpotrf (f07fdc).\n%s\n", fail.message);
    exit_status = 1;
    goto END;
  }
  for (i = 0; i < n; ++i) {
    TU(i, i) = IH(i, i);
    for (j = 0; j < i; ++j) {
      TU(j, i) = IH(i, j);
      TU(i, j) = zero;
    }
  }

  /* Do three iterations of the Kalman filter recursion  */
  for (istep = 1; istep <= 3; ++istep) {
    /* nag_kalman_sqrt_filt_info_var (g13ecc).
     * One iteration step of the time-varying Kalman filter
     * recursion using the square root information
     * implementation
     */
    nag_kalman_sqrt_filt_info_var(n, m, p, inp_ab, t, tdt, ainv,
                                  tdainv, ainvb, tdainvb, rinv, tdrinv,
                                  c, tdc, qinv, tdqinv, x,
                                  rinvy, z, tol, &fail);
    if (fail.code != NE_NOERROR) {
      printf("Error from nag_kalman_sqrt_filt_info_var (g13ecc).\n%s\n",
             fail.message);
      exit_status = 1;
      goto END;
    }
    /* nag_kalman_sqrt_filt_info_invar (g13edc), see above. */
    nag_kalman_sqrt_filt_info_invar(n, m, p, tu, tdtu, ainvu, tdainvu,
                                    ainvbu, tdainvbu, rinv, tdrinv, cu,
                                    tdcu, qinv, tdqinv, ux, rinvy,
                                    z, tol, &fail);
    if (fail.code != NE_NOERROR) {
      printf("Error from nag_kalman_sqrt_filt_info_invar (g13edc).\n%s\n",
             fail.message);
      exit_status = 1;
      goto END;
    }
  }

  /* Print Results */
  printf("\nResults from nag_kalman_sqrt_filt_info_var (g13ecc) \n\n");
  /* let ig = t' * t */
  nag_dgemm(Nag_RowMajor, Nag_Trans, Nag_NoTrans, n, n, n, one, t, tdt,
            t, tdt, zero, ig, tdig, &fail);
  mat_io(n, n, ig, tdig, print, "The information matrix ig is\n");
  printf("\nThe components of the estimated filtered state are\n\n");
  printf("  k       x(k)  \n");
  for (i = 0; i < n; ++i)
    printf("  %ld   %8.4f \n", i, x[i]);
  printf("\nResults from nag_kalman_sqrt_filt_info_invar (g13edc) \n\n");
  /* let ih = tu' * tu */
  nag_dgemm(Nag_RowMajor, Nag_Trans, Nag_NoTrans, n, n, n, one, tu, tdtu,
            tu, tdtu, zero, ih, tdih, &fail);
  mat_io(n, n, ih, tdih, print, "The information matrix ih is\n");

  /* Calculate ih = u'*ih*u   */
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_NoTrans, n, n, n, one, ih, tdih,
            u, tdu, zero, rwork, tdrwork, &fail);
  nag_dgemm(Nag_RowMajor, Nag_Trans, Nag_NoTrans, n, n, n, one, u, tdu,
            rwork, tdrwork, zero, ih, tdih, &fail);
  mat_io(n, n, ih, tdih, print, "\nThe matrix u' * ih * u is\n");

  /* Calculate x = u' * ux */
  nag_dgemv(Nag_RowMajor, Nag_Trans, n, n, one, u, tdu, ux, ione,
            zero, x, ione, &fail);
  printf("\nThe components of the estimated filtered state are \n\n");
  printf("  k       x(k)  \n");
  for (i = 0; i < n; ++i)
    printf("  %ld   %8.4f \n", i, x[i]);

END:
  nAG_FREE(ainv);
  nAG_FREE(ainvb);
  nAG_FREE(c);
  nAG_FREE(ainvu);
  nAG_FREE(ainvbu);
  nAG_FREE(qinv);
  nAG_FREE(rinv);
  nAG_FREE(t);
  nAG_FREE(x);
  nAG_FREE(z);
  nAG_FREE(rwork);
  nAG_FREE(tu);
  nAG_FREE(rinvy);
  nAG_FREE(ig);
  nAG_FREE(ih);
  nAG_FREE(cu);
  nAG_FREE(u);
  nAG_FREE(ux);

  return exit_status;
}

static void mat_io(Integer n, Integer m, double mat[], Integer tdmat,
                   ioflag flag, const char *message)
{
  Integer i, j;
#define MAT(I, J) mat[((I) -1) * tdmat + (J) -1]
  if (flag == print)
    printf("%s \n", message);
  for (i = 1; i <= n; ++i) {
    for (j = 1; j <= m; ++j) {
      if (flag == read)
        scanf("%lf", &MAT(i, j));
      if (flag == print)
        printf("%8.4f ", MAT(i, j));
    }
    if (flag == print)
      printf("\n");
  }
} /* mat_io */

平方根情報フィルタを用いた : 時不変カルマンフィルタの観測更新と時間更新

C言語によるサンプルソースコード : 使用関数名：nag_kalman_sqrt_filt_info_invar (g13edc)

概要

入力データ

出力結果

ソースコード