MATLAB实现卡尔曼滤波器仿真

MATLAB实现卡尔曼滤波器仿真

本仿真是在学习B站DR_CAN的视频之后的练习。联系针对的是第五个视频中的二维示例。

视频连接为

【【卡尔曼滤波器】1_递归算法_Recursive Processing】 https://www.bilibili.com/video/BV1ez4y1X7eR/?share_source=copy_web&vd_source=b275b7cab48480c7de4a23f928695bfc

顺带记录一些看到的对学习卡尔曼滤波算法有益的网页。

https://zhuanlan.zhihu.com/p/36745755

https://blog.csdn.net/qq_38364548/article/details/120250324

https://blog.csdn.net/weixin_44702605/article/details/125093141

https://www.zhihu.com/question/46869663

再次感谢DR_CAN的视频让我可以对卡尔曼滤波有一个清晰的认识，同时也感谢在网上分享自己学习心得的网友大神，使我在学习过程中有更多的参考。

仿真的思路为首先根据系统的状态方程以及观测方程得出系统的实际变化以及测量结果，之后再对测量结果进行卡尔曼滤波得出最后的结果。

close all
clc;clear;

Q = [0.1,0;0,0.1];%过程噪声的协方差矩阵
R = [1,0;0,1];%测量误差的协方差矩阵
A=[1,1;0,1];%定义A矩阵
H=[1,0;0,1];%定义H矩阵
I=eye(2);%定义单位矩阵

x_real = [0;1];%实际值

z_out = [0;0];%测量值

x_pre_out = [0;0];%输出的先验估计
x_aft_out = [0;1];%输出的后验估计

P_pre=[0,0;0,0];%先验误差协方差
P_aft=[1,0;0,1];%后验误差协方差

T = 500;%仿真步数
t = 1:1:T;
for i = 2:1:T
	X=A*x_real(:,i-1)+normrnd(0,sqrt(Q(1,1)),[2,1]);%计算实际值
	x_real = [x_real,X];

    Z=H*X+normrnd(0,sqrt(R(1,1)),[2,1]);%计算当前时刻的测量值
    z_out = [z_out,Z];

end
% subplot(2,1,1)
% plot(x_real(1,:));
% hold on
% plot(z_out(1,:));
% 
% subplot(2,1,2)
% plot(x_real(2,:));
% hold on
% plot(z_out(2,:));

for i = 2:1:T
    %预测
    %计算先验估计
    x_pre = A * x_aft_out(:,i - 1);
    %计算先验误差协方差矩阵
    P_pre = A * P_aft * A' + Q;
    %计算当前的卡尔曼增益
    K=P_pre*H'*inv(H*P_pre*H'+R);
    %更新误差协方差
    P_aft = (I - K * H) * P_pre;
    %计算当前时刻的后验估计（最优估计）
    x_aft = x_pre + K * (z_out(:,i) - H * x_pre);

    x_pre_out = [x_pre_out,x_pre];
    x_aft_out = [x_aft_out,x_aft];





    
%     X_pre=A*x_aft_out(:,k-1);%计算当前时刻的先验估计
%     
%     P_pre=A*P_aft*A'+Q;%计算当前时刻的误差协方差的先验
%     K=P_pre*H'*inv(H*P_pre*H'+R);%计算当前时刻的卡尔曼增益
%     P_aft=(I-K*H)*P_pre;%计算当前时刻的误差协方差的后验并更新
%     X_pro=X_pre+K*(z_out(:,k)-H*X_pre);%计算当前时刻的后验估计（最优估计）
%     
% 
% 
%     x_aft_out=[x_aft_out,X_pro];%将每一次计算出来的后验估计放入X_pro_out数组中
%     x_pre_out=[x_pre_out,X_pre];%将每一次计算出来的先验估计放入X_pre_out数组中




end


subplot(2,1,1)
plot(x_real(1,:));
hold on
plot(z_out(1,:));
plot(x_pre_out(1,:));
plot(x_aft_out(1,:));
grid on
legend('位置原始数据','位置测量数据','位置先验估计','位置最优估计');


subplot(2,1,2)
plot(x_real(2,:));
hold on
plot(z_out(2,:));
plot(x_pre_out(2,:));
plot(x_aft_out(2,:));
grid on
legend('速度原始数据','速度测量数据','速度先验估计','速度最优估计');