【Matlab】卡尔曼滤波器的程序实现(kalman、Matlab)

Matlab——卡尔曼滤波器(matlab实现)


一、原理:看了网上很多资料,这两个讲的比较好。

1.高中生能看懂的详细通俗讲解卡尔曼滤波Kalman Filter原理及Python实现教程 https://blog.csdn.net/varyshare/article/details/95065650
2. 图像处理之目标跟踪(一)之卡尔曼kalman滤波跟踪(主要为知识梳理)(转载)https://blog.csdn.net/coming_is_winter/article/details/79048700


二、方程概述:

1、状态预测方程与不确定性的传递

方程1
方程2

1、卡尔曼滤波方程组中的第一条公式,状态预测公式,而F就是状态转移矩阵,它表示我们如何从上一状态来推测当前状态。而B则是控制矩阵,它表示控制量u如何作用于当前状态。
2、卡尔曼滤波方程组中的第二条公式,它表示不确定性在各个时刻间的传递关系。

3、输出变量都是输入变量的线性组合,这也就是称卡尔曼滤波器为线性滤波器的原因所在。
4、卡尔曼滤波算法是一种基于最小均方误差的最优线性递归滤波方法,以状态方程和观测方程为基础,运用递归方法来预测线性系统变化。

  • F:状态转移矩阵,它表示我们如何从上一状态来推测当前状态。
  • B:控制矩阵,它表示控制量u如何作用于当前状态。

2、协方差矩阵 Σ -

  • 在这里插入图片描述 : 系统中每一个时刻的不确定性都是通过协方差矩阵 Σ 来给出的。

    • 这种不确定性在每个时刻间还会进行传递。
    • 不仅当前物体的状态(例如位置或者速度)是会(在每个时刻间)进行传递的,而且物体状态的不确定性也是会(在每个时刻间)进行传递的。
    • 这种不确定性的传递就可以用状态转移矩阵来表示
  • Q: 协方差矩阵 Q 来表示预测模型本身的噪声(也即是噪声在传递过程中的不确定性)


3、卡尔曼系数K(t)

在这里插入图片描述

  • K:称为卡尔曼系数,它也是一个矩阵,它是对残差的加权矩阵,有的资料上称其为滤波增益阵。

  • K首先权衡预测状态协方差矩阵Σ和观测值矩阵R的大小,并以此来觉得我们是更倾向于相信预测模型还是详细观测模型。

    • 如果相信预测模型多一点,那么这个残差的权重就会小一点。
    • 如果相信观察模型多一点,这个残差的权重就会大一点。
  • K(滤波增益阵)还负责把残差的表现形式从观测域转换到了状态域。

    • 观测值与状态值所采用的描述特征或者单位都有可能不同,显然直接用观测值的残差去更新状态值是不合理的。
  • R(协方差矩阵)来表示观测中的不确定性。

  • H:真实状态到其观测状态的一个变换关系,这个变换关系我们记为h(•),而且这个h(•)还是一个线性函数,可记为矩阵变换H()。


4、对估计值进行修正

在这里插入图片描述

  • 在这里插入图片描述是根据上一状态推测而来的,那么它与“最优”估计值之间的差距现在就是等式右端加号右侧的部分。
  • 在这里插入图片描述表示实际观察值与预估的观测值之间的残差。这个残差再乘以一个系数K就可以用来对估计值进行修正。K称为卡尔曼系数。

5、噪声分布 Σ 进行更新。

在这里插入图片描述

  • 对最优估计值的噪声分布进行更新。
  • 我们将这五个公式分成预测组和更新组。预测组总是根据前一个状态来估计当前状态。更新组则根据观测信息来对预测信息进行修正,以期达到最优估计之目的。
    -----【Matlab】卡尔曼滤波器的程序实现(kalman、Matlab)_第1张图片
  • 我们将这五个公式分成预测组和更新组。预测组总是根据前一个状态来估计当前状态。更新组则根据观测信息来对预测信息进行修正,以期达到最优估计之目的。

三、Matlab程序

%kalman滤波器
%阿汪先生的博客.ws
clc          %清除命令行
clear        %清空变量
N=200;    	 %数据个数N
w(1)=0;      %w为预测模型的过程噪声  
w=randn(1,N) %randn()创建随机矩阵,服从正态分布
x(1)=0;    	 %初始值
f=1;         %a为方程1中的状态转移矩阵,这里为一维变量

%x的预测值
for k=2:N;    	
x(k)=f*x(k-1)+w(k-1);     %方程1,为x的推测值
end   

%观察不确定性
V=randn(1,N);    %V为观察噪声,randn()创建随机矩阵,服从正态分布
q1=std(V);    	 %返回一个包含与每列对应的标准差的行向量
R=q1.^2;    	% R 来表示观测中的不确定性(协方差矩阵)

%实际观察值Y
H=0.4;          %H为方程中H(k)  
y=H*x+V;        %Y为实际观察值  

%预测不确定性
q2=std(w);      %返回一个包含与每列对应的标准差的行向量,预测模型
Q=q2.^2;    	% Q 预测模型本身的噪声(协方差矩阵)

%x的修正值(经卡尔曼滤波的值)
p(1)=0;      %协方差矩阵 Σ初值,代表一个时刻的不确定性
s(1)=0;        %修正值初值
for t=2:N;    
p1(t)=f.^2*p(t-1)+Q;		 %方程2,p1(t)协方差矩阵 Σ-,由上一状态推测而来
k(t)=H*p1(t)/(H.^2*p1(t)+R);    	%方程3 ,k(t)为卡尔曼系数
s(t)=f*s(t-1)+k(t)*(y(t)-f*H*s(t-1)); %方程4,根据估计值进行修正
p(t)=p1(t)-H*k(t)*p1(t);    		%方程5,p(t)为协方差矩阵 Σ,对最优值的噪声分布进行更新
end

%画曲线图
t=1:N;    
plot(t,s,'r',t,y,'g',t,x,'b');
%红色曲线:x的修正值
%绿色曲线,x的实际观察值
%蓝色曲线,x的预测值

程序效果

【Matlab】卡尔曼滤波器的程序实现(kalman、Matlab)_第2张图片

你可能感兴趣的:(算法实现,Matlab)