(8.3.2)--新息法推导卡尔曼滤波算法.pdf

2.2.算法推导算法推导卡尔曼滤波的基本问题是：卡尔曼滤波的基本问题是：在假定信号模型和观测模型的条件下，根据给定的观测在假定信号模型和观测模型的条件下，根据给定的观测:建立建立的线性最小均方估计的递推算法，的线性最小均方估计的递推算法，的的估计也可以表示估计也可以表示为：为：00,1,.,kkkzzz kx0/,kj kk kk jjxHz kx根据观测与新息的对等关系，利用观测进行估计和利用新息进行估计是根据观测与新息的对等关系，利用观测进行估计和利用新息进行估计是完全等价的，所以完全等价的，所以的估计也可以表示为的估计也可以表示为 kx0/,kj kk kk jjxB由正交原理：由正交原理：0 ,kTj kEkk jjixB00,.,ikk00(),(),kkTTjij kj kEkik j Ejik jjk iixBBPBP1,()Tk iEkiiBxP01/()kTj kk kEkjjjxxP001111/()()()kTj kkTTj kk kEkjjjEkjjjEkkkkxxPxPxP/1k k x kK/1 k kk kkkxxK011/1()kTj kk kEkjjjxxP1()TkEkkkKxP增益增益Kk的推导：的推导：/1 /1/1k kkk kk kzHxw kkkkzHxw由于由于所以所以1111/1(,)()kkkk kCovkVarwwzzz0而而 /1 /1 /1 kkk kkkkkk kkk kkzzHxwHxHxw/1 /1k kkk kzHx所以所以 /1 kkk kkHxw /1 Txkkk kkkPHPHR()(/1/1)(/1)/1(/1)/1 TTTTTxEkkEk kk kkEk kkEk kkk kkk kkxxxxxHxwPH另外，另外，11()/1(/1 )TTTxxkEkkkk kkkk kkkKxvPPHHPHR所以所以的推导：的推导：/1k k x011/1()kTj kk kEkjjjxxP0001111/(1)(1,)(1,)1,/kTj kkTj kkTj kkkEkjjjEkkkkkjjjEkkkjjjkkk kxxPxnPxPx/1 /1(/1)k kk kkkk kkkkk kxxKxKzHx的推导：的推导：/k kx/()/1 (),1 1/1 k kkkk kkkkkk kkkkkxIKHxKzIKHxKz经整理后得到滤波的递推算法：经整理后得到滤波的递推算法：预测的递推算法：预测的递推算法：1/1,/kkkkk kxx/1(/1)k kkkkk kxKzHx1,()/11,kkkkk kkkkkIKHxKz预测误差方差阵和滤波误差矩阵的计算与预测误差方差阵和滤波误差矩阵的计算与正交投影法中的推导相同，在此不再重复。正交投影法中的推导相同，在此不再重复。3.3.算法总结算法总结/(),1 1/1 k kkkk kkkkkxIKHxKz1 /1(/1 )TTxxkk kkkk kkkKPHHPHR/1,11/1,11 11xTTxk kk kkkk kkkkPPQ/()/1xxk kkkk kPIKHP预测误差方差阵：预测误差方差阵：卡尔曼增益：卡尔曼增益：状态更新：状态更新：滤波方差阵更新滤波方差阵更新：小结：小结：1.1.新息的定义与性质新息的定义与性质 /1kkk kzz定义：定义：（2 2）()0TEijji新息是零均值白噪声新息是零均值白噪声，即，即（1）k时刻的新息与时刻的新息与k时刻以前的观测时正交的时刻以前的观测时正交的00(),1,.,1TEkjjkkkz0性质：性质：00,1,.,kkkzzz00,1,.,kkk（3 3）小结：小结：2.2.算法推导算法推导001111/()()()kTj kkTTj kk kEkjjjEkjjjEkkkkxxPxPxP/1k k x kK推导中的几个关键点：推导中的几个关键点：/1 kkk kkHxw/1k k w0()0TEkjjkn(/1)0TEk kkxw小结：小结：3.3.算法总结算法总结/(),1 1/1 k kkkk kkkkkxIKHxKz1 /1(/1 )TTxxkk kkkk kkkKPHHPHR/1,11/1,11 11xTTxk kk kkkk kkkkPPQ/()/1xxk kkkk kPIKHP预测误差方差阵：预测误差方差阵：卡尔曼增益：卡尔曼增益：状态更新：状态更新：滤波方差阵更新滤波方差阵更新：1/1,()/11,kkkkkkk kkkkkxIKHxKz预测算法：预测算法：