MATLAB Coder

最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

MATLAB のカルマン フィルター アルゴリズムの C コードの生成

この例では、ノイズを含む過去の測定値に基づいて移動する物体の位置を推測する MATLAB のカルマン フィルター関数 'kalmanfilter' の C コードを生成する方法を説明します。また、MATLAB でアルゴリズムの実行速度を上げるために、この MATLAB コードの MEX 関数を生成する方法も示します。

必要条件

この例を実行するには、C コンパイラをインストールしてから、'mex -setup' コマンドを使ってセットアップしなければなりません。詳細については、C コンパイラの設定C コンパイラの設定を参照してください。

新規フォルダーの作成と関連ファイルのコピー

以下のコードは、現在の作業フォルダー (pwd) 内にフォルダーを作成します。この新規フォルダーには、この例に関連するファイルのみが含められます。現在のフォルダーに影響を与えたくない (またはこのフォルダーにファイルを生成できない) 場合は、作業フォルダーを変更する必要があります。

コマンドの実行:新規フォルダーの作成と関連ファイルのコピー

coderdemo_setup('coderdemo_kalman_filter');

関数 'kalmanfilter' について

関数 'kalmanfilter'は、移動する物体の位置をその過去の値に基づいて予測します。この関数ではカルマン フィルター推定器が使用されますが、この推定器はノイズを含む一連の測定値から動的システムの状態を推定する再帰的な適応フィルターです。カルマン フィルターには、信号 / 画像処理、制御設計、および金融工学などの分野で使用される多様なアプリケーションがあります。

カルマン フィルター推定器のアルゴリズムについて

カルマン推定器は、カルマン状態ベクトルを計算、更新して位置ベクトルを計算します。状態ベクトルは、2 次元の直交座標空間に、位置 (x および y)、速度 (Vx Vy)、および加速 (Ax および Ay) の測定値を含む 6 行 1 列の列ベクトルです。以下のように古典的な運動の法則に基づいています。

