Main Content

本页的翻译已过时。点击此处可查看最新英文版本。

为 MATLAB Kalman 滤波算法生成 C 代码

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

前提条件

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

关于 kalmanfilter 函数

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

关于 Kalman 滤波器估算器算法

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

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

捕获这些定律的迭代公式反映在 Kalman 状态转移矩阵“A”中。请注意,通过编写大约 10 行的 MATLAB 代码,您可以基于在许多自适应滤波教科书中找到的理论数学公式来实现 Kalman 估算器。

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 文件并绘制位置的轨迹。测试数据包括位置上的两次突然改变或不连续,用于检查 Kalman 滤波器能否快速重新调整和跟踪物体。

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. The axes with title Test vector for the Kalman filtering with 2 sudden discontinuities contains 310 objects of type line.

Current plot released

检查并运行 ObjTrack 函数

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

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

Figure contains an axes. The axes with title Trajectory of object [blue] its Kalman estimate [green] contains 600 objects of type line.

Current plot released

生成 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/
.                          kalmanfilter.h             
..                         kalmanfilter_data.c        
.gitignore                 kalmanfilter_data.h        
_clang-format              kalmanfilter_initialize.c  
buildInfo.mat              kalmanfilter_initialize.h  
codeInfo.mat               kalmanfilter_rtw.mk        
codedescriptor.dmr         kalmanfilter_terminate.c   
compileInfo.mat            kalmanfilter_terminate.h   
defines.txt                kalmanfilter_types.h       
examples                   rtw_proj.tmw               
html                       rtwtypes.h                 
interface                  
kalmanfilter.c             

检查 kalmanfilter.c 函数的 C 代码

type codegen/lib/kalmanfilter/kalmanfilter.c
/*
 * File: kalmanfilter.c
 *
 * MATLAB Coder version            : 5.2
 * C/C++ source code generated on  : 13-Mar-2021 21:04:51
 */

/* 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 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_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;
  double d1;
  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)a[r1] * x_est[i];
      d = 0.0;
      for (r2 = 0; r2 < 6; r2++) {
        d += (double)a[k + 6 * r2] * p_est[r2 + 6 * i];
      }
      b_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 += b_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++) {
    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;
      b_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 += b_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 算法。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/Bdoc21a_1622859_74957/tp6298490a/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 20.6 times using the generated MEX over the baseline MATLAB function.