卡尔曼滤波与组合导航-第三讲

状态一步预测方程
X k / k 1 k ,k 1 X k 1
X k X k / k 1 K k ( Z k H k X k / k 1 )
T T K k P k / k 1 H k ( H k Pk / k 1 H k Rk ) 1



状态估值计算方程 滤波增益方程
一般情况下经常采用离散卡尔曼滤波方程 但也可以直接从连续系统的滤波方程求出系统状 态的估值，设连续系统的状态方程和量测方程分 别为 ：
X (t ) F (t ) X (t ) G(t )W (t ) Z (t ) H (t ) X (t ) V (t )

X (t)和Z (t)分别为t时刻的系统状态矢量和量测矢量； F(t)，G(t)和H(t)，分别为系统矩阵，系统噪声矩阵和 量测矩阵； W(t)和V(t) 和分别为系统噪声矢量和量测噪声矢量， 卡尔曼滤波要求它们都是零均值的白噪声过程。
和 RZ k 分别就是新息中的两部分内容 H k Pk / k 1 Rk X k / k 1 , X k / k 1T E 小，Kk就大 k
T Pk / k 1一步预测均方差阵，即：k就小 也称为一步预测误差方差阵。上式中的 H k Pk / k 1 H k 如果Rk大，K
16
（4）一步预测均方误差方程
T T P k / k 1 k ,k 1 Pk 1k ,k 1 k 1Qk 1k 1

从下式可以看出，求Kk必须先求出Pk/k-1
~ ˆ T ( ，为 T ˆ k 1 式中 XX1 的均方误差阵，即： k 的估计误差，可以看 K kˆ PXkk11 H kX H1k Pk / k 1 H kX R ) 1 k k/ k k 1 出一步预测均方误差阵Pk/k-1是从估计均方误差阵Pk-1 ~ ~ T
9

卡尔曼滤波要求{Wk}和{Vk}是互不相关的零均值的
白噪声序列，有：
E Wk W jT Qk kj
k T j k kj

E V R V
Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，在 卡尔曼滤波中要求它们分别是已知值的非负定阵和正 定阵； δk j是Kronecker δ函数，即：



P k / k 1 k ,k 1Pk 1kT,k 1 k 1Qk 1kT1
T P k ( I K k H k ) Pk / k 1 ( I K k H k )T K k Rk K k
13

各滤波方程的物理意义：
（1）状态一步预测方程
ˆ X k 1
最小方差 估计 线性最小 方差估计 递推线性最小 方差估计

3
1、最小方差估计

最小方差估计的估计准则是估计的均方误差最小，即：
Z是m维随机 量测向量
E{[ X X ( Z )][ X X ( Z )]T } E{[ X ( Z )][ X ( Z )]T }

根据其他方法 系统的n维随 利用Z计算得到的 机向量 估计均方差阵 估计的误差 用Z计算得到 最小方差估计的误差小于等于其他估计的均方误差! X的最小方差估值 的X的估值




~ 上式就是通过 计算新息，把 X k / k 1 估计出来，并 计算估值X K 加到 中，从而得到 估 左乘一个系数矩阵k的方程。它是在一步预测Xk/k-1 k X k / k 1 的基础上，根据量测值Zk计算出来的 ˆ 值 和， K称为滤波增益矩阵 Xk ~ ~ k 由两部分组成： X k / k 1 和 ZV的一步预测， 正是在 X k ， k / k 1 若把 H k X k / k 1看作是量测 k 一步预测误差 X k 所需信息，因此 ~ X k 则 (Z k H k X k / k 1 )就是量测的一步预测误差 X / k 1 基础上估计 k / k 1 X k X k / k 1 又称( Z k H k X k / k 1 ) 为新息 15
ˆ Xk
19
滤波计算回路
增益计算回路
Pk
（7）初值的确定

ˆ 在滤波开始时，必须有初始值 X 0 和 P0 才能进行

为了保证估值的无偏性，应选择：
ˆ X 0 EX 0 mx 0 ˆ ˆ P0 E ( X 0 X 0 )( X 0 X 0 )T E ( X 0 mx 0 )( X 0 mx 0 )T


因此，最小方差估计不但使估值 X (Z )的均方误差最小， 而且这种最小的均方误差就是估计的误差方差
5
2、线性最小方差估计

如果将估值 X 规定为量测矢量Z的线性函数，即

X AZ b


式中A和b分别是（n×m）阶和n维的矩阵和矢量。这 样的估计方法称为线性最小方差估计。

可证明，这种估计只需要被估计值X和量测值Z的一、 二阶统计特性，所以，它比最小方差估计较为实用。
计算量小，但在计算机有舍入误 差的条件下，不能始终保证算出 的Pk是对称的
18
（6）卡尔曼滤波的计算流程
Pk 1
ˆ X k 1
k 1Qk 1kT1
k / k 1
k k 1
X k / k 1 k ,k 1 X k 1

Leabharlann Baidu
k ,k 1
P k / k 1 k ,k 1Pk 1kT,k 1 k 1Qk 1kT1
8
3.2 卡尔曼滤波方程
1、离散系统的数学描述

设离散化后的系统状态方程和量测方程分别为：
X k k ,k 1 X k 1 k 1Wk 1 Z k H k X k Vk
Xk为k时刻的n维状态向量 k-1到k时刻的系统一步状态 ΓWk-1为k-1时刻的系统噪声 k-1为系统噪声矩阵 Hk为k时刻系统量测矩阵 Zk为k时刻的m维量测向量 （r维） Vk为k时刻m维量测噪声 （被估计量） （n×r阶） 转移矩阵（n×n阶） （m×n阶）

