卡尔曼滤波器及其简matlab仿真
一、卡尔曼滤波的起源
谈到信号的分析与处理，就离不开滤波两个字。通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内，为了消除噪声，可以进行频域滤波。但在许多应用场合，需要直接进行时域滤波，从带噪声的信号中提取有用信号。虽然这样的过程其实也算是对信号的滤波，但其所依据的理论，即针对随机信号的估计理论，是自成体系的。人们对于随机信号干扰下的有用信号不能“确知”，只能“估计”。为了“估计”，要事先确定某种准则以评定估计的好坏程度。
1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。
其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。
对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据和前一时刻的储值的估计，借助于系统本身的状态转移方程，按照一套递推公式，即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量，并且突破了平稳随机过程的限制，使卡尔曼滤波器适用于对时变信号的实时处理。
二、卡尔曼滤波的原理
卡尔曼滤波思想的来源是在海图作业中，航海长通常以前一时刻的船位为基准，根据航向、船速和海流等一系列因素推算下一个船位，但是他并不轻易认为船位就一定在推算船位上，还要选择适当的方法，通过仪器得到另一个推算船位。观测和推算这两个船位一般不重合，航海长需要通过分析和判断选择一个可靠的船位，作为船舰当前的位置。就是以现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。
卡尔曼滤波在数学上是一种线性最小方差统计估算方法，它是通过处理一系列带有误差的实际测量数据而得到物理参数的最佳估算。其实质要解决的问题是要寻找在最小均方误差下K X 的估计值^
K X 。它的特点是可以用递推的方法计算
K X ，其所需数据存储量较小，便于进行实时处理。具体来说，卡尔曼滤波就
是要用预测方程和测量方程对系统状态进行估计。
设动态系统的状态方程和测量方程分别为：
11,11,----Γ+Φ=K K K K K K K W X X
K K K K V X H Z +=
上两式子中，K X 是k 时刻的系统状态，1,-ΦK K 和1,-ΓK K 是k-1时刻到k 时刻的状态转移矩阵，K Z 是k 时刻的测量值，K H 是测量系统的参数，K W 和K V 分别表示过程和测量的噪声，他们被假设成高斯白噪声。如果被估计状态和观测量是满足上述第一式，系统过程噪声和观测噪声满足第二式的假设，k 时刻的观测K X 的估计^
X 可按下述方程求解。
状态的一步预测： (1)
均方误差进一步预测： (2) 滤波增益矩阵： (3) 滤波估计方程： (4) 均方误差更新矩阵（K 时刻的最优均方误差）： (5) 上述就是卡尔曼滤波器的5条基本公式，只有给定初值0X 和0P ，根据k 时刻的观测值K Z ，就可以递推计算得k 时刻的状态估计^
K X 。
/1,11ˆˆk k k k k x x φ---=*/1,11,1111
ˆT
T
k k k k k k k k k k P P Q φφ-------=**+ΓΓ1
/1/1ˆˆT T k k k k k k k k K P H H P H R ---⎡⎤=***+⎣⎦[
]
/1
/1
ˆˆk k k k k k k x x K Z H x
--=+-*[]/1
ˆk k k k P I K H P -=-**
下面论述卡尔曼五个公式的推导过程：
设系统
1111,----Γ+Φ=k k k k k k x x ω （1）
k k k k v x H z += k>=1 （2）
其中, 动态噪声{k ω}与量测噪声{k v }是互不相关的零均值白噪声序列，对任意k,j 其基本统计性质为:
E{k ω} = 0 Cov(k ω,j ω)=E[k ωT
j ω]=kj k Q δ
E{k v } = 0 Cov(k v ,j v )=E[k v T j v ]=kj k R δ Cov(k ω,j v )=E[k ωT
j v ]=0
其中kj δ是克罗内克δ函数，即：
又设初始状态的统计特征为 E[x 0]=
Var x 0 = E{(x 0-)( x 0-)T }=P 0
且x 0与{k ω}，{k v }都不相关，即 Cov(0x ,k ω)=0 Cov(0x ,k v )=0
在量测（k-1）次之后，已经有一个1/11ˆˆ---=k k k x x
的估计值，要推测k 状态的状态值，因为E{1-k ω} = 0，可定义1/ˆ-k k x 为由k-1次量测值所估计值1ˆ-k x 的一步预测合理数值，即
11,1/ˆˆ---Φ=k k k k k x x （3）
同样，考虑到E{k v } = 0，因而量测的期望值为1ˆ-k k x H 也是合适的。
考虑到这两点以后利用第k 次的量测数据k z 来估计k x ˆ的递推形式，其应
