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

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