matlab仿真-卡尔曼滤波（KF）与扩展卡尔曼滤波（EKF）

孤独的傅里叶

已于 2023-10-23 10:07:19 修改

阅读量2k

点赞数 5

分类专栏：水声工程 matlab 文章标签： matlab 信号处理算法

于 2023-10-17 21:20:30 首次发布

本文链接：https://blog.csdn.net/Xujing1143/article/details/133894013

版权

水声工程同时被 2 个专栏收录

9 篇文章 68 订阅

订阅专栏

matlab

9 篇文章 5 订阅

订阅专栏

matlab仿真-卡尔曼滤波与扩展卡尔曼滤波

近期由于毕设需要，学习推导了卡尔曼滤波和扩展卡尔曼滤波（KF,EKF），在此做一个记录，不做理论上的推导，只做仿真记录。

一、卡尔曼滤波器

（一）理论简述

卡尔曼滤波简单来说就是使用测量到的量测结果对本地预测结果，根据一定比例，进行修正的过程，其基本公式包含五个：
在这里插入图片描述

对这个过程进行一遍详细解读：卡尔曼滤波分为两个部分——时间更新和状态更新。

1、时间更新过程就是根据本地信息进行预测，参见式1和式2。 Xk表示上一时刻系统状态，A称为状态转移矩阵， Xk+1表示本地对当前时刻状态的预测。在这个基础上，我们计算先验估计的协方差矩阵Pk+1- ，它只和状态转移矩阵A、上一时刻协方差矩阵Pk 、过程激励噪声协方差矩阵Q有关。
PS，（1）k+1时刻的先验估计协方差Pk+1-，这个协方差矩阵只要确定了一开始的P0，后面都可以递推出来，而且初始协方差矩阵P只要不是为0，它的取值对滤波效果影响很小，都能很快收敛；（2）过程激励噪声认为是状态转移矩阵与实际过程之间的误差，在仿真阶段一般可以把它看成一个简单的高斯噪声叠加上，也可以理解为对预测结果的信任程度，比如我的目标在y轴运动不太均匀，但是我把它简单看成匀速运动了，那么我对我在y轴的目标运动预测结果就不太信任，就可以把其对应的协方差值增大。
2、状态更新过程就是测到一个量测值了，即传感器传回来这时候的测量值后，对前面本地预测的结果进行修正。为了方便理解，可以把式5转换为两个式，如下。要结合预测的状态和量测，我们的状态要转换成量测的形式才能和量测比较，对预测的状态经过式6转换为预测的量测，再将差乘以卡尔曼增益K，用以修正先验估计Xk+1-，此时得到后验估计Xk+1 。

PS，（1）R表示观测噪声协方差矩阵，可以理解为仪器误差，同样可以看成对量测的信任程度，例如如果运动目标携带的速度传感器特别精准，可以将对应的协方差值减小；（2）卡尔曼增益K与先验估计协方差P-存在一定关系，如果先验估计误差特别大，那么估计不可信，模型应当加大对量测的信任程度，也就是增加K值；反之减小K值。
由于本篇只介绍其应用，就不仔细推导了。下面以仿真案例介绍其使用方法：
假设一个水下航行器，自身带有一个测量位置的传感器，预设该航行器在一个二维平面做直线运动：
在这里插入图片描述

设状态向量
在这里插入图片描述

量测
(1.10)

（二）仿真：

%% CSDN_kf
clear all;clc; close all;
%% 初始化
tf = 500; %时刻
Q = diag(ones(4, 1) * 0.1);%预测噪声协方差矩阵Q：假设预测过程上叠加一个高斯噪声，协方差矩阵为Q
R = diag(ones(2, 1) * 0.2);%观测噪声协方差矩阵R：假设观测过程上存在一个高斯噪声，协方差矩阵为R
P =eye(4);
X=zeros(4,tf);    %X表示目标现在的实际状态X=[X,Y,VX,VY]
Xnew=zeros(4,tf); %Xnew表示目标状态的后验估计值
X(:,1)=[1;7;1;2]; %按照y=2x+5初始化
Xnew(:,1)=X(:,1);
Z=zeros(2,tf);    %Z表示测得的量测值Z=[X,Y]
Z(:,1)=[1;7];
z_=zeros(2,tf);
z_(:,1)=Z(:,1);   %z_表示预测的量测，即从估计的状态转换过来的量测

%%
for k = 2 : tf 
    %% %%%%%%%%%%%%%%%%模拟系统%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    w=sqrt(0.1)*randn(4,tf);%模拟航行器在水下航行受到的扰动
    X(:,k) =[k;2*k+5;1;2]+w(:,k);
    v=sqrt(0.2)*randn(2,tf);%模拟量测在测量中受到的扰动
    Z(:,k) =[k;2*k+5]+v(:,k);%模拟的测得的量测 
    
    %% %%%%%%%%%%%%%%%%EKF开始%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    A=[1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1];%状态转移矩阵，xk对xk-1求导
    Xpre =A*Xnew(:,k-1); %先验估计，即预测值
    H =[1,0,0,0;0,1,0,0];%量测矩阵
    z_ =H*Xpre;          %预测的量测    
    PP=A*P*A'+Q;         %先验估计协方差矩阵
    Kk=PP*H'*inv(H*PP*H'+R);       %卡尔曼增益
    Xnew(:,k)=Xpre+Kk*(Z(:,k)-z_); %后验估计
    P=PP-Kk*H*PP;                  %后验估计误差协方差矩阵
