✅作者简介热爱科研的Matlab仿真开发者修心和技术同步精进matlab项目合作可私信。个人主页Matlab科研工作室个人信条格物致知。更多Matlab仿真内容点击⛄ 内容介绍在非线性的简化车辆动力学模型基础上本文使用无迹卡尔曼滤波器进行状态估计。但在实际应用中算法由于噪声和计算误差会出现协方差矩阵非正定的问题本文应用平方根无迹卡尔曼滤波来提高数值的稳定性⛄ 部分代码function [X, S] srufk_update(Xpre, Spre, Y, h, Rs, h_param, sigmax, Wm, Wc)L size(Xpre, 1);O size(Y, 1);% pass sigmax through measure modulesigmay zeros(O, 2*L1); % 3x13Ypre zeros(O, 1);for i 1:2*L1sigmay(:,i) feval(h, sigmax(:,i));Ypre Ypre Wm(i) * sigmay(:, i);end% measure predict%Ypre sigmay*Wm;% measure covirance predict% tmp sqrt(Wc(2)) * (sigmay(:,2:2*L1) - repmat(Ypre,1,2*L));for i 2 : size(sigmay, 2)tmp(:, i-1) sqrt(Wc(2)) * (sigmay(:, i) - Ypre);end[tmp, Sy] qr([tmp Rs], 0);Sy cholupdate( Sy, sqrt(Wc(1)) * (sigmay(:,1) - Ypre) );Pxy zeros(L, O);for i 1 : 2*L1Pxy Pxy Wc(i) * (sigmax(:,i) - Xpre) * (sigmay(:,i) - Ypre);endK Pxy/Sy/Sy;X Xpre K*(Y-Ypre);U K*Sy;for i 1:OSpre cholupdate(Spre, U(:,i), -);endS Spre;end⛄ 运行结果⛄ 参考文献[1] 吕太之, 赵春霞. 一种基于SR-UKF的FastSLAM算法[J]. 计算机应用研究, 2012, 29(10):4.[2] 王宝宝吴盘龙. 基于平方根无迹卡尔曼滤波平滑算法的水下纯方位目标跟踪[J]. 中国惯性技术学报, 2016, 024(002):180-184.[3] 杨静, 郑南宁. 一种基于平方根Unscented卡尔曼滤波的GPS/DR组合定位算法[C]// 2008系统仿真技术及应用学术会议. 2008.[4] 翟丽霞. 基于集合无迹卡尔曼滤波的输油管道泄漏检测和定位[D]. 中国石油大学(华东), 2014.⛳️ 完整代码❤️部分理论引用网络文献若有侵权联系博主删除❤️ 关注我领取海量matlab电子书和数学建模资料