Main Content

本页翻译不是最新的。点击此处可查看最新英文版本。

为 MATLAB 卡尔曼滤波算法生成 C 代码

此示例说明如何为 MATLAB® 卡尔曼滤波器函数 kalmanfilter 生成 C 代码,该函数基于过去的含噪测量值来估计运动物体的位置。示例还说明如何为此 MATLAB 代码生成 MEX 函数,以提高算法在 MATLAB 中的执行速度。

前提条件

此示例没有任何前提条件。

关于 kalmanfilter 函数

kalmanfilter 函数根据运动物体过去的位置值预测其位置。该函数使用一个卡尔曼滤波器估计器,这是一种递归自适应滤波器,它根据一系列含噪测量值估计动态系统的状态。卡尔曼滤波在信号和图像处理、控制设计和计算金融等领域有着广泛的应用。

关于卡尔曼滤波器估计器算法

卡尔曼估计器通过计算和更新卡尔曼状态向量来计算位置向量。状态向量定义为 6×1 列向量,其中包括二维笛卡尔空间中的位置(x 和 y)、速度 (Vx Vy) 和加速度(Ax 和 Ay)测量值。基于经典的运动定律:

{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

加载测试数据

要跟踪的物体的位置记录为笛卡尔空间中的 x 和 y 坐标,存储在名为 position_data.mat 的 MAT 文件中。以下代码会加载该 MAT 文件并绘制位置的轨迹。测试数据包括位置上的两次突然改变或不连续,用于检查卡尔曼滤波器能否快速重新调整和跟踪物体。

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 函数调用卡尔曼滤波器算法,用蓝色绘制物体轨迹,用绿色绘制卡尔曼滤波器估算位置。最初,您会看到估计的位置只用很短时间就与物体的实际位置重合在一起。然后,位置会出现三次突然改变。每次卡尔曼滤波器都会重新调整并在经过几次迭代后跟踪上该物体的实际位置。

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 文件中所有变量的属性。在这里,-args 命令行选项会提供一个示例输入,以便 codegen 基于输入类型推断新类型。

-report 选项生成一个编译报告,其中包含编译结果的汇总和所生成文件的链接。编译 MATLAB 代码后,codegen 提供指向该报告的超链接。

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
/*
 * File: kalmanfilter.c
 *
 * MATLAB Coder version            : 23.2
 * C/C++ source code generated on  : 03-Aug-2023 18:52:22
 */

/* 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 函数。循环迭代次数等于函数输入的第二个维度。

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 命令生成随机数,并创建由 100000 个 (2×1) 位置向量样本组成的输入矩阵 position。从当前文件夹中删除所有 MEX 文件。运行 kalman_loop 函数时,使用 MATLAB 秒表计时器(tictoc 命令)来测量处理这些样本所需的时间。

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

生成 MEX 函数用于测试

下一步,使用命令 codegen 后跟要编译的 MATLAB 函数的名称 kalman_loop 生成 MEX 函数。codegen 命令生成名为 kalman_loop_mex 的 MEX 函数。然后,您可以将此 MEX 函数的执行速度与原始 MATLAB 算法的执行速度进行比较。

codegen -args {position} kalman_loop.m
Code generation successful.
which kalman_loop_mex
/tmp/Bdoc23b_2347338_2909509/tp488683c7/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 12.6 times using the generated MEX over the baseline MATLAB function.