第2章 卡尔曼滤波和非线性系统滤波方法

第2章 卡尔曼滤波和非线性系统滤波方法
2.2.3 卡尔曼滤波 卡尔曼滤波是一种离散状态空间模型下的线性最小方差
估计。系统的动态方程和量测方程都是线性高斯的，通常表
示为
xk =Fk 1 xk 1 qk 1
(2-11)
yk Hk xk rk
(2-12)
其中，xk∈Rn表示状态向量，yk∈Rm表示量测向量，qk－1~ N(0， Qk－1)和rk－1~N(0，Rk)分别是服从高斯分布的过程噪声序列和量 测噪声序列，矩阵Fk－1是动态模型的转移矩阵，Hk是量测模型 矩阵。假设先验分布也符合高斯分布，即x0~N(x0|0，P0)，则可 将上述模型以概率形式表示为
p( xk
|
y1:k )

1 Zk
p( yk | xk , y1:k 1 ) p( xk | y1:k 1 )
1
= Zk
p( yk | xk ) p( xk | y1:k 1)
(2-10)
其中，归一化常数项如式(2-7)所示。式(2-10)在推导过程中丢
掉了历史量测数据y1: k－1，这是因为，在给定xk条件下yk独立于 历史量测信息y1: k－1。
p( xk | y1:k 1) N( xk | xˆk|k 1, Pk|k 1)
(2-15)
p( xk | y1:k ) N( xk | xˆk|k , Pk|k )
(2-16)
p( yk | y1:k1) N( yk | Hk xˆk|k1, Sk )
(2-17)
上述分布中的参数可以根据下面的卡尔曼滤波器的预测和更
(1) 初始化。递归是从先验分布p(x0)开始的。 (2) 预测。状态向量xk在第k步的预测分布可以通过 Chapman Kolmogoro方程获取
p( xk | y1:k 1 ) p( xk | xk 1 ) p( xk 1 | y1:k 1 )dxk 1
(2-5)
(3) 更新。当获得第k步的量测值yk后，后验分布可由贝叶
p( xk , xk 1 | y1:k 1) p( xk | xk 1, y1:k 1) p( xk 1 | y1:k 1)
p( xk | xk 1) p( xk 1 | y1:k 1)
(2-8)
考虑到系统的马尔可夫特性，在上式推导过程中丢弃了历史量
测值y1: k－1。给定y1: k－1时，xk的边缘分布可以通过对式(2-8)在 xk－1处求积分来获取，也就是使用Chapman Kolmogoro方程
p( xk | y1:k 1 ) p( xk | xk 1 ) p( xk 1 | y1:k 1 )dxk 1
(2-9)
第2章 卡尔曼滤波和非线性系统滤波方法
如果xk－1是离散的，那么式(2-9)中的积分变为在xk－1处的累加 和。在给定y1: k的条件下，xk的分布可以通过贝叶斯规则进行 计算
新步骤计算。
预测步骤为
xˆk|k 1 Fk 1 xˆk 1|k 1
(2-18)
Pk|k 1

F P FT k 1 k 1|k 1 k 1
Qk 1
(2-19)
第2章 卡尔曼滤波和非线性系统滤波方法
更新步骤为
vk yk Hk xˆk|k 1
(2-20)
Sk

H
第2章 卡尔曼滤波和非线性系统滤波方法
第2章 卡尔曼滤波和非线性系统滤波方法
2.2 卡尔曼滤波算法 2.3 扩展卡尔曼滤波算法 2.4 不敏卡尔曼滤波算法 2.5 积分卡尔曼滤波算法 2.6 容积卡尔曼滤波算法 2.7 傅立叶厄米特卡尔曼滤波算法 2.8 中心差分卡尔曼滤波算法 2.9 小结
第2章 卡尔曼滤波和非线性系统滤波方法
时刻之后(k+2，k+3，…，T)的状态都无关，即
p( xk1 | xk:T , yk:T ) p( xk1 | xk )
(2-3)
(2) 量测向量的条件独立性。当前时刻的量测值yk与当前时 刻的状态值xk有关，而与k时刻之前(k－1，k－2，…，1)的量测 值和状态值无关，即
p( yk | x1:k , y1:k1) p( yk | xk )
序列是一个马尔科夫链。符合该性质，意味着k时刻的状态估计
xk只与k－1时刻的状态值有关，而与k－1时刻之前(k－2，k－ 3，…，1)的状态值无关，即
第2章 卡尔曼滤波和非线性系统滤波方法
p( xk | x1:k1, y1:k1) p( xk | xk1)
Baidu Nhomakorabea
(2-2)
同样，k－1时刻的状态估计值也只与k时刻的状态值有关，而与k
(2-4)
第2章 卡尔曼滤波和非线性系统滤波方法
2.2.2 最优滤波方程 最优滤波就是在给定历史量测值的情况下，计算出每一
时刻状态向量xk的边缘后验分布值p(xk|y1: k)。 定理2.1 计算预测分布p(xk|y1: k－1)及滤波分布p(xk|y1: k)的
递归方程由下述贝叶斯滤波方程给出。
2.2 卡尔曼滤波算法
2.2.1 状态空间模型 对于离散状态空间模型或者贝叶斯非线性滤波模型，一
般都包含有下列条件概率分布函数
xk p(xk | xk1), yk p( yk | xk )
(2-1)
该模型假定符合马尔科夫理论，因此具有以下两个性质：
(1) 状态向量的马尔可夫性质。状态值序列{xk: k=0，1， 2，…}构成了一个马尔科夫序列，如果状态值是离散的，那么
p( xk | xk1) N( xk | Fk1 xk1,Qk1)
(2-13)
第2章 卡尔曼滤波和非线性系统滤波方法
p( yk | xk ) N( yk | Hk xk , Rk )
(2-14)
线性滤波模型(2-11)、(2-12)的最优滤波方程可以通过下
列近似方法获取，其结果都符合高斯分布，即
斯规则求取
p( xk
| y1:k )
1 Zk
p( yk
| xk ) p( xk
| y1:k1)
(2-6)
第2章 卡尔曼滤波和非线性系统滤波方法
其中归一化常数Zk为
Zk p( yk | xk ) p(xk | y1:k1)dxk
(2-7)
证明：在给定y1: k－1的情况下，xk和xk－1的联合分布值计算如 下