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滤波器
%阿汪先生的博客.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   

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

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

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

%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的预测值