$$\left\{ \begin{array}{rcl}
            X & = & X_0 + V_x dt    \\
            Y & = & Y_0 + V_y dt    \\
          V_x & = & V_{x0} + A_x dt \\
          V_y & = & V_{y0} + A_y dt \\
         \end{array} \right.
$$

これらの法則をキャプチャする反復式が、カルマン状態遷移行列 "A" に反映されます。約 10 行の MATLAB コードを作成するだけで、適応フィルターに関する多くの書籍に記載されている理論的な数式に基づいてカルマン推定器を実装できます。

type kalmanfilter.m
%   Copyright 2010 The MathWorks, Inc.
function y = kalmanfilter(z) 
%#codegen
dt=1;
% Initialize state transition matrix
A=[ 1 0 dt 0 0 0;...     % [x  ]            
       0 1 0 dt 0 0;...     % [y  ]
       0 0 1 0 dt 0;...     % [Vx]
       0 0 0 1 0 dt;...     % [Vy]
       0 0 0 0 1 0 ;...     % [Ax]
       0 0 0 0 0 1 ];       % [Ay]
H = [ 1 0 0 0 0 0; 0 1 0 0 0 0 ];    % Initialize measurement matrix
Q = eye(6);
R = 1000 * eye(2);
persistent x_est p_est                % Initial state conditions
if isempty(x_est)
    x_est = zeros(6, 1);             % x_est=[x,y,Vx,Vy,Ax,Ay]'
    p_est = zeros(6, 6);
end
% Predicted state and covariance
x_prd = A * x_est;
p_prd = A * p_est * A' + Q;
% Estimation
S = H * p_prd' * H' + R;
B = H * p_prd';
klm_gain = (S \ B)';
% Estimated state and covariance
x_est = x_prd + klm_gain * (z - H * x_prd);
p_est = p_prd - klm_gain * H * p_prd;
% Compute the estimated measurements
y = H * x_est;
end                % of the function

テスト データの読み込み

追跡する物体の位置は、'position.mat' と呼ばれる MAT ファイル内の直交座標空間に x および y 座標として記録されます。以下のコードは MAT ファイルを読み込んで位置のトレースをプロットします。テスト データには、カルマン フィルターが迅速に再調整を行って物体を追跡できるかどうかチェックするために使用する、2 回の位置の突然の変化または不連続が含まれています。

load position.mat
hold; grid;
for idx = 1: numPts
z = position(:,idx);
plot(z(1), z(2), 'bx');
axis([-1 1 -1 1]);
end
title('Test vector for the Kalman filtering, including 2 sudden discontinuities ');
xlabel('x-axis');ylabel('y-axis');
hold;
Current plot held
Current plot released

関数 'ObjTrack.m' の確認と実行

関数 'ObjTrack.m' はカルマン フィルター アルゴリズムを呼び出し、物体の軌跡を青で、カルマン フィルターの推定位置を緑でプロットします。最初、推定位置が物体の実際の位置と収束するまで少し時間がかかります。その後、位置の突然の変化が 3 回発生します。そのたびに、カルマン フィルターは再調整を行い、数回の反復の後、物体を追跡します。

type ObjTrack
ObjTrack(position)
%   Copyright 2010 The MathWorks, Inc.
function ObjTrack(position)
%#codegen
% First, setup the figure
numPts = 300;               % Process and plot 300 samples
figure;hold;grid;            % Prepare plot window
% Main loop
for idx = 1: numPts
    z = position(:,idx);     % Get the input data
    y = kalmanfilter(z);        % Call Kalman filter to estimate the position
    plot_trajectory(z,y);    % Plot the results
end
hold;
end   % of the function
Current plot held
Current plot released

C コードの生成

'-config;lib' オプションを指定された 'codegen' コマンドは、スタンドアロン C ライブラリとしてパッケージ化された C コードを生成します。

C では静的なデータ型が使用されるため、'codegen' は MATLAB ファイル内のすべての変数のプロパティをコンパイル時に判別しなければなりません。ここでは、'codegen' が入力されたデータ型に基づいて新しいデータ型を推測できるように、コマンド ライン オプション '-args' がサンプル入力を提供します。

'-report' オプションは、コンパイル結果と生成されたファイルへのリンクを含むコンパイル レポートを生成します。'codegen' は MATLAB コードをコンパイルした後、このレポートへのハイパーリンクを提供します。

z = position(:,1);
codegen  -config:lib -report -c kalmanfilter.m -args {z}
Code generation successful: To view the report, open('C:\TEMP\R2013bd_689_5180\tpad8c65a1_e3df_4668_84da_1aece68d5262\coderdemo_kalman_filter\codegen\lib\kalmanfilter\html\index.html').

生成されたコードの確認

生成された C コードは 'codegen/lib/kalmanfilter/' フォルダーにあります。ファイルは、以下のとおりです。

dir codegen/lib/kalmanfilter/
.                          kalmanfilter_terminate.c   
..                         kalmanfilter_terminate.h   
buildInfo.mat              kalmanfilter_types.h       
codeInfo.mat               rtGetInf.c                 
html                       rtGetInf.h                 
interface                  rtGetNaN.c                 
kalmanfilter.c             rtGetNaN.h                 
kalmanfilter.h             rt_nonfinite.c             
kalmanfilter_initialize.c  rt_nonfinite.h             
kalmanfilter_initialize.h  rtw_proj.tmw               
kalmanfilter_ref.rsp       rtwtypes.h                 
kalmanfilter_rtw.bat       
kalmanfilter_rtw.mk        

関数 'kalmanfilter.c' の C コードの確認

type codegen/lib/kalmanfilter/kalmanfilter.c
/*
 * kalmanfilter.c
 *
 * Code generation for function 'kalmanfilter'
 *
 * C source code generated on: Fri Aug 09 01:26:17 2013
 *
 */

/* Include files */
#include "rt_nonfinite.h"
#include "kalmanfilter.h"

/* Variable Definitions */
static double x_est[6];
static double p_est[36];

/* Function Definitions */
void kalmanfilter(const double z[2], double y[2])
{
  signed char Q[36];
  int r2;
  double a[36];
  int k;
  double x_prd[6];
  static const signed char b_a[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0,
    1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1 };

  int i0;
  double p_prd[36];
  double a21;
  int r1;
  static const signed char b[36] = { 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1,
    0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };

  double klm_gain[12];
  static const signed char c_a[12] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0 };

  double S[4];
  static const signed char b_b[12] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 };

  static const short R[4] = { 1000, 0, 0, 1000 };

  double B[12];
  double a22;
  double Y[12];
  double b_z[2];

  /*    Copyright 2010 The MathWorks, Inc. */
  /*  Initialize state transition matrix */
  /*      % [x  ] */
  /*      % [y  ] */
  /*      % [Vx] */
  /*      % [Vy] */
  /*      % [Ax] */
  /*  [Ay] */
  /*  Initialize measurement matrix */
  for (r2 = 0; r2 < 36; r2++) {
    Q[r2] = 0;
  }

  for (k = 0; k < 6; k++) {
    Q[k + 6 * k] = 1;

    /*  Initial state conditions */
    /*  Predicted state and covariance */
    x_prd[k] = 0.0;
    for (r2 = 0; r2 < 6; r2++) {
      x_prd[k] += (double)b_a[k + 6 * r2] * x_est[r2];
    }

    for (r2 = 0; r2 < 6; r2++) {
      a[k + 6 * r2] = 0.0;
      for (i0 = 0; i0 < 6; i0++) {
        a[k + 6 * r2] += (double)b_a[k + 6 * i0] * p_est[i0 + 6 * r2];
      }
    }
  }

  for (r2 = 0; r2 < 6; r2++) {
    for (i0 = 0; i0 < 6; i0++) {
      a21 = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        a21 += a[r2 + 6 * r1] * (double)b[r1 + 6 * i0];
      }

      p_prd[r2 + 6 * i0] = a21 + (double)Q[r2 + 6 * i0];
    }
  }

  /*  Estimation */
  for (r2 = 0; r2 < 2; r2++) {
    for (i0 = 0; i0 < 6; i0++) {
      klm_gain[r2 + (i0 << 1)] = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        klm_gain[r2 + (i0 << 1)] += (double)c_a[r2 + (r1 << 1)] * p_prd[i0 + 6 *
          r1];
      }
    }
  }

  for (r2 = 0; r2 < 2; r2++) {
    for (i0 = 0; i0 < 2; i0++) {
      a21 = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        a21 += klm_gain[r2 + (r1 << 1)] * (double)b_b[r1 + 6 * i0];
      }

      S[r2 + (i0 << 1)] = a21 + (double)R[r2 + (i0 << 1)];
    }
  }

  for (r2 = 0; r2 < 2; r2++) {
    for (i0 = 0; i0 < 6; i0++) {
      B[r2 + (i0 << 1)] = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        B[r2 + (i0 << 1)] += (double)c_a[r2 + (r1 << 1)] * p_prd[i0 + 6 * r1];
      }
    }
  }

  if (fabs(S[1]) > fabs(S[0])) {
    r1 = 1;
    r2 = 0;
  } else {
    r1 = 0;
    r2 = 1;
  }

  a21 = S[r2] / S[r1];
  a22 = S[2 + r2] - a21 * S[2 + r1];
  for (k = 0; k < 6; k++) {
    Y[1 + (k << 1)] = (B[r2 + (k << 1)] - B[r1 + (k << 1)] * a21) / a22;
    Y[k << 1] = (B[r1 + (k << 1)] - Y[1 + (k << 1)] * S[2 + r1]) / S[r1];
  }

  for (r2 = 0; r2 < 2; r2++) {
    for (i0 = 0; i0 < 6; i0++) {
      klm_gain[i0 + 6 * r2] = Y[r2 + (i0 << 1)];
    }
  }

  /*  Estimated state and covariance */
  for (r2 = 0; r2 < 2; r2++) {
    a21 = 0.0;
    for (i0 = 0; i0 < 6; i0++) {
      a21 += (double)c_a[r2 + (i0 << 1)] * x_prd[i0];
    }

    b_z[r2] = z[r2] - a21;
  }

  for (r2 = 0; r2 < 6; r2++) {
    a21 = 0.0;
    for (i0 = 0; i0 < 2; i0++) {
      a21 += klm_gain[r2 + 6 * i0] * b_z[i0];
    }

    x_est[r2] = x_prd[r2] + a21;
  }

  for (r2 = 0; r2 < 6; r2++) {
    for (i0 = 0; i0 < 6; i0++) {
      a[r2 + 6 * i0] = 0.0;
      for (r1 = 0; r1 < 2; r1++) {
        a[r2 + 6 * i0] += klm_gain[r2 + 6 * r1] * (double)c_a[r1 + (i0 << 1)];
      }
    }
  }

  for (r2 = 0; r2 < 6; r2++) {
    for (i0 = 0; i0 < 6; i0++) {
      a21 = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        a21 += a[r2 + 6 * r1] * p_prd[r1 + 6 * i0];
      }

      p_est[r2 + 6 * i0] = p_prd[r2 + 6 * i0] - a21;
    }
  }

  /*  Compute the estimated measurements */
  for (r2 = 0; r2 < 2; r2++) {
    y[r2] = 0.0;
    for (i0 = 0; i0 < 6; i0++) {
      y[r2] += (double)c_a[r2 + (i0 << 1)] * x_est[i0];
    }
  }

  /*  of the function */
}

