Matlabのカルマンフィルタ，アルゴリズムのcコ，ドの生成

ラ@ @ブスクリプトを開く

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

必要条件

この例には必要条件はありません。

関数`kalmanfilter`にいて

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

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

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

$｛ \begin{array}{rcl} X & ＝ & X_{0} + V_{x} d t \\ Y & ＝ & Y_{0} + V_{y} d t \\ V_{x} & ＝ & V_{x 0} + {一个}_{x} d t \\ V_{y} & ＝ & V_{y 0} + {一个}_{y} d t \end{array}$

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

类型kalmanfilter.m

函数y = kalmanfilter(z) %#codegen dt=1;%初始化状态转换矩阵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;0 1 0 0 0 0];初始化测量矩阵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ファaaplル内の直交座標空間にxおよびy座標として記録されます。以下のコ、ドはmatファ、ルを読み込んで位置のトレ、スをプロットします。テストデータには,カルマンフィルターが迅速に再調整を行って物体を追跡できるかどうかチェックするために使用する,2回の位置の突然の変化または2つの不連続点が含まれています。

负载position_data.mat持有;网格;

目前的地块

为idx = 1: numPts z = position(:，idx);情节(z z (1), (2),“软”）;轴([-1 1 -1 1]);结束标题(具有2个突然不连续点的卡尔曼滤波的检验向量）;包含(“轴”); ylabel (“轴”）;持有;

图中包含一个轴对象。具有2个突发性不连续的卡尔曼滤波测试向量的轴对象包含310个类型为直线的对象。

最新剧情公布

関数`ObjTrack`の検査と実行

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

类型ObjTrack

首先，设置数字numPts = 300;处理并绘制300个样品图;保持;网格;%准备绘图窗口%主循环为idx = 1: numPts z =位置(:，idx);%获取输入数据y = kalmanfilter(z);%调用卡尔曼滤波估计位置plot_trajectory(z,y);%绘制结果结束hold;函数的结束。%

ObjTrack(位置)

目前的地块

图中包含一个轴对象。axis对象的标题为对象的轨迹(蓝色)，其卡尔曼估计值(绿色)包含600个类型为直线的对象。

最新剧情公布

Cコ，ドの生成

配置:自由オプションを指定されたcodegenコマンドは，スタンドアロンcラブラリとしてパッケジ化されたcコドを生成します。

Cでは静的なデ，タ型が使用されるため，codegenはmatlabファesc esc escル時に判別しなければなりません。ここでは，codegenが入力されたデ、タ型に基づいて新しいデ、タ型を推測できるように、コマンドラ、ンオプションarg游戏がサンプル入力を提供します。

报告オプションは,コンパイル結果の概要と生成されたファイルへのリンクを含むコンパイルレポートを生成します。codegenはmatlabコドをコンパルした後，このレポトへのハパリンクを提供します。

Z = position(:，1);codegen配置:自由报告- ckalmanfilter.marg游戏{z}

要查看报告，打开('codegen/lib/kalmanfilter/html/report.mldatx')

生成されたコ，ドの確認

生成されたcコ，ドはcodegen / lib / kalmanfilter /フォルダ，にあります。ファesc escルは，以下のとおりです。

dircodegen / lib / kalmanfilter /

．kalmanfilter.h . .kalmanfilter_data.c .gitignore kalmanfilter_data.h _clang-format kalmanfilter_initialize.c buildInfo. cmat kalmanfilter_initialize.h codeInfo. mat垫kalmanfilter_rtw。可codedescriptor。dmr kalmanfilter_terminate.c compileInfo.mat kalmanfilter_terminate.h examples kalmanfilter_types.h html rtw_proj.tmw interface rtwtypes.h kalmanfilter.c

関数`kalmanfilter.c`のcコ，ドの検査

类型codegen / lib / kalmanfilter / kalmanfilter.c

/* *文件:kalmanfilter.c * * MATLAB编码版本:5.4 * C/ c++源代码生成时间:26-Feb-2022 10:45:01 */ /* Include Files */ # Include "kalmanfilter.h" # Include "kalmanfilter_data.h" # Include "kalmanfilter_initialize.h" # Include  # Include  /*变量定义*/ static double x_est[6];静态双p_est[36];/*函数定义*/ /* *参数:const double z[2] * double y[2] *返回类型:void */ void kalmanfilter(const double z[2]， double y[2]) {static const short R[4] = {1000,0,0,1000};静态const 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,1,0,0, 0,0, 1,0,0,0, 1,0,1,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,0,0,0,1,1,0,0,0,1,1,0,0,0,1,1,0,0,0,1,1,0,0,0,1,1,0,0,0,0,1,1,0,0,0,0,0,0,1};Static const signed char c_a[12] = {1,0,0,1,0,0,0,0,0};静态const signed char iv1[12] = {1,0,0,0, 0,0,0, 0,0,1,0,0,0};双[36];双p_prd [36];双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コードから墨西哥人関数を生成すれば,大規模なデータセットを処理する関数kalmanfilterの実行速度を上げることができます。

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

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

类型kalman_loop

function y=kalman_loop(z) %在循环中调用Kalman估计器用于大数据集测试%#codegen [DIM, LEN]=size(z);y = 0(暗淡,LEN);初始化n=1的输出:LEN %循环中的输出y(:，n)=kalmanfilter(z(:，n));结束;

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

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

清晰的墨西哥人删除([“*”。Mexext]) position = randn(2,100000);抽搐,kalman_loop(位置);一个= toc;

テスト用のmex関数の生成

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

codegenarg游戏{职位}kalman_loop.m

代码生成成功。

哪一个kalman_loop_mex

/ tmp / Bdoc22a_1891349_28235 tp87f0aa09 / coder-ex53054096 / kalman_loop_mex.mexa64

墨西哥関数の実行速度の測定

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

抽搐,kalman_loop_mex(位置);b = toc;

実行速度の比較

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

显示器(sprintf ('加速为%。1f次使用生成的MEX对基准MATLAB函数。a / b));

使用生成的MEX比基准MATLAB函数加速17.1倍。