【滤波】基于平方根无迹卡尔曼滤波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电子书和数学建模资料