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
![Figure contains an axes object. The axes object with title Trajectory of object [blue] its Kalman estimate [green], xlabel horizontal position, ylabel vertical position contains 600 objects of type line.](../../examples/coder/win64/CCodeGenerationForAMATLABKalmanFilteringAlgorithmExample_02.png)
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: View report
生成されたコードの確認
生成された C コードは codegen/lib/kalmanfilter/ フォルダーにあります。ファイルは、以下のとおりです。
dir codegen/lib/kalmanfilter/. .. _clang-format buildInfo.mat codeInfo.mat codedescriptor.dmr compileInfo.mat examples eye.c eye.h html interface kalmanfilter.c kalmanfilter.h kalmanfilter_data.c kalmanfilter_data.h kalmanfilter_initialize.c kalmanfilter_initialize.h kalmanfilter_rtw.bat kalmanfilter_rtw.mk kalmanfilter_rtw.rsp kalmanfilter_rtw_comp.rsp kalmanfilter_rtw_ref.rsp kalmanfilter_terminate.c kalmanfilter_terminate.h kalmanfilter_types.h mldivide.c mldivide.h rtw_proj.tmw rtwtypes.h setup_mingw.bat
関数 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 : 25.1
* C/C++ source code generated on : 10-Dec-2024 10:46:46
*/
/* Include Files */
#include "kalmanfilter.h"
#include "eye.h"
#include "kalmanfilter_data.h"
#include "kalmanfilter_initialize.h"
#include "mldivide.h"
#include <emmintrin.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 B[36];
double a[36];
double p_prd[36];
double b_B[12];
double dv[12];
double x_prd[6];
double c_B[4];
double b_z[2];
double d;
int B_tmp;
int i;
int i1;
int i2;
int x_prd_tmp;
if (!isInitialized_kalmanfilter) {
kalmanfilter_initialize();
}
/* Copyright 2010 The MathWorks, Inc. */
/* Initialize state transition matrix */
/* % [x ] */
/* % [y ] */
/* % [Vx] */
/* % [Vy] */
/* % [Ax] */
/* [Ay] */
/* Initialize measurement matrix */
/* Initial state conditions */
/* Predicted state and covariance */
memset(&x_prd[0], 0, 6U * sizeof(double));
eye(B);
memset(&a[0], 0, 36U * sizeof(double));
for (i = 0; i < 6; i++) {
for (i1 = 0; i1 < 6; i1++) {
x_prd_tmp = i1 + 6 * i;
x_prd[i1] += (double)b_a[x_prd_tmp] * x_est[i];
d = p_est[x_prd_tmp];
for (i2 = 0; i2 < 6; i2++) {
x_prd_tmp = i2 + 6 * i;
a[x_prd_tmp] += (double)b_a[i2 + 6 * i1] * d;
}
}
}
/* Estimation */
memset(&b_B[0], 0, 12U * sizeof(double));
for (i2 = 0; i2 < 6; i2++) {
B_tmp = i2 << 1;
for (i1 = 0; i1 < 6; i1++) {
d = 0.0;
for (i = 0; i < 6; i++) {
d += a[i2 + 6 * i] * (double)iv[i + 6 * i1];
}
x_prd_tmp = i2 + 6 * i1;
d += B[x_prd_tmp];
p_prd[x_prd_tmp] = d;
x_prd_tmp = i1 << 1;
b_B[B_tmp] += (double)c_a[x_prd_tmp] * d;
b_B[B_tmp + 1] += (double)c_a[x_prd_tmp + 1] * d;
}
}
for (i2 = 0; i2 < 2; i2++) {
for (i1 = 0; i1 < 2; i1++) {
d = 0.0;
for (i = 0; i < 6; i++) {
d += b_B[i2 + (i << 1)] * (double)iv1[i + 6 * i1];
}
x_prd_tmp = i2 + (i1 << 1);
c_B[x_prd_tmp] = d + (double)R[x_prd_tmp];
}
}
mldivide(c_B, b_B, dv);
for (i = 0; i < 2; i++) {
for (i2 = 0; i2 < 6; i2++) {
b_B[i2 + 6 * i] = dv[i + (i2 << 1)];
}
}
/* Estimated state and covariance */
for (i = 0; i < 2; i++) {
d = 0.0;
for (i2 = 0; i2 < 6; i2++) {
d += (double)c_a[i + (i2 << 1)] * x_prd[i2];
}
b_z[i] = z[i] - d;
}
memset(&B[0], 0, 36U * sizeof(double));
for (i2 = 0; i2 < 6; i2++) {
d = 0.0;
x_prd_tmp = 6 * i2 + 2;
B_tmp = 6 * i2 + 4;
for (i = 0; i < 2; i++) {
__m128d r;
__m128d r1;
__m128d r2;
d += b_B[i2 + 6 * i] * b_z[i];
r = _mm_loadu_pd(&b_B[6 * i]);
r1 = _mm_loadu_pd(&B[6 * i2]);
r2 = _mm_set1_pd(c_a[i + (i2 << 1)]);
_mm_storeu_pd(&B[6 * i2], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
r = _mm_loadu_pd(&b_B[6 * i + 2]);
r1 = _mm_loadu_pd(&B[x_prd_tmp]);
_mm_storeu_pd(&B[x_prd_tmp], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
r = _mm_loadu_pd(&b_B[6 * i + 4]);
r1 = _mm_loadu_pd(&B[B_tmp]);
_mm_storeu_pd(&B[B_tmp], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
}
x_est[i2] = x_prd[i2] + d;
}
/* Compute the estimated measurements */
memset(&y[0], 0, sizeof(double) << 1);
for (i1 = 0; i1 < 6; i1++) {
for (i = 0; i < 6; i++) {
d = 0.0;
for (i2 = 0; i2 < 6; i2++) {
d += B[i1 + 6 * i2] * p_prd[i2 + 6 * i];
}
x_prd_tmp = i1 + 6 * i;
p_est[x_prd_tmp] = p_prd[x_prd_tmp] - d;
}
x_prd_tmp = i1 << 1;
d = x_est[i1];
y[0] += (double)c_a[x_prd_tmp] * d;
y[1] += (double)c_a[x_prd_tmp + 1] * 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.
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 6.7 times using the generated MEX over the baseline MATLAB function.
