为 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
![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.](../../examples/coder/win64/CCodeGenerationForAMATLABKalmanFilteringAlgorithmExample_02.png)
生成 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: View report
检查生成的代码
生成的 C 代码位于 codegen/lib/kalmanfilter/ 文件夹中。这些文件是：
dir codegen/lib/kalmanfilter/. .. _clang-format buildInfo.mat codeInfo.mat codedescriptor.dmr compileInfo.mat examples eye.c eye.h html interface kalmanfilter.c kalmanfilter.h kalmanfilter_data.c kalmanfilter_data.h kalmanfilter_initialize.c kalmanfilter_initialize.h kalmanfilter_rtw.bat kalmanfilter_rtw.mk kalmanfilter_rtw.rsp kalmanfilter_rtw_comp.rsp kalmanfilter_rtw_ref.rsp kalmanfilter_terminate.c kalmanfilter_terminate.h kalmanfilter_types.h mldivide.c mldivide.h rtw_proj.tmw rtwtypes.h setup_mingw.bat
检查 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 : 25.1
* C/C++ source code generated on : 10-Dec-2024 10:46:46
*/
/* Include Files */
#include "kalmanfilter.h"
#include "eye.h"
#include "kalmanfilter_data.h"
#include "kalmanfilter_initialize.h"
#include "mldivide.h"
#include <emmintrin.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 B[36];
double a[36];
double p_prd[36];
double b_B[12];
double dv[12];
double x_prd[6];
double c_B[4];
double b_z[2];
double d;
int B_tmp;
int i;
int i1;
int i2;
int x_prd_tmp;
if (!isInitialized_kalmanfilter) {
kalmanfilter_initialize();
}
/* Copyright 2010 The MathWorks, Inc. */
/* Initialize state transition matrix */
/* % [x ] */
/* % [y ] */
/* % [Vx] */
/* % [Vy] */
/* % [Ax] */
/* [Ay] */
/* Initialize measurement matrix */
/* Initial state conditions */
/* Predicted state and covariance */
memset(&x_prd[0], 0, 6U * sizeof(double));
eye(B);
memset(&a[0], 0, 36U * sizeof(double));
for (i = 0; i < 6; i++) {
for (i1 = 0; i1 < 6; i1++) {
x_prd_tmp = i1 + 6 * i;
x_prd[i1] += (double)b_a[x_prd_tmp] * x_est[i];
d = p_est[x_prd_tmp];
for (i2 = 0; i2 < 6; i2++) {
x_prd_tmp = i2 + 6 * i;
a[x_prd_tmp] += (double)b_a[i2 + 6 * i1] * d;
}
}
}
/* Estimation */
memset(&b_B[0], 0, 12U * sizeof(double));
for (i2 = 0; i2 < 6; i2++) {
B_tmp = i2 << 1;
for (i1 = 0; i1 < 6; i1++) {
d = 0.0;
for (i = 0; i < 6; i++) {
d += a[i2 + 6 * i] * (double)iv[i + 6 * i1];
}
x_prd_tmp = i2 + 6 * i1;
d += B[x_prd_tmp];
p_prd[x_prd_tmp] = d;
x_prd_tmp = i1 << 1;
b_B[B_tmp] += (double)c_a[x_prd_tmp] * d;
b_B[B_tmp + 1] += (double)c_a[x_prd_tmp + 1] * d;
}
}
for (i2 = 0; i2 < 2; i2++) {
for (i1 = 0; i1 < 2; i1++) {
d = 0.0;
for (i = 0; i < 6; i++) {
d += b_B[i2 + (i << 1)] * (double)iv1[i + 6 * i1];
}
x_prd_tmp = i2 + (i1 << 1);
c_B[x_prd_tmp] = d + (double)R[x_prd_tmp];
}
}
mldivide(c_B, b_B, dv);
for (i = 0; i < 2; i++) {
for (i2 = 0; i2 < 6; i2++) {
b_B[i2 + 6 * i] = dv[i + (i2 << 1)];
}
}
/* Estimated state and covariance */
for (i = 0; i < 2; i++) {
d = 0.0;
for (i2 = 0; i2 < 6; i2++) {
d += (double)c_a[i + (i2 << 1)] * x_prd[i2];
}
b_z[i] = z[i] - d;
}
memset(&B[0], 0, 36U * sizeof(double));
for (i2 = 0; i2 < 6; i2++) {
d = 0.0;
x_prd_tmp = 6 * i2 + 2;
B_tmp = 6 * i2 + 4;
for (i = 0; i < 2; i++) {
__m128d r;
__m128d r1;
__m128d r2;
d += b_B[i2 + 6 * i] * b_z[i];
r = _mm_loadu_pd(&b_B[6 * i]);
r1 = _mm_loadu_pd(&B[6 * i2]);
r2 = _mm_set1_pd(c_a[i + (i2 << 1)]);
_mm_storeu_pd(&B[6 * i2], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
r = _mm_loadu_pd(&b_B[6 * i + 2]);
r1 = _mm_loadu_pd(&B[x_prd_tmp]);
_mm_storeu_pd(&B[x_prd_tmp], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
r = _mm_loadu_pd(&b_B[6 * i + 4]);
r1 = _mm_loadu_pd(&B[B_tmp]);
_mm_storeu_pd(&B[B_tmp], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
}
x_est[i2] = x_prd[i2] + d;
}
/* Compute the estimated measurements */
memset(&y[0], 0, sizeof(double) << 1);
for (i1 = 0; i1 < 6; i1++) {
for (i = 0; i < 6; i++) {
d = 0.0;
for (i2 = 0; i2 < 6; i2++) {
d += B[i1 + 6 * i2] * p_prd[i2 + 6 * i];
}
x_prd_tmp = i1 + 6 * i;
p_est[x_prd_tmp] = p_prd[x_prd_tmp] - d;
}
x_prd_tmp = i1 << 1;
d = x_est[i1];
y[0] += (double)c_a[x_prd_tmp] * d;
y[1] += (double)c_a[x_prd_tmp + 1] * 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.
对 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 6.7 times using the generated MEX over the baseline MATLAB function.
