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

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

Keyword: 平方根共分散フィルタ, カルマンフィルタ, 観測更新, 時間更新

概要

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

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

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

入力データ

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

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

nag_kalman_sqrt_filt_cov_invar (g13ebc) Example Program Data
Example 1
   4     2     2   0.0
   0.0000  0.0000  0.0000  0.0000
   0.0000  0.0000  0.0000  0.0000
   0.0000  0.0000  0.0000  0.0000
   0.0000  0.0000  0.0000  0.0000
   0.2113  0.8497  0.7263  0.0000
   0.7560  0.6857  0.1985  0.6525
   0.0002  0.8782  0.5442  0.3076
   0.3303  0.0683  0.2320  0.9329
   0.5618  0.5042
   0.5896  0.3493
   0.6853  0.3873
   0.8906  0.9222
   1.0000  0.0000
   0.0000  1.0000
   0.3616  0.0000  0.0000  0.0000
   0.2922  0.4826  0.0000  0.0000
   0.9488  0.0000
   0.3760  0.7340
Example 2
   4     2     2     0.0
   1.0000  0.0000  0.0000  0.0000
   0.8400  0.9010  0.0000  0.0000
   0.3000  0.7001  0.8300  0.0000
   0.5000  0.2300  0.1100  0.4303
   0.2113  0.8497  0.7263  0.8833
   0.7560  0.6857  0.1985  0.6525
   0.0002  0.8782  0.5442  0.3076
   0.3303  0.0683  0.2320  0.9329
   0.5618  0.5042
   0.5896  0.3493
   0.6853  0.3873
   0.8906  0.9222
   1.0000  0.0000
   0.0000  1.0000
   0.3616  0.5664  0.5015  0.2693
   0.2922  0.4826  0.4368  0.6325
   0.9488  0.0000
   0.3760  0.7340

１行目はタイトル行で読み飛ばされます。
２～２１行目に例１の入力データを指定しています。
３行目に状態の次元(n)、入力の次元(m)、出力の次元(p)を指定しています。４番目の値(tol)は行列K（カルマンフィルタゲイン行列と状態遷移行列の積）を定義しない場合は指定する必要はありません。
４～７行目は状態共分散行列の下三角コレスキー因子(s)を指定しています。
８～１１行目は下オブサーバヘッセンベルグ行列 UAU^T（A は状態遷移行列、U はユニタリ行列）を指定しています。
１２～１５行目はユニタリ変換と入力重み行列の積(b)を指定しています。
１６～１７行目は下三角コレスキー因子プロセスノイズ共分散行列(q)を指定しています。
１８～１９行目は下オブサーバヘッセンベルグ行列 CU^T（C は出力重み行列、U はユニタリ行列）を指定しています。
２０～２１行目は観測ノイズ共分散行列の下三角コレスキー因子(r)を指定しています。
２２～４１行目に例２の入力データを指定しています。
２３行目に状態の次元(n)、入力の次元(m)、出力の次元(p)を指定しています。４番目の値(tol)は行列K（カルマンフィルタゲイン行列と状態遷移行列の積）を定義しない場合は指定する必要はありません。
２４～２７行目に状態共分散行列の下三角コレスキー因子(s)を指定しています。
２８～３１行目に下オブサーバヘッセンベルグ行列UAU^T（A は状態遷移行列、U はユニタリ行列）を指定しています。
３２～３５行目にユニタリ変換と入力重み行列の積を指定(b)しています。
３６～３７行目に下三角コレスキー因子プロセスノイズ共分散行列(q)を指定しています。
３８～３９行目に下オブサーバヘッセンベルグ行列CU^T（Cは出力重み行列、Uはユニタリ行列）を指定しています。
４０～４１行目に観測ノイズ共分散行列の下三角コレスキー因子(r)を指定しています。

出力結果

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

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

nag_kalman_sqrt_filt_cov_invar (g13ebc) Example Program Results

Example 1

