卡尔曼滤波应用（DR_CAN教授卡尔曼滤波——matlab版）

🌈写这篇blog 的初衷是记录一下学习卡尔曼滤波代码的复现的过程
本文的数据和学习过程 来自b站的DR_CAN教授卡尔曼滤波器 主要是复现一下编写的经过~ 其实就是DR_CAN教授excel文档的 matlab版~
学习数据来源

⭐️当然，大家在学习卡尔曼滤波的时候欢迎看看我的前面的几篇blog （学习路径是谢钢老师的那本书的第六章和DR_CAN教授的讲解~）
blog1
blog2
牛顿迭代法和最小二乘法matlab代码
☘️关于整个的数据~大家可以点一下上面的学习链接（DR_lCAN教授说明了要自己能够破解密码哈哈哈哈哈 ）

放一张卡尔曼滤波经典的5大公式

🐬下面是代码的复现

clear,clc;
%%%%%%%%%%%%该程序是经典卡尔曼滤波最简化的版本

%%%%%%%%%%%%%%%%
%初始化一切值 读取数据
%拟创建一个table 
k =1;%迭代次数
max=170;%迭代总次数

Q=[0.1,0;0,0.1]; %过程误差
R=[1,0;0,1];%测量误差
A=[1,1;0,1];%状态矩阵
H=[1,0;0,1];%控制矩阵
I=[1,0;0,1];%单位矩阵



%数据准备  注意 readtable之前要转换一下
filename ='initial_data.xlsx';
data=table2array(readtable(filename));

%已知量
%%%%%%先验预测量 全部用小写表示
%x_l location  x_v speed
%初始值 
x_l=0;
x_v=0;



x_k=[x_l;x_v];

%过程噪声 测量噪声
w=sqrt(0.1)*randn(1,2);
v=randn(1,2);


w_k=w;
v_k=v;


%%%%%%测量量 全部用大写表示

X_A=[0;1];
Z_k=[0;0];



p_k=zeros(2,2);
P_K=[1,0;0,1];

K=zeros(2,2);

X=[0;1];

%%%%%%%%%%%%开始迭代
%从k=2开始 由于matlab索引没有0  所以第一个为初始值 第二个开始正式迭代
%x_k(k+1:k+2,:) 指的就是x_k  而 x_k(k-1:k,1)指的就是x_k-1;

while k<max

    k=k+1;
    %噪声
    w=sqrt(0.1)*randn(1,2);
    v=randn(1,2);
    w_k(k,:)=w;
    v_k(k,:)=v;


    X_A(2*k-1:2*k,1)=A*X_A(2*k-3:2*k-2,1)+(w_k(k,:))';
    Z_k(2*k-1:2*k,1)=H*X_A(2*k-3:2*k-2,1)+(v_k(k,:))';


    x_k(2*k-1:2*k,1)=A*X(2*k-3:2*k-2,1);
    
    p_k(2*k-1:2*k,:)=A*P_K(2*k-3:2*k-2,:)*A'+Q;
    
    K(2*k-1:2*k,:)=p_k(2*k-1:2*k,:)*H'*((H*p_k(2*k-1:2*k,:)*H'+R)^-1);

    X(2*k-1:2*k,1)=x_k(2*k-1:2*k,1)+K(2*k-1:2*k,:)*(Z_k(2*k-1:2*k,1)-H*x_k(2*k-1:2*k,1));

    P_K(2*k-1:2*k,:)=(I-K(2*k-1:2*k,:)*H)*p_k(2*k-1:2*k,:);
 
    

end

%%%%开始转换一下 让表格变好看一点
for i=1:max
    x_k_k(i,1:2)=x_k(2*i-1:2*i,1);
    X_k(i,1:2)=X(2*i-1:2*i,1);
    Z(i,1:2)=Z_k(2*i-1:2*i,1);
    X_AA(i,1:2)=X_A(2*i-1:2*i,1);

end

% T = table(x_k_k,w_k,X_k,v_k);
T = table(X_AA,w_k,Z,v_k,x_k_k,X_k,'VariableNames',{'实际值','过程误差','测量值','测量误差','先验估计值','最优估计值'});
Q = table(p_k,P_K,K); 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画图%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(1);
plot(x_k_k(:,1),'color',[186,85,211]/255,'Linewidth', 1);
hold on
plot(Z(:,1),'color',[0 0.4470 0.7410],'Linewidth', 1);
hold on

plot(X_AA(:,1),'color',[128,128,0]/255,'Linewidth', 1);
hold on
plot(X_k(:,1),'color',[255,127,80]/255,'Linewidth', 1.5);
title('位置');
legend('先验位置','测量位置','实际位置','最优估计值')

xlabel('迭代次数/k','fontweight','bold');%坐标label 字体加粗
ylabel('实际位置 m','fontweight','bold');
grid on

figure(2);
plot(x_k_k(:,2),'color',[0.9290 0.6940 0.1250],'Linewidth', 1);
hold on
plot(Z(:,2),'color',[0.4940 0.1840 0.5560],'Linewidth', 1);
hold on
plot(X_AA(:,2),'color',[65,105,225]/255,'Linewidth', 1);
hold on
plot(X_k(:,2),'color',[0.4660 0.6740 0.1880],'Linewidth', 1.5);
title('速度');
legend('先验速度','测量速度','实际速度','最优估计值')
xlabel('迭代次数/k');
ylabel('实际速度 m/s');
grid on

🌴最后复现的结果：

---

🍊具体的思路为：

1️⃣设置好初始的值
包括我们的H矩阵（观测量与系统状态之间的关系矩阵），A（状态转移矩阵） Q测量量的误差协方差矩阵 等

Q=[0.1,0;0,0.1]; %过程误差
R=[1,0;0,1];%测量误差
A=[1,1;0,1];%状态矩阵
H=[1,0;0,1];%控制矩阵
I=[1,0;0,1];%单位矩阵

2️⃣数据准备
先给定初值 这个初值假定的是实际的速度和位置 X₁ X₂ 以及我们的初始的P矩阵 和初始的最优估计值 因为刚开始的第一轮循环都是要用到的这些初值的 （如果这些值都没有的话，没办法进行循环）

3️⃣对于误差而言，采用的是直接随机生成误差，然后直接加入到生成的下一轮的过程量和预测量中

4️⃣ 进行5大公式的计算循环

5️⃣将其导出为表格的形式（每一个量有两列是因为这里采用了两个变量，一个是速度和位置）

6️⃣绘图

---

🌿大家可以改改相应的误差矩阵和迭代次数看看不同的效果~

对了~ 分享一个颜色的网站 大家可以根据自己的喜好，画出不同的颜色~
🌈颜色和16进制转换

🌱当然，如果这个代码有误，或者有什么需要改进的话，欢迎大家批评指正！（由于刚刚接触matlab 对于矩阵存储这块不太熟悉， 如果有更好的方法，欢迎留言讨论~）

🌈ok,完结~(●’◡’●) 看到这里 点个赞叭 (●’◡’●)