为 MATLAB 卡尔曼滤波算法生成 C 代码 |
您所在的位置：网站首页 › matlab函数递归 › 为 MATLAB 卡尔曼滤波算法生成 C 代码 |
|
前提条件 此示例没有任何前提条件。 关于 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;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/* * Prerelease License - for engineering feedback and testing purposes * only. Not for sale. * File: kalmanfilter.c * * MATLAB Coder version : 5.6 * C/C++ source code generated on : 30-Jan-2023 12:49:07 */ /* Include Files */ #include "kalmanfilter.h" #include "kalmanfilter_data.h" #include "kalmanfilter_initialize.h" #include #include /* 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 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |