卡尔曼滤波应用(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
结果2


🍊具体的思路为:
在这里插入图片描述

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️⃣数据准备
先给定初值 这个初值假定的是实际的速度和位置 X1 X2 以及我们的初始的P矩阵 和初始的最优估计值 因为刚开始的第一轮循环都是要用到的这些初值的 (如果这些值都没有的话,没办法进行循环)

3️⃣对于误差而言,采用的是直接随机生成误差,然后直接加入到生成的下一轮的过程量和预测量中
在这里插入图片描述
4️⃣ 进行5大公式的计算循环

5️⃣将其导出为表格的形式(每一个量有两列是因为这里采用了两个变量,一个是速度和位置)
在这里插入图片描述
6️⃣绘图


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

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

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

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

  • 12
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

~光~~

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值