MATLAB编写的EKF和UKF滤波程序源代码 扩展卡尔曼滤波、无迹卡尔曼滤波的MATLAB程序有误差对比图像和最大误差数字的显示。 只有一个m文件打开就能运行。 带中文注释。直接双击EKFUKFComparison.m就能看到两个滤波器在非线性系统里的较量。这个文件里塞了扩展卡尔曼滤波和无迹卡尔曼滤波的完整实现还自带三维轨迹误差对比图。咱先看段有意思的代码——状态预测部分藏着非线性系统的精髓% 状态转移函数非线性 function x_pred stateTransition(x, dt) theta x(3); % 航向角 v x(4); % 速度 x_pred x [v*dt*cos(theta); v*dt*sin(theta); 0; % 航向角不变 -0.1*v*dt]; % 速度衰减 end这个函数里航向角不做变化但速度会随着时间衰减。注意看速度项-0.1vdt这模拟了现实中的运动阻力。当物体速度越快减速效果越明显这种非线性特性正是考验滤波器的关键。再看EKF的核心——雅可比矩阵计算。这里用数值微分替代手动求导代码量直接砍半% 数值计算雅可比矩阵 J zeros(4,4); eps 1e-6; for i 1:4 dx zeros(4,1); dx(i) eps; J(:,i) (stateTransition(xdx,dt) - stateTransition(x-dx,dt))/(2*eps); end循环里逐个维度扰动状态量用中心差分算偏导数。数值方法的优势在修改状态方程时特别明显不用每次调整模型都重新推导公式。MATLAB编写的EKF和UKF滤波程序源代码 扩展卡尔曼滤波、无迹卡尔曼滤波的MATLAB程序有误差对比图像和最大误差数字的显示。 只有一个m文件打开就能运行。 带中文注释。UKF的sigma点生成策略更有意思。看这段权重计算代码lambda 3 - n; % 缩放参数 Wm [lambda/(lambdan) 0.5/(lambdan)zeros(1,2*n)]; % 均值权重 Wc Wm; Wc(1) Wc(1) (1 - alpha^2 beta); % 协方差权重这里的alpha控制sigma点分布范围beta用于融合先验分布信息。调试时把alpha从1改到0.1会看到轨迹估计突然发疯——参数敏感度超乎想象。运行完程序命令行窗口直接喷出最大误差对比EKF最大位置误差: 2.3743米 UKF最大位置误差: 1.8956米误差曲线图上UKF的蓝色线条明显更贴近零点但在第25秒左右有个突然的抖动。翻到观测方程部分找原因function z observationModel(x) % 雷达测距方位角观测 z [sqrt(x(1)^2 x(2)^2); % 距离 atan2(x(2), x(1))]; % 方位角 end方位角观测的非线性特性在物体靠近原点时会急剧变化这时候UKF的泰勒展开阶数更高优势就出来了。想要复现这个现象把初始位置设为(5,5)试试误差差异会更大。代码最后藏的彩蛋才好玩——调整过程噪声矩阵Q时如果手贱把角速度噪声设得太大Q diag([0.1, 0.1, 0.5, 0.1]); % 过程噪声协方差这时候EKF的轨迹估计会像醉汉走路UKF虽然也晃但还能保持大体方向。这说明在强非线性条件下UKF对噪声的鲁棒性确实更胜一筹。