end
t = 2 : tf;
figure;
plot(X(1,t),X(2,t),'b',Xnew(1,t),Xnew(2,t),'r*');
xlabel('x位置');ylabel('y位置');
legend('真实值','KF估计值');

仿真结果：
在这里插入图片描述

二、扩展卡尔曼滤波器

（一）理论简述

在此基础上，做扩展卡尔曼滤波的仿真。在上面我们可以发现，k+1时刻状态与k时刻状态必须呈现线性关系，量测与状态之间也是，但是目标呈线性关系还是较少的，我们以抛物线运动为例，仿真一下扩展卡尔曼滤波（EKF）。
在这里插入图片描述

扩展卡尔曼是将这种非线性的函数使用泰勒展开，并只保留其一次项，以作近似。
在这里插入图片描述

所以，将式1换为式11，式6换为式12，并将状态转换矩阵A和量测矩阵H换为以下：
在这里插入图片描述

仿真：设状态向量如下
在这里插入图片描述

所以状态转移矩阵
在这里插入图片描述

量测（这里为了使用EKF，一时没想到合适的量测，胡乱设置的量测，没有什么物理意义）
在这里插入图片描述

（二）仿真：

仿真代码如下：

%% CSDN_ekf
clear all;clc; close all;
%% 初始化
tf = 500; %时刻
Q = diag(ones(4, 1) * 0.001);%预测噪声协方差矩阵Q：假设预测过程上叠加一个高斯噪声，协方差矩阵为Q
R = diag(ones(2, 1) * 0.02);%观测噪声协方差矩阵R：假设观测过程上存在一个高斯噪声，协方差矩阵为R
P =eye(4);
X=zeros(4,tf);    %X表示目标现在的实际状态X=[X,Y,VX,VY]
Xnew=zeros(4,tf); %Xnew表示目标状态的后验估计值
X(:,1)=[1;1;1;pi/4]; %按照y=2x+5初始化
Xnew(:,1)=X(:,1);
Z=zeros(2,tf);    %Z表示测得的量测值Z=[f(X,V),f(Y,theta)]
Z(:,1)=[1/10+1;1+pi/4];
z_=zeros(2,tf);
z_(:,1)=Z(:,1);   %z_表示预测的量测，即从估计的状态转换过来的量测

%%
for k = 2 : tf 
    %% %%%%%%%%%%%%%%%%模拟系统%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    w=sqrt(0.001)*randn(4,tf);%模拟航行器在水下航行受到的扰动
    X(:,k) =[X(1,k-1)+X(3,k-1)*cos(X(4,k-1));X(2,k-1)+X(3,k-1)*sin(X(4,k-1));X(3,k-1);X(4,k-1)]+w(:,k);
    v=sqrt(0.02)*randn(2,tf);%模拟量测在测量中受到的扰动
    Z(:,k) =[X(1,k)^2/10+X(3,k);X(2,k)+X(4,k)]++v(:,k);%模拟的测得的量测 
    
    %% %%%%%%%%%%%%%%%%EKF开始%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Xpre =[Xnew(1,k-1)+X(3,k-1)*cos(Xnew(4,k-1));Xnew(2,k-1)+Xnew(3,k-1)*sin(Xnew(4,k-1));Xnew(3,k-1);Xnew(4,k-1)]; %先验估计，即预测值
    A=[1,0,cos(Xnew(4,k-1)),-Xnew(3,k-1)*sin(Xnew(4,k));0,1,-sin(Xnew(4,k-1)),Xnew(3,k-1)*cos(Xnew(4,k));0,0,1,0;0,0,0,1];%状态转移矩阵，xk对xk-1求导
    H =[Xpre(1)/5,0,1,0;0,1,0,1];%量测矩阵
    z_ =[Xpre(1)^2/10+Xpre(3);Xpre(2)+Xpre(4)];          %预测的量测    
    PP=A*P*A'+Q;         %先验估计协方差矩阵
    Kk=PP*H'*inv(H*PP*H'+R);       %卡尔曼增益
    Xnew(:,k)=Xpre+Kk*(Z(:,k)-z_); %后验估计
    P=PP-Kk*H*PP;                  %后验估计误差协方差矩阵
end
t = 2 : tf;
figure;
plot(X(1,t),X(2,t),'b',Xnew(1,t),Xnew(2,t),'r*');
xlabel('x位置');ylabel('y位置');
legend('真实值','KF估计值');