MATLAB实现卡尔曼滤波器仿真

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

视频连接为

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

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

/p/36745755

/qq_38364548/article/details/120250324

/weixin_44702605/article/details/125093141

/question/46869663

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

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

close allclc;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:TX=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数组中endsubplot(2,1,1)plot(x_real(1,:));hold onplot(z_out(1,:));plot(x_pre_out(1,:));plot(x_aft_out(1,:));grid onlegend('位置原始数据','位置测量数据','位置先验估计','位置最优估计');subplot(2,1,2)plot(x_real(2,:));hold onplot(z_out(2,:));plot(x_pre_out(2,:));plot(x_aft_out(2,:));grid onlegend('速度原始数据','速度测量数据','速度先验估计','速度最优估计');