The square root of the state covariance matrix is

 -1.7223   0.0000   0.0000   0.0000 
 -2.1073   0.5467   0.0000   0.0000 
 -1.7649   0.1412  -0.1710   0.0000 
 -1.8291   0.2058  -0.1497   0.7760 

The matrix AK (the product of the Kalman gain
matrix with the state transition matrix) is

 -0.2135   1.6649 
 -0.2345   2.1442 
 -0.2147   1.7069 
 -0.1345   1.4777 

Example 2

Covariance matrix PE from nag_kalman_sqrt_filt_cov_var (g13eac) is
 
  1.6761   1.4744   1.2519   1.6852 
  1.4744   1.3646   1.1367   1.4651 
  1.2519   1.1367   1.0668   1.3445 
  1.6852   1.4651   1.3445   2.2045 

Covariance matrix PF from nag_kalman_sqrt_filt_cov_invar (g13ebc) is
 
  5.0635  -1.5512   0.0231   1.1756 
 -1.5512   0.8503  -0.0492  -0.3631 
  0.0231  -0.0492   0.0648  -0.0217 
  1.1756  -0.3631  -0.0217   0.3336 

Matrix U' * PF * U is 
 
  1.6761   1.4744   1.2519   1.6852 
  1.4744   1.3646   1.1367   1.4651 
  1.2519   1.1367   1.0668   1.3445 
  1.6852   1.4651   1.3445   2.2045 

The matrix KE from nag_kalman_sqrt_filt_cov_var (g13eac) is
 
  0.3699   0.9447 
  0.3526   0.8199 
  0.2783   0.5375 
  0.1588   0.6704 

The matrix KF from nag_kalman_sqrt_filt_cov_invar (g13ebc) is
 
 -0.5857  -1.4263 
 -0.0280   0.2239 
  0.0170   0.1200 
 -0.1405  -0.4519 

U' * KF is
 
  0.3699   0.9447 
  0.3526   0.8199 
  0.2783   0.5375 
  0.1588   0.6704

３～１８行目までは例１の出力結果です。
５～１０行目は状態共分散行列の平方根が出力されています。
１２～１８行目はカルマンゲイン行列と状態遷移行列の積が出力されています。
２０～６２行目までは例２の出力結果です。
２２～２７行目はnag_kalman_sqrt_filt_cov_var (g13eac)から得られた共分散行列PEが出力されています。
２９～３４行目はnag_kalman_sqrt_filt_cov_invar (g13ebc) から得られた共分散行列PFが出力されています。
３６～４１行目は行列U'x PF x Uが出力されています。
４３～４８行目はnag_kalman_sqrt_filt_cov_var (g13eac)から得られた行列KEが出力されています。
５０～５５行目はnag_kalman_sqrt_filt_cov_invar (g13ebc)から得られた行列KFが出力されています。
５７～６２行目は行列U' x KFが出力されています。

ソースコード

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

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

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