void kalmanfilter_init(void)
{
  int i;
  for (i = 0; i < 6; i++) {
    x_est[i] = 0.0;
  }

  memset(&p_est[0], 0, 36U * sizeof(double));
}

/* End of code generation (kalmanfilter.c) */

MATLAB アルゴリズムの実行速度の高速化

'codegen' コマンドを使って MATLAB コードから MEX 関数を生成すれば、大規模なデータセットを処理する関数 'kalmanfilter' の実行速度を上げることができます。

大規模なデータセット処理のための関数 'kalman_loop' の呼び出し

最初に、MATLAB で多数のデータ サンプルを使用してカルマン アルゴリズムを実行します。関数 'kalman_loop' が関数 'kalmanfilter' をループで実行します。ループの反復回数は、関数への入力の 2 次元目と同じです。

type kalman_loop
%   Copyright 2010 The MathWorks, Inc.
function y=kalman_loop(z)
% Call Kalman estimator in the loop for large data set testing
%#codegen
[DIM, LEN]=size(z);
y=zeros(DIM,LEN);           % Initialize output
for n=1:LEN                     % Output in the loop
    y(:,n)=kalmanfilter(z(:,n));
end;

コンパイルなしでの基準実行速度

ここで、MATLAB アルゴリズムの実行速度を測定します。'randn' コマンドを使用して乱数を生成し、(2x1) 位置ベクトルの 100,000 個のサンプルで構成される入力行列 'position' を作成します。現在のフォルダーからすべての MEX ファイルを削除します。MATLAB ストップウオッチ タイマー ('tic' および 'toc' コマンド) を使用して、関数 'kalman_loop' を実行したときにこれらのサンプルの処理にかかる時間を測定します。

