✅作者简介：热爱科研的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 module
sigmay = zeros(O, 2*L+1); % 3x13
Ypre = zeros(O, 1);
for i = 1:2*L+1
sigmay(:,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*L+1) - 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*L+1
Pxy = Pxy + Wc(i) * (sigmax(:,i) - Xpre) * (sigmay(:,i) - Ypre)';
end
K = Pxy/Sy/Sy';
X = Xpre + K*(Y-Ypre);
U = K*Sy';
for i = 1:O
Spre = cholupdate(Spre, U(:,i), '-');
end
S = 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.