/* nag_kalman_sqrt_filt_cov_invar (g13ebc) 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;

  printf("nag_kalman_sqrt_filt_cov_invar (g13ebc) Example Program "
         "Results\n\n");

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

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

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

#define A(I, J) a[(I) *tda + J]
#define B(I, J) b[(I) *tdb + J]
#define C(I, J) c[(I) *tdc + J]
#define K(I, J) k[(I) *tdk + J]
#define Q(I, J) q[(I) *tdq + J]
#define R(I, J) r[(I) *tdr + J]
#define S(I, J) s[(I) *tds + J]

static int ex1()
{ /* simple example (matrices A and C are supplied in lower observer
     Hessenberg form) */
  Integer exit_status = 0, i, istep, j, m, n, p, tda, tdb, tdc, tdh, tdk, tdq;
  Integer tdr, tds;
  NagError fail;
  double *a = 0, *b = 0, *c = 0, *h = 0, *k = 0, *q = 0, *r = 0, *s = 0, tol;

  INIT_FAIL(fail);

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

  printf("Example 1\n");

  scanf("%ld%ld%ld%lf", &n, &m, &p, &tol);
  if (n >= 1 && m >= 1 && p >= 1) {
    if (!(a = nAG_ALLOC(n * n, double)) ||
        !(b = nAG_ALLOC(n * m, double)) ||
        !(c = nAG_ALLOC(p * n, double)) ||
        !(k = nAG_ALLOC(n * p, double)) ||
        !(q = nAG_ALLOC(m * m, double)) ||
        !(r = nAG_ALLOC(p * p, double)) ||
        !(s = nAG_ALLOC(n * n, double)) || !(h = nAG_ALLOC(n * p, double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
    tda = n;
    tdb = m;
    tdc = n;
    tdk = p;
    tdq = m;
    tdr = p;
    tds = n;
    tdh = p;
  }
  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", &S(i, j));
  for (i = 0; i < n; ++i)
    for (j = 0; j < n; ++j)
      scanf("%lf", &A(i, j));
  for (i = 0; i < n; ++i)
    for (j = 0; j < m; ++j)
      scanf("%lf", &B(i, j));

  if (q) {
    for (i = 0; i < m; ++i)
      for (j = 0; j < m; ++j)
        scanf("%lf", &Q(i, j));
  }
  for (i = 0; i < p; ++i)
    for (j = 0; j < n; ++j)
      scanf("%lf", &C(i, j));
  for (i = 0; i < p; ++i)
    for (j = 0; j < p; ++j)
      scanf("%lf", &R(i, j));

  /* Perform three iterations of the Kalman filter recursion  */
  for (istep = 1; istep <= 3; ++istep)
    /* nag_kalman_sqrt_filt_cov_invar (g13ebc).
     * One iteration step of the time-invariant Kalman filter
     * recursion using the square root covariance implementation
     * with (AC) in lower observer Hessenberg form
     */
    nag_kalman_sqrt_filt_cov_invar(n, m, p, s, tds, a, tda, b, tdb, q, tdq,
                                   c, tdc, r, tdr, k, tdk, h, tdh, tol,
                                   &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_kalman_sqrt_filt_cov_invar (g13ebc).\n%s\n",
           fail.message);
    exit_status = 1;
    goto END;
  }

  printf("\nThe 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 ", S(i, j));
    printf("\n");
  }
  if (k) {
    printf("\nThe matrix AK (the product of the Kalman gain\n");
    printf("matrix with the state transition matrix) is\n\n");
    for (i = 0; i < n; ++i) {
      for (j = 0; j < p; ++j)
        printf("%8.4f ", K(i, j));
      printf("\n");
    }
  }
END:
  nAG_FREE(a);
  nAG_FREE(b);
  nAG_FREE(c);
  nAG_FREE(k);
  nAG_FREE(q);
  nAG_FREE(r);
  nAG_FREE(s);
  nAG_FREE(h);

  return exit_status;
}

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

#define UB(I, J)    ub[(I) *tdub + J]
#define SF(I, J)    sf[(I) *tdsf + J]
#define SE(I, J)    se[(I) *tdse + J]
#define PF(I, J)    pf[(I) *tdpf + J]
#define UAUT(I, J)  uaut[(I) *tduaut + J]
#define CUT(I, J)   cut[(I) *tdcut + J]
#define U(I, J)     u[(I) *tdu + J]

