卡尔曼滤波 之 matlab仿真

卡尔曼滤波的matlab仿真

前言

毕业设计时选择了超宽带定位设计的硬件电路设计，但苦于硬件难以取得大的进步，开始对算法的实现进行研究。通过阅读论文发现，很多人都对卡尔曼滤波进行各种各样的改进，以达到对数据的最好处理，本着对算法更好的理解，然后在MATLAB上对卡尔曼滤波进行了仿真。

1. 结果

本文对匀速运动进行仿真，先看一下实验结果

细节处，目测结果确实还可以，黑线（预测值）比白线（估计值）更逼近红线（真实路径）
并进行蒙特卡洛实验，得到结果。红色尖峰由于运动突然发生变化后，模型不够完善。

2.仿真程序

1. 模拟运动

Xtrue = zeros(4,simTime);%simTime次仿真，4行simTime列
Xtrue(:, 1) = [100,100,10,15]';%初始位置
for simscan = 2:simTime
        if simscan < 100  %第100秒处转弯
                Xtrue(:, simscan) = F_x * Xtrue(:, simscan-1);
        else
                Xtrue(:, simscan) = F_y * Xtrue(:, simscan-1);
        end
end

Xtrue = Xtrue + mvnrnd(zeros(4,1), Q, simTime)'; %加入噪声

2. 观测运动

%% 观测结果
Xupd = zeros(4, simTime);
Xupd(:,1) = Xtrue(:, 1);
Z = zeros(2,simTime);%x y
for simscan = 1:simTime
        Z(:, simscan) = H * Xtrue(:,simscan);
end
Z = Z +mvnrnd(zeros(2,1),R,simTime)';

3. 卡尔曼滤波

%% 卡尔曼处理
%Xtrue是真实值
%现在是根据Z和估计值对
%当前的测量值和前一个采样周期的估计值，对下一状态进行估计
Xupd = zeros(4, simTime);
Xupd(:,1) = [Z(:,1);0;0];
Pupd = [R,zeros(2,2);
        zeros(2,2),40^2/4*eye(2)];
for simscan = 2:simTime
        Xpre = F * Xupd(:, simscan-1);% 算法根据上一时刻估计的值，后期对其进行优化
        Ppre = F * Pupd * F' + Q;%同样 算法根据上一时刻得到的值对协方差矩阵进行估计，后期对其进行优化

        Zpre = H * Xpre;% 这是测量值
        Sk=H * Ppre * H' + R;% 以及测量值的协方差矩阵

        K = Ppre * H' * inv(Sk); % PH'/H*Ppre*H'+R

        Xupd(:,simscan) = Xpre + K*(Z(:,simscan)- Zpre);%对状态的估计
        Pupd=(eye(4)-K * H)*Ppre;% 以及滤波后的协方差矩阵

end

4. 进行蒙特卡洛实验后计算RMSE

 RMSE_Z_Rec(1,simscan,mtclInd) =sum((Z(1:2,simscan)-Xtrue(1:2,simscan)).^2,1);
 RMSE_Rec(1,simscan,mtclInd) =sum((Xupd(1:2,simscan)-Xtrue(1:2,simscan)).^2,1);

总结

1. 根据实验结果，预测误差明显小于观测误差，但还是存在5 个单位的误差，接下来继续努力，希望能够将其变得更小！！！

源码：https://download.csdn.net/download/cry_smile/15533627