Main Content

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

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

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

必要条件

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

関数 kalmanfilter について

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

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

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

{X=X0+VxdtY=Y0+VydtVx=Vx0+AxdtVy=Vy0+Aydt

これらの法則をキャプチャする反復式が、カルマン状態遷移行列 "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;

Figure contains an axes object. The axes object with title Test vector for the Kalman filtering with 2 sudden discontinuities, xlabel x-axis, ylabel y-axis contains 310 objects of type line.

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.

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            : 23.2
 * C/C++ source code generated on  : 27-Jul-2023 14:57:28
 */

/* 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/Bdoc23b_2340827_475699/tpe5b4f050/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 15.9 times using the generated MEX over the baseline MATLAB function.