一步预测方程改为：
ˆ ˆ X k k 1 k ,k 1 X k 1 Bk 1U k 1

状态估计方程改为：

21
其他滤波方程不变
ˆ ˆ ˆ X k X k k 1 K k ( Z k Yk H k X k k 1 )
3.3 连续系统的卡尔曼滤波方程

0 (k j ) kj 1 (k j )

10

初始状态的 一、二阶统计特性为：
EX 0 mx 0 VarX 0 C x 0

Var{〃} 为对{〃}求方差的符号
卡尔曼滤波要求mx0和Cx0为已知量， 且要求X0与{Wk}和{Vk}都不相关


11
2、离散卡尔曼滤波方程
卡尔曼滤波与组合导航
Theory of Kalman filter and Integrated Navigation
第三章 卡尔曼滤波原理

3.1 卡尔曼滤波与最优估计 3.2 卡尔曼滤波方程 3.3 连续系统的卡尔曼滤波方程 3.4 连续—离散系统卡尔曼滤波方程


3.5 卡尔曼滤波在组合导航中的应用方式
E{[ X X ( Z )]} E X 0


计算方法——递推形式
7

在k时刻以前估值的基础上，根据k时刻的量测值Zk,
ˆ 递推得到k时刻的状态估计值 X (t ) ：

X k 1

Xk
Zk
根据k-1时刻以前 X（k）也可以说是综合利用k 主要适用于线性动态系统！ 一次仅处理一个量测量 所有的量测值得到时刻以前的所有量测值得到 的 计算量大大减小
P k / k 1
Rk Hk

K k P k / k 1 H kT ( H k Pk / k 1H kT Rk ) 1
Kk
Zk
X k X k / k 1 K k ( Z k H k X k / k 1 )
k k 1
Rk Hk
T P k ( I K k H k ) Pk / k 1 ( I K k H k )T K k Rk K k
12
2、离散卡尔曼滤波方程
X k / k 1 k ,k 1 X k 1

时间修正 方程 量测修正 方程
X k X k / k 1 K k ( Z k H k X k / k 1 )
T T K k P k / k 1 H k ( H k Pk / k 1 H k Rk ) 1
（3）滤波增益方程

Kk选取的标准就是卡尔曼滤波的估计准则，也就是 ˆ 使得 均方误差阵最小： X
k
T T K k P k / k 1 H k ( H k Pk / k 1 H k Rk ) 1

~ ˆ 也具有无偏性，即 X k / k 1 的均值为零，所以 由于 X k / k 1
3.6 非线性系统的卡尔曼滤波

2
3.7 卡尔曼滤波与系统可观测性分析
3.1 卡尔曼滤波与最优估计

卡尔曼滤波是一种最优估计技术 ！ 它能将仅与部分状态有关的测量值进行处理，得
出从某种统计意义上讲估计误差最小的更多的状
态的估计值。

估计误差最小的标准称为估计准则。 根据不同的估计准则和估计计算方法，有各种不 同的最优估计。 卡尔曼滤波是一种递推线性最小方差估计。



VarX 0 Cx 0

20
这样才能保证估计均方差阵Pk始终最小。

另外，如果系统和量测值中都有已知确定输入量 即系统的状态方程和量测方程为
X k k ,k 1 X k 1 k 1Wk 1 Bk 1U k 1 Z k H k X k Vk Yk

22

根据估计均方误差最小的估计准则，按上述系统和
量测值，可以推导出连续系统的滤波方程，即
K (t ) P(t ) H T (t ) R 1 (t ) ˆ ˆ ˆ X (t ) F (t ) X (t ) K (t )[ Z (t ) H (t ) X (t )] P(t ) P(t ) F T (t ) F (t ) P(t ) P(t ) H T (t ) R 1 (t ) H (t ) P(t ) G(t )Q(t )G T (t )
4

最小方差估计具有无偏性质，即它的估计误差（亦可 ~ 用 X 表示）的均值为零。即：
~ E{[ X X ( Z )]} E X 0



估计的均方误差就是估计误差的方差，即：
~~ T ~ ~ ~ ~ T E XX E [ X E X ][ X E X ]




一步预测均方差方程 估计均方差方程


P k / k 1 k ,k 1Pk 1kT,k 1 k 1Qk 1kT1
T P k ( I K k H k ) Pk / k 1 ( I K k H k )T K k Rk K k

或
P k ( I K k H k ) Pk / k 1
6
3、递推线性最小方差估计——卡尔曼滤波

卡尔曼滤波的准则与线性最小方差估计相同 估值同样是量测值的线性函数 只要包括初始值在内的滤波器初值选择正确，它的估 值也是无偏的 T T

E{[ X X ( Z )][ X X ( Z )] } AZ {[ X ~ ( Z )][ X ( Z )] } X Eb
ˆ X k / k 1
Xk-1的卡尔曼滤波估值 利用Xk-1计算得到的一步预测
也可以认为是利用k-1时刻和以前时 刻的量测值得到的Xk的一步预测
14
（2）状态估值计算方程
X k X k / k 1 K k ( Z k H k X k / k 1 ) ~ Z k H k X k / k 1 H k X k Vk H k X k / k 1 H k X k / k 1 Vk

转移过来的，并且再加上系统噪声方差的影响。
Pk 1 E X k 1 , X k 1


17
（5）估计均方误差方程
T P k ( I K k H k ) Pk / k 1 ( I K k H k )T K k Rk K k
或
P k ( I K k H k ) Pk / k 1