为 MATLAB 卡尔曼滤波算法生成 C 代码
此示例说明如何为 MATLAB® 卡尔曼滤波器函数 kalmanfilter
生成 C 代码,该函数基于过去的含噪测量值来估计运动物体的位置。示例还说明如何为此 MATLAB 代码生成 MEX 函数,以提高算法在 MATLAB 中的执行速度。
前提条件
此示例没有任何前提条件。
关于 kalmanfilter
函数
kalmanfilter
函数根据运动物体过去的位置值预测其位置。该函数使用一个卡尔曼滤波器估计器,这是一种递归自适应滤波器,它根据一系列含噪测量值估计动态系统的状态。卡尔曼滤波在信号和图像处理、控制设计和计算金融等领域有着广泛的应用。
关于卡尔曼滤波器估计器算法
卡尔曼估计器通过计算和更新卡尔曼状态向量来计算位置向量。状态向量定义为 6×1 列向量,其中包括二维笛卡尔空间中的位置(x 和 y)、速度 (Vx Vy) 和加速度(Ax 和 Ay)测量值。基于经典的运动定律:
捕获这些定律的迭代公式反映在卡尔曼状态转移矩阵“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;
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
生成 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 秒表计时器(tic
和 toc
命令)来测量处理这些样本所需的时间。
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.