卡尔曼滤波器及其简matlab仿真

其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间 模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出 当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出 满足最小均方误差的估计。
对于解决很大部分的问题,它是最优,效率最高甚至是最有用的。它的广泛应 用已经超过 30 年,包括机器人导航,控制，传感器数据融合甚至在军事方面的雷 达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别, 图像分割，图像边缘检测等等。
Cov（v
k
，v
j
)=E［v
k
v
T j
]＝
R
k kj
Coｖ（ k
,v

］=０
其中kj 是克罗内克 函数，即:
又设初始状态的统计特征为
Ｅ[ｘ０］＝
Var x０ ＝ E{(x0- )( x０- ）Ｔ}=Ｐ０
且ｘ0与{k ｝,{v k }都不相关,即
Coｖ( x 0 ,k )=0
Ｃoｖ( x 0 ,v k )=0
在量测(k-1)次之后，已经有一个 xˆk 1 xˆk 1 / k 1 的估计值，要推测 k 状态
的状态值,因为Ｅ｛ k -1 } = ０，可定义 xˆk / k 1 为由 k-１次量测值所估计值 xˆk 1 的
一步预测合理数值,即
xˆk / k 1 k ,k 1xˆk 1
卡尔曼滤波思想的来源是在海图作业中，航海长通常以前一时刻的船位为基
准，根据航向、船速和海流等一系列因素推算下一个船位,但是他并不轻易认为
船位就一定在推算船位上,还要选择适当的方法，通过仪器得到另一个推算船位。
观测和推算这两个船位一般不重合,航海长需要通过分析和判断选择一个可靠的
船位,作为船舰当前的位置。就是以现时刻的最佳估计为在前一时刻的最佳估计
足上述第一式，系统过程噪声和观测噪声满足第二式的假设,ｋ时刻的观测 X K 的
估计
^
X
可按下述方程求解。
状
态
的
xˆ一k /k 1 步 k ,k 1 预 xˆk 1 测
：
(
１
）
均
方
误
差 Pˆk /k1进 k ,k1 一Pk1 步kT,k1 k预1Qk1Tk测1
：
(
2
)
滤K
Pˆk
/
k波1
H
T k
增H k
卡 尔 曼 滤 波 器 及 其 简 ｍ ａ tlab 仿真
———————————————————————————————— 作者: ———————————————————————————————— 日期:
‫ﻩ‬
卡尔曼滤波器及其简 mａtｌａb 仿真
一、 卡尔曼滤波的起源 谈到信号的分析与处理,就离不开滤波两个字。通常,信号的频谱处于有限的
要用预测方程和测量方程对系统状态进行估计。
设动态系统的状态方程和测量方程分别为：
X K K ,K 1X K 1 K ,K 1WK 1
ZK HK X K VK
上两式子中, X K 是 k 时刻的系统状态， K ,K 1 和 K ,K1 是 k－1时刻到 k 时刻
的状态转移矩阵, ZK 是 k 时刻的测量值， H K 是测量系统的参数,WK 和VK 分别表 示过程和测量的噪声,他们被假设成高斯白噪声。如果被估计状态和观测量是满
Pˆk
/
k
益1
H
T k
矩Rk
1
阵
:
(
3
)
滤xk xˆk波/k1 K 估Zk Hk计 xˆk/k1方
程
:
（
4
)
均
方
误
差
更
新
矩
阵
(
Ｋ
时
刻
的
P最k
优 I
均K
方H k
误 Pˆk
差
/k 1
)
：
(
5
)
上述就是卡尔曼滤波器的5条基本公式,只有给定初值 X0 和 P0 ,根据ｋ时刻
^
的观测值 ZK ，就可以递推计算得ｋ时刻的状态估计 X K 。 下面论述卡尔曼五个公式的推导过程:
‫ﻩ‬设系统
x k x k ,k 1 k 1 k 1 k 1
（1)
zk H kxk vk
k＞=１
(２）
其中， 动态噪声{k }与量测噪声{v k }是互不相关的零均值白噪声序列，对
任意 k，j 其基本统计性质为:
E{k } = 0
Coｖ(k ， j )=Ｅ[k jT ]=Q k kj
E{v k } = 0
的基础上根据现时刻的观测值作线性修正。
卡尔曼滤波在数学上是一种线性最小方差统计估算方法,它是通过处理一系
列带有误差的实际测量数据而得到物理参数的最佳估算。其实质要解决的问题是
要寻找在最小均方误差下
X
K
的估计值
^
XK
。它的特点是可以用递推的方法计算
X K ,其所需数据存储量较小，便于进行实时处理。具体来说,卡尔曼滤波就是
卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据 和前一时刻的储值的估计,借助于系统本身的状态转移方程，按照一套递推公式， 即可算出新的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量,并 且突破了平稳随机过程的限制,使卡尔曼滤波器适用于对时变信号的实时处理。 二、 卡尔曼滤波的原理
频率范围内,而噪声的频谱则散布在很广的频率范围内，为了消除噪声,可以进行 频域滤波。但在许多应用场合，需要直接进行时域滤波，从带噪声的信号中提取 有用信号。虽然这样的过程其实也算是对信号的滤波,但其所依据的理论,即针对 随机信号的估计理论,是自成体系的。人们对于随机信号干扰下的有用信号不能 “确知”,只能“估计”。为了“估计”,要事先确定某种准则以评定估计的好坏 程度。
1９６0 年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文 Ａ Neｗ Ａpproach to Liｎｅar Filｔerinｇ and Ｐredictioｎ P ｒｏbｌeｍs(线性滤波与预测问题的新方法),在这篇文章里一种克服了维纳滤 波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤 波应用广泛且功能强大,它可以估计信号的过去和当前状态甚至能估计将来的状 态即使并不知道模型的确切性质。
(3)
同样,考虑到 E{v k } = ０，因而量测的期望值为 H k xˆk 1 也是合适的。
‫ﻩ‬考虑到这两点以后利用第 k 次的量测数据 zk 来估计 xˆk 的递推形式,其应该 为:
xˆk xˆk / k xˆk / k 1 K k(zk H k xˆk / k 1 )
（4）
这里的 K k 是一个待定的增益矩阵,其应使误差矩阵极小。