static int ex2()
{ /* more general example which requires the data to be transformed. The
     results produced by nag_kalman_sqrt_filt_cov_var (g13eac) and
     nag_kalman_sqrt_filt_cov_invar (g13ebc) are compared */
  Integer exit_status = 0, i, istep, j, m, n, p, tda, tdb;
  Integer tdc, tdcut, tdh, tdke, tdkf, tdpe, tdpf, tdq, tdr, tdrwork, tdse;
  Integer tdsf, tdu, tduaut, tdub;
  NagError fail;
  Nag_ObserverForm reduceto = Nag_LH_Observer;
  double *a = 0, *b = 0, *c = 0, *cut = 0, *h = 0;
  double *ke = 0, *kf = 0, one = 1.0, *pe = 0, *pf = 0, *q = 0;
  double *r = 0, *rwork = 0, *se = 0, *sf = 0, tol, *u = 0;
  double *uaut = 0, *ub = 0, zero = 0.0;

  INIT_FAIL(fail);

  printf("\nExample 2\n\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 (!(a = nAG_ALLOC(n * n, double)) ||
        !(b = nAG_ALLOC(n * m, double)) ||
        !(c = nAG_ALLOC(p * n, double)) ||
        !(ke = nAG_ALLOC(n * p, double)) ||
        !(kf = nAG_ALLOC(n * p, double)) ||
        !(ub = nAG_ALLOC(n * m, double)) ||
        !(q = nAG_ALLOC(m * m, double)) ||
        !(r = nAG_ALLOC(p * p, double)) ||
        !(rwork = nAG_ALLOC(n * n, double)) ||
        !(sf = nAG_ALLOC(n * n, double)) ||
        !(se = nAG_ALLOC(n * n, double)) ||
        !(h = nAG_ALLOC(n * p, double)) ||
        !(pf = nAG_ALLOC(n * n, double)) ||
        !(pe = nAG_ALLOC(n * n, double)) ||
        !(uaut = nAG_ALLOC(n * n, double)) ||
        !(cut = nAG_ALLOC(p * n, double)) || !(u = nAG_ALLOC(n * n, double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
    tda = n;
    tdb = m;
    tdc = n;
    tdke = p;
    tdkf = p;
    tdub = m;
    tdq = m;
    tdr = p;
    tdrwork = n;
    tdsf = n;
    tdse = n;
    tdh = p;
    tdpf = n;
    tdpe = n;
    tduaut = n;
    tdcut = n;
    tdu = n;
  }
  else {
    printf("Invalid n or m or p.\n");
    exit_status = 1;
    return exit_status;
  }
  mat_io(n, n, se, tdse, read, "");
  mat_io(n, n, a, tda, read, "");
  mat_io(n, m, b, tdb, read, "");
  if (q)
    mat_io(m, m, q, tdq, read, "");
  mat_io(p, n, c, tdc, read, "");
  mat_io(p, p, r, tdr, read, "");
  for (i = 0; i < n; ++i) {
    for (j = 0; j < n; ++j) {
      if (i < p)
        CUT(i, j) = C(i, j);
      SF(i, j) = SE(i, j);
      UAUT(i, j) = A(i, j);
      U(i, j) = zero;
    }
    U(i, i) = one;
  }
  /* Set up the matrix pair (A,C) in the lower observer hessenberg form */
  /* nag_trans_hessenberg_observer (g13ewc).
   * Unitary state-space transformation to reduce (AC) to
   * lower or upper observer Hessenberg form
   */
  nag_trans_hessenberg_observer(n, p, reduceto, uaut, tduaut, cut, tdcut,
                                u, tdu, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_trans_hessenberg_observer (g13ewc).\n%s\n",
           fail.message);
    exit_status = 1;
    goto END;
  }
  for (j = 0; j < m; ++j)
    for (i = 0; i < n; ++i)
      nag_ddot(Nag_NoConj, n, 1.0, &U(i, 0), 1, 0.0, &B(0, j), tdb, &UB(i,j),
               &fail);

  /* Generate noise covariance matrices PE and PF = U * PE * U' */
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_Trans, n, n, n, one, se, tdse,
            se, tdse, zero, pe, tdpe, &fail);
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_Trans, n, n, n, one, pe, tdpe,
            u, tdu, zero, rwork, tdrwork, &fail);
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_NoTrans, n, n, n, one, u, tdu,
            rwork, tdrwork, zero, pf, tdpf, &fail);

  /* Now find the lower triangular (left) Cholesky factor of PF. */
  /* nag_dpotrf (f07fdc).
   * Cholesky factorization of real symmetric positive definite matrix.
   */
  nag_dpotrf(Nag_RowMajor, Nag_Lower, n, pf, tdpf, &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)
    for (j = 0; j <= i; ++j)
      SF(i, j) = PF(i, j);
  /* Perform three steps of the Kalman filter recursion */
  for (istep = 1; istep <= 3; ++istep) {
    /* nag_kalman_sqrt_filt_cov_var (g13eac).
     * One iteration step of the time-varying Kalman filter
     * recursion using the square root covariance implementation
     */
    nag_kalman_sqrt_filt_cov_var(n, m, p, se, tdse, a, tda, b, tdb, q,
                                 tdq, c, tdc, r, tdr, ke, tdke, h, tdh, tol,
                                 &fail);
    if (fail.code != NE_NOERROR) {
      printf("Error from nag_kalman_sqrt_filt_cov_var (g13eac).\n%s\n",
             fail.message);
      exit_status = 1;
      goto END;
    }
    /* nag_kalman_sqrt_filt_cov_invar (g13ebc), see above. */
    nag_kalman_sqrt_filt_cov_invar(n, m, p, sf, tdsf, uaut, tduaut, ub, tdub,
                                   q, tdq, cut, tdcut, r, tdr, kf, tdkf, h,
                                   tdh, tol, &fail);
    if (fail.code != NE_NOERROR) {
      printf("Error from nag_kalman_sqrt_filt_cov_invar (g13ebc).\n%s\n",
             fail.message);
      exit_status = 1;
      goto END;
    }
  }
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_Trans, n, n, n, one, se, tdse,
            se, tdse, zero, pe, tdpe, &fail);
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_Trans, n, n, n, one, sf, tdsf,
            sf, tdsf, zero, pf, tdpf, &fail);
  mat_io(n, n, pe, tdpe, print, "Covariance matrix PE from "
         "nag_kalman_sqrt_filt_cov_var (g13eac) is\n");
  mat_io(n, n, pf, tdpf, print, "Covariance matrix PF from "
         "nag_kalman_sqrt_filt_cov_invar (g13ebc) is\n");

  /* Calculate PF = U' * PF * U */
  nag_dgemm(Nag_RowMajor, Nag_NoTrans, Nag_NoTrans, n, n, n, one, pf, tdpf,
            u, tdu, zero, rwork, tdrwork, &fail);
  nag_dgemm(Nag_RowMajor, Nag_Trans, Nag_NoTrans, n, n, n, one, u, tdu,
            rwork, tdrwork, zero, pf, tdpf, &fail);
  mat_io(n, n, pf, tdpf, print, "Matrix U' * PF * U is \n");
  mat_io(n, p, ke, tdke, print,
         "The matrix KE from nag_kalman_sqrt_filt_cov_var (g13eac) is\n");
  mat_io(n, p, kf, tdkf, print,
         "The matrix KF from nag_kalman_sqrt_filt_cov_invar (g13ebc) is\n");

  /* calculate U' * K */
  nag_dgemm(Nag_RowMajor, Nag_Trans, Nag_NoTrans, n, p, n, one, u, tdu,
            kf, tdkf, zero, rwork, tdrwork, &fail);
  mat_io(n, p, rwork, tdrwork, print, "U' * KF is\n");

END:
  nAG_FREE(a);
  nAG_FREE(b);
  nAG_FREE(c);
  nAG_FREE(ke);
  nAG_FREE(kf);
  nAG_FREE(ub);
  nAG_FREE(q);
  nAG_FREE(r);
  nAG_FREE(rwork);
  nAG_FREE(sf);
  nAG_FREE(se);
  nAG_FREE(h);
  nAG_FREE(pf);
  nAG_FREE(pe);
  nAG_FREE(uaut);
  nAG_FREE(cut);
  nAG_FREE(u);

  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");
  }
  if (flag == print)
    printf("\n");
} /* mat_io */

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

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

概要

入力データ

出力結果

ソースコード