🌈写这篇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️⃣数据准备
先给定初值 这个初值假定的是实际的速度和位置 X1 X2 以及我们的初始的P矩阵 和初始的最优估计值 因为刚开始的第一轮循环都是要用到的这些初值的 (如果这些值都没有的话,没办法进行循环)
3️⃣对于误差而言,采用的是直接随机生成误差,然后直接加入到生成的下一轮的过程量和预测量中
4️⃣ 进行5大公式的计算循环
5️⃣将其导出为表格的形式(每一个量有两列是因为这里采用了两个变量,一个是速度和位置)
6️⃣绘图
🌿大家可以改改相应的误差矩阵和迭代次数看看不同的效果~
对了~ 分享一个颜色的网站 大家可以根据自己的喜好,画出不同的颜色~
🌈颜色和16进制转换
🌱当然,如果这个代码有误,或者有什么需要改进的话,欢迎大家批评指正!(由于刚刚接触matlab 对于矩阵存储这块不太熟悉, 如果有更好的方法,欢迎留言讨论~)
🌈ok,完结~(●’◡’●) 看到这里 点个赞叭 (●’◡’●)