clear mex
delete(['*.' mexext])
position =randn(2,100000);
tic, kalman_loop(position); a=toc;

テスト用の MEX 関数の生成

次に、コマンド codegen を使用して MEX 関数を生成し、MATLAB 関数 'kalman_loop' の名前を指定します。'codegen' コマンドは、'kalman_loop_mex' という名前の MEX 関数を生成します。これで、この MEX 関数の実行速度を元の MATLAB アルゴリズムの実行速度と比較できます。

codegen -args {position} kalman_loop.m
which kalman_loop_mex
C:\TEMP\R2013bd_689_5180\tpad8c65a1_e3df_4668_84da_1aece68d5262\coderdemo_kalman_filter\kalman_loop_mex.mexw64

MEX 関数の実行速度の測定

今度は、MEX 関数 'kalman_loop_mex'の実行速度を測定します。実行速度を正しく比較するために、入力として前回と同じ信号 'position' を使用します。

tic, kalman_loop_mex(position); b=toc;

実行速度の比較

生成された MEX 関数を使用して、実行速度の違いを確認します。

display(sprintf('The speedup is %.1f times using the generated MEX over the baseline MATLAB function.',a/b));
The speedup is 63.4 times using the generated MEX over the baseline MATLAB function.

クリーンアップ

ファイルを削除して元のフォルダーに戻ります。

コマンドの実行: クリーンアップ

cleanup

製品評価版の入手または製品の購入