MATLAB のカルマン フィルター アルゴリズムの C コードの生成
この例では、ノイズを含む過去の測定値に基づいて移動する物体の位置を推測する MATLAB® のカルマン フィルター関数 kalmanfilter
の C コードを生成する方法を説明します。また、MATLAB でアルゴリズムの実行速度を上げるために、この MATLAB コードの MEX 関数を生成する方法も示します。
必要条件
この例には必要条件はありません。
関数 kalmanfilter
について
関数 kalmanfilter
は、移動する物体の位置をその過去の値に基づいて予測します。この関数ではカルマン フィルター推定器が使用されますが、この推定器はノイズを含む一連の測定値から動的システムの状態を推定する再帰的な適応フィルターです。カルマン フィルターには、信号およびイメージ処理、制御設計、および金融工学などの分野で使用される多様なアプリケーションがあります。
カルマン フィルター推定器のアルゴリズムについて
カルマン推定器は、カルマン状態ベクトルを計算、更新して位置ベクトルを計算します。状態ベクトルは、2 次元の直交座標空間に、位置 (x および y)、速度 (Vx Vy)、および加速 (Ax および Ay) の測定値を含む 6 行 1 列の列ベクトルです。以下のように古典的な運動の法則に基づいています。
これらの法則をキャプチャする反復式が、カルマン状態遷移行列 "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_data.mat
と呼ばれる MAT ファイル内の直交座標空間に x および y 座標として記録されます。以下のコードは MAT ファイルを読み込んで位置のトレースをプロットします。テスト データには、カルマン フィルターが迅速に再調整を行って物体を追跡できるかどうかチェックするために使用する、2 回の位置の突然の変化または 2 つの不連続点が含まれています。
load position_data.mat
hold; grid;
Current plot held
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 with 2 sudden discontinuities '); xlabel('x-axis');ylabel('y-axis'); hold;
Current plot released
関数 ObjTrack
の検査と実行
関数 ObjTrack.m
はカルマン フィルター アルゴリズムを呼び出し、物体の軌跡を青で、カルマン フィルターの推定位置を緑でプロットします。最初は、短時間でオブジェクトの推定位置が実際の位置に収束します。その後、位置の突然の変化が 3 回発生します。そのたびに、カルマン フィルターは再調整を行い、数回の反復の後、物体を追跡します。
type ObjTrack
% 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
ObjTrack(position)
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('codegen/lib/kalmanfilter/html/report.mldatx')
生成されたコードの確認
生成された C コードは codegen/lib/kalmanfilter/
フォルダーにあります。ファイルは、以下のとおりです。
dir codegen/lib/kalmanfilter/
. .. _clang-format buildInfo.mat codeInfo.mat codedescriptor.dmr compileInfo.mat examples html interface kalmanfilter.c kalmanfilter.h kalmanfilter_data.c kalmanfilter_data.h kalmanfilter_initialize.c kalmanfilter_initialize.h kalmanfilter_rtw.mk kalmanfilter_terminate.c kalmanfilter_terminate.h kalmanfilter_types.h rtw_proj.tmw rtwtypes.h
関数 kalmanfilter.c
の C コードの検査
type codegen/lib/kalmanfilter/kalmanfilter.c
/* * Prerelease License - for engineering feedback and testing purposes * only. Not for sale. * File: kalmanfilter.c * * MATLAB Coder version : 24.1 * C/C++ source code generated on : 25-Jan-2024 15:02:16 */ /* Include Files */ #include "kalmanfilter.h" #include "kalmanfilter_data.h" #include "kalmanfilter_initialize.h" #include <math.h> #include <string.h> /* Variable Definitions */ static double x_est[6]; static double p_est[36]; /* Function Definitions */ /* * Arguments : const double z[2] * double y[2] * Return Type : void */ void kalmanfilter(const double z[2], double y[2]) { static const short R[4] = {1000, 0, 0, 1000}; 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}; static const signed char iv[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}; static const signed char c_a[12] = {1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0}; static const signed char iv1[12] = {1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0}; double a[36]; double p_prd[36]; double B[12]; double Y[12]; double x_prd[6]; double S[4]; double b_z[2]; double a21; double a22; double a22_tmp; double d; int i; int k; int r1; int r2; signed char Q[36]; if (!isInitialized_kalmanfilter) { kalmanfilter_initialize(); } /* Copyright 2010 The MathWorks, Inc. */ /* Initialize state transition matrix */ /* % [x ] */ /* % [y ] */ /* % [Vx] */ /* % [Vy] */ /* % [Ax] */ /* [Ay] */ /* Initialize measurement matrix */ for (i = 0; i < 36; i++) { Q[i] = 0; } /* Initial state conditions */ /* Predicted state and covariance */ for (k = 0; k < 6; k++) { Q[k + 6 * k] = 1; x_prd[k] = 0.0; for (i = 0; i < 6; i++) { r1 = k + 6 * i; x_prd[k] += (double)b_a[r1] * x_est[i]; d = 0.0; for (r2 = 0; r2 < 6; r2++) { d += (double)b_a[k + 6 * r2] * p_est[r2 + 6 * i]; } a[r1] = d; } } for (i = 0; i < 6; i++) { for (r2 = 0; r2 < 6; r2++) { d = 0.0; for (r1 = 0; r1 < 6; r1++) { d += a[i + 6 * r1] * (double)iv[r1 + 6 * r2]; } r1 = i + 6 * r2; p_prd[r1] = d + (double)Q[r1]; } } /* Estimation */ for (i = 0; i < 2; i++) { for (r2 = 0; r2 < 6; r2++) { d = 0.0; for (r1 = 0; r1 < 6; r1++) { d += (double)c_a[i + (r1 << 1)] * p_prd[r2 + 6 * r1]; } B[i + (r2 << 1)] = d; } for (r2 = 0; r2 < 2; r2++) { d = 0.0; for (r1 = 0; r1 < 6; r1++) { d += B[i + (r1 << 1)] * (double)iv1[r1 + 6 * r2]; } r1 = i + (r2 << 1); S[r1] = d + (double)R[r1]; } } if (fabs(S[1]) > fabs(S[0])) { r1 = 1; r2 = 0; } else { r1 = 0; r2 = 1; } a21 = S[r2] / S[r1]; a22_tmp = S[r1 + 2]; a22 = S[r2 + 2] - a21 * a22_tmp; for (k = 0; k < 6; k++) { double d1; i = k << 1; d = B[r1 + i]; d1 = (B[r2 + i] - d * a21) / a22; Y[i + 1] = d1; Y[i] = (d - d1 * a22_tmp) / S[r1]; } for (i = 0; i < 2; i++) { for (r2 = 0; r2 < 6; r2++) { B[r2 + 6 * i] = Y[i + (r2 << 1)]; } } /* Estimated state and covariance */ for (i = 0; i < 2; i++) { d = 0.0; for (r2 = 0; r2 < 6; r2++) { d += (double)c_a[i + (r2 << 1)] * x_prd[r2]; } b_z[i] = z[i] - d; } for (i = 0; i < 6; i++) { d = B[i + 6]; x_est[i] = x_prd[i] + (B[i] * b_z[0] + d * b_z[1]); for (r2 = 0; r2 < 6; r2++) { r1 = r2 << 1; a[i + 6 * r2] = B[i] * (double)c_a[r1] + d * (double)c_a[r1 + 1]; } for (r2 = 0; r2 < 6; r2++) { d = 0.0; for (r1 = 0; r1 < 6; r1++) { d += a[i + 6 * r1] * p_prd[r1 + 6 * r2]; } r1 = i + 6 * r2; p_est[r1] = p_prd[r1] - d; } } /* Compute the estimated measurements */ for (i = 0; i < 2; i++) { d = 0.0; for (r2 = 0; r2 < 6; r2++) { d += (double)c_a[i + (r2 << 1)] * x_est[r2]; } y[i] = d; } } /* * Arguments : void * Return Type : void */ void kalmanfilter_init(void) { int i; for (i = 0; i < 6; i++) { x_est[i] = 0.0; } /* x_est=[x,y,Vx,Vy,Ax,Ay]' */ memset(&p_est[0], 0, 36U * sizeof(double)); } /* * File trailer for kalmanfilter.c * * [EOF] */
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
Code generation successful.
which kalman_loop_mex
/tmp/Bdoc24a_2511836_1614815/tp63257fd9/coder-ex53054096/kalman_loop_mex.mexa64
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 14.6 times using the generated MEX over the baseline MATLAB function.