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

9

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

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


计算方法——递推形式
7

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

X k 1

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


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

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

X AZ b


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

可证明，这种估计只需要被估计值X和量测值Z的一、 二阶统计特性，所以，它比最小方差估计较为实用。
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阶）
一步预测均方差方程 估计均方差方程


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
最小方差 估计 线性最小 方差估计 递推线性最小 方差估计

3
1、最小方差估计

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

根据其他方法 系统的n维随 利用Z计算得到的 机向量 估计均方差阵 估计的误差 用Z计算得到 最小方差估计的误差小于等于其他估计的均方误差! X的最小方差估值 的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
13

各滤波方程的物理意义：
（1）状态一步预测方程
ˆ X k 1
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、离散卡尔曼滤波方程
ˆ 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是对称的
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


k ,k 1
P k / k 1 k ,k 1Pk 1kT,k 1 k 1Qk 1kT1
卡尔曼滤波与组合导航
Theory of Kalman filter and Integrated Navigation
第三章 卡尔曼滤波原理

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


3.5 卡尔曼滤波在组合导航中的应用方式
和 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



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
4

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



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





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 )

一步预测方程改为：
ˆ ˆ 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 连续系统的卡尔曼滤波方程

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
6
3、递推线性最小方差估计——卡尔曼滤波

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

E{[ X X ( Z )][ X X ( Z )] } AZ {[ X ~ ( Z )][ X ( Z )] } X Eb
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

转移过来的，并且再加上系统噪声方差的影响。
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




~ 上式就是通过 计算新息，把 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
（3）滤波增益方程