【滤波】基于平方根无迹卡尔曼滤波SR-UKF实现信号去噪附matlab代码

✅作者简介：热爱科研的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.

⛳️ 完整代码

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料