卡尔曼滤波方程的推导

《卡尔曼滤波方程的推导》由会员分享，可在线阅读，更多相关《卡尔曼滤波方程的推导（6页珍藏版）》请在金锄头文库上搜索。

1、1 1、 随机线性离散系统的状态方程和观测方程为：随机线性离散系统的状态方程和观测方程为： ( )( ,1)(1)( ,1) (1)( ,1)(1) ( )( )( )( ) X kk kX kB k kU kk kW k Z kH k X kV k 式(1) 式中状态向量( ) n X kR，观测矢量( ) m Z kR，随机过程噪声( ) r W kR，随 机观测噪声( ) m V kR，状态转移矩阵( ,1) n n k kR ，过程噪声输入矩阵 ( ,1) n r k kR ，观测矩阵( ) m n H kR ，( ,1) n q B k kR 是作用在控制向量上的 控制输入模型(输入

2、输出矩阵)，(1) q U kR是控制向量。 2 2、 卡尔曼滤波器使用假设卡尔曼滤波器使用假设 假设 1：( )W k和( )V k是零均值或非零均值的白噪声或高斯白噪声，即： ( )0( ) ( )( )( ) ( )0 ( ) ( )( )( ) W T kj V T kj E W kE W k E W k WjQ k E V kE V k E V k VjR k 或 或 式（2） 其中 1 () 0 () kj kj kj ，这里( )0Q k 是激励噪声的一个n n维的协 方差矩阵，( )0R k 是观测噪声的一个m m维的协方差矩阵。 假设 2：( )W k和( )V k不相关或相

3、关，即： ( )( )0( )( )S( ) TT kj E W k VjE W k Vjk或 式（3） 假设 3：(0)X是某种已知分布的随机向量， 其均值和协方差已知， 且与( )W k 和( )V k均不相关，即： (0)(0)(0) (0), (0)(0) (0) (0)0, (0)0 T XXX TT kk E XE XXP E XWE XV 式（4） 卡尔曼滤波器适用于线性高斯系统，即状态方程和测量方程是线性的，加性 噪声是的高斯的。 3 3、 KalmanKalman 滤波估计的结构框图滤波估计的结构框图 从 Kalman 滤波方程可以看出，递推计算过程分解到每一步，实际上是一种

4、 “一步预测-修正”结构，如图所示。新息是新的观测值与单步预测值的差值。 如果新息为零，那么就不需要进行修正。 4 4、 公式推导公式推导 卡尔曼滤波器分为两个计算回路：滤波计算回路和增益计算回路。这两个回 路之间的联系是增益计算回路每个循环都求出一个新的卡尔曼滤波器增益( )K k， 然后滤波计算回路通过这个增益更新滤波估值。 首先我们定义三个量： ( | )mn-1mn ( | ) kk ( | ) X n m X k k P k k ： 代表已知 到包括 时刻的观测在 时刻的估计值 ： 已知 时刻以前时刻观测值， 时刻的状态估计值 ： 误差协方差矩阵，度量状态估计的精确程度 定义：定义：

5、 ( |1)( ,1)(1|1)( ,1) (1)X k kk kX kkB k kU k 式（5） 则误差协方差估计预测：则误差协方差估计预测： ( |1)cov( )( |1) cov ( ,1)(1)( ,1) (1)( ,1)(1) ( ,1)(1|1)( ,1) (1) cov( ,1)(1)(1|1)( ,1)(1) ( ,1)cov P k kX kX k k k kX kB k kU kk kW k k kX kkB k kU k k kX kX kkk kW k k kX 将式（1）和式（5）代入得： 根据协方差矩阵的性质得： (1)(1|1)( ,1)( ,1)cov(1)

6、( ,1) ( ,1) (1|1)( ,1)( ,1) (,)61)(1 TT TT kX kkk kk kW kk k k kP kkk kk kQ kk k 由之前的定得： 式（ 义 ） 定义新息或测量余差为：定义新息或测量余差为： ( )( )( )( |1)y kZ kH k X k k 式（7） 则新息的协方差为：则新息的协方差为： ( )cov( ( ) cov ( )( )( |1) cov( )( )( )( )( |1) ( ) ( |18)( )( ) T S ky k Z kH k X k k H k X kV kH k X k k H k P k kHkR k 将式（7

7、）代入得： 代入式（1）得 式（ ： ） 定义状态估计更新为：定义状态估计更新为： ( | )( |1)( ) ( )X k kX k kK k y k 式（9） 后验误差协方差矩阵后验误差协方差矩阵更新：更新： ( | )cov( )( | ) cov( ) ( |1)( ) ( ) ( )( )( |1) =cov( ) ( |1)( )( ( )( )( |1) =cov( ) ( |1)( )( )( )( )( )( |1) c P k kX kX k k X kX k kK k y k Z kH k X k k X kX k kK k Z kH k X k k X kX k kK

8、k H k X kV kH k X k k 将式（9）代入得： 将式（7）代入得： 将式（1）代入得： ov( )( |1)( )( )( )( )( |1)( ) ( ) cov( )( |1)( )( )( )( |1)( ) ( ) cov ( )( )( )( |1)( ) ( ) ( )( )cov( )( |1)( )( )( )cov T X kX k kK k H k X kH k X k kK k V k X kX k kK k H kX kX k kK k V k IK k H kX kX k kK k V k IK k H kX kX k kIK k H kK k ( )

9、( ) ( )( ) ( |1)( )( )1)( )0( )( ) T TT V k Kk IK k H k P k kIK k H kK k R k Kk式（ ） KalmanKalman 增益推导：增益推导： Kalman 滤波器是一个最小均方误差估计器，先验状态误差估计可表示为 ( )( | )X kX k k，我们最小化这个矢量幅度平方的期望值 2 ( )( | )EX kX k k ， 这等价于最小化后验估计协方差矩阵( | )P k k的迹，通过展开合并( | )P k k公式， 可得： ( | )( |1)( )( ) ( |1)( |1)( )( ) ( )( ) ( |1)

10、( )( )( ) ( |1)( )( ) ( |1)( |1)( )( ) ( ) ( )( )11 TT TT TT T P k kP k kK k H k P k kP k kHk Kk K k H k P k kHkR k Kk P k kK k H k P k kP k kHk Kk K k S k Kk 代入式（8） 式（ 得： ） 我们通过式（11）求导的方式求得( | )P k k的极值点： 1 ( ( | ) 2( ) ( |1)2( ) ( )0 ( ) ( )( |1)( )( ) T T tr P k k H k P k kK k S k K k K kP k kHk

11、Sk 注：这里协方差矩阵式对称矩阵 式（12） 后验误差协方差矩阵简化后验误差协方差矩阵简化： ( ) ( )( )( |1)( )( ) ( | )( |1)( )( ) ( |1)( |1)( )( ) ( |1)( )( ) ( |1)( )( ) ( |1) ( )( ) ( |1) T TTT TT TT K k S k KkP k kHk Kk P k kP k kK k H k P k kP k kHk Kk P k kHk Kk P k kK k H k P k k IK k H k P k k 将式（12）两边同乘S(k)K (k)得： 式（13） 将式（13）代入式（11）得： 式（14） 这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公 式仅在最优卡尔曼增益时它才成立。 如果算术精度总是很低而导致数值稳定性出 现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用 上面导出的后验误差协方差公式。 式（5） 、 （6） 、 （9） 、 （10） 、 （12）这几个方程便构成了卡尔曼滤波器的 5 个 基本方程，其中式（10）可以简化为式（14） 。 5 5、 卡尔曼滤波的计算框图卡尔曼滤波的计算框图