卡尔曼滤波方程的推导

1、随机线性离散系统的状态方程和观测方程为：()(,1)(1)(,1)(1)(,1)(1)()()()()XkkkXkBkkUkkkWkZkHkXkVk式(1)式中状态向量()nXkR，观测矢量()mZkR，随机过程噪声()rWkR，随机观测噪声()mVkR，状态转移矩阵(,1)nnkkR，过程噪声输入矩阵(,1)nrkkR，观测矩阵()mnHkR，(,1)nqBkkR是作用在控制向量上的控制输入模型(输入输出矩阵)，(1)qUkR是控制向量。2、卡尔曼滤波器使用假设假设1：()Wk和()Vk是零均值或非零均值的白噪声或高斯白噪声，即：[()]0[()][()()]()[()]0[()][()()]()WTkjVTkjEWkEWkEWkWjQkEVkEVkEVkVjRk或或式（2）其中1()0()kjkjkj，这里()0Qk是激励噪声的一个nn维的协方差矩阵，()0Rk是观测噪声的一个mm维的协方差矩阵。假设2：()Wk和()Vk不相关或相关，即：[()()]0[()()]S()TTkjEWkVjEWkVjk或式（3）假设3：(0)X是某种已知分布的随机向量，其均值和协方差已知，且与()Wk和()Vk均不相关，即：(0)(0)(0)[(0)],[((0))((0))](0)[(0)]0,[(0)]0TXXXTTkkEXEXXPEXWEXV式（4）卡尔曼滤波器适用于线性高斯系统，即状态方程和测量方程是线性的，加性噪声是的高斯的。3、Kalman滤波估计的结构框图从Kalman滤波方程可以看出，递推计算过程分解到每一步，实际上是一种“一步预测-修正”结构，如图所示。新息是新的观测值与单步预测值的差值。如果新息为零，那么就不需要进行修正。4、公式推导卡尔曼滤波器分为两个计算回路：滤波计算回路和增益计算回路。这两个回路之间的联系是增益计算回路每个循环都求出一个新的卡尔曼滤波器增益()Kk，然后滤波计算回路通过这个增益更新滤波估值。首先我们定义三个量：ˆ(|)mn-1mnˆ(|)kk(|)XnmXkkPkk：代表已知到包括时刻的观测在时刻的估计值：已知时刻以前时刻观测值，时刻的状态估计值：误差协方差矩阵，度量状态估计的精确程度定义：ˆˆ(|1)(,1)(1|1)(,1)(1)XkkkkXkkBkkUk式（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[PkkXkXkkkkXkBkkUkkkWkkkXkkBkkUkkkXkXkkkkWkkkX将式（1）和式（5）代入得：根据协方差矩阵的性质得：ˆ(1)(1|1)](,1)(,1)cov[(1)](,1)(,1)(1|1)(,1)(,1)(,)61)(1TTTTkXkkkkkkWkkkkkPkkkkkkQkkk由之前的定得：式（义）定义新息或测量余差为：ˆ()()()(|1)ykZkHkXkk式（7）则新息的协方差为：()cov(())ˆcov[()()(|1)]ˆcov[()()()()(|1)]()(|18)()()TSkykZkHkXkkHkXkVkHkXkkHkPkkHkRk将式（7）代入得：代入式（1）得式（：）定义状态估计更新为：ˆˆ(|)(|1)()()XkkXkkKkyk式（9）后验误差协方差矩阵更新：ˆ(|)cov(()(|))ˆcov()[(|1)()()]ˆ()()(|1)ˆˆ=cov()[(|1)()(()()(|1))]ˆˆ=cov()[(|1)()(()()()()(|1))]cPkkXkXkkXkXkkKkykZkHkXkkXkXkkKkZkHkXkkXkXkkKkHkXkVkHkXkk将式（9）代入得：将式（7）代入得：将式（1）代入得：ˆˆov()(|1)()[()()()(|1)]()()ˆˆcov()(|1)()()[()(|1)]()()ˆcov[()()][()(|1)]()()ˆ(()())cov[()(|1)](()())()covTXkXkkKkHkXkHkXkkKkVkXkXkkKkHkXkXkkKkVkIKkHkXkXkkKkVkIKkHkXkXkkIKkHkKk[()]()(()())(|1)(()()1)()0()()TTTVkKkIKkHkPkkIKkHkKkRkKk式（）Kalman增益推导：Kalman滤波器是一个最小均方误差估计器，先验状态误差估计可表示为ˆ()(|)XkXkk，我们最小化这个矢量幅度平方的期望值2ˆ()(|)EXkXkk，这等价于最小化后验估计协方差矩阵(|)Pkk的迹，通过展开合并(|)Pkk公式，可得：(|)(|1)()()(|1)(|1)()()()[()(|1)()()]()(|1)()()(|1)(|1)()()()()()11TTTTTTTPkkPkkKkHkPkkPkkHkKkKkHkPkkHkRkKkPkkKkHkPkkPkkHkKkKkSkKk代入式（8）式（得：）我们通过式（11）求导的方式求得(|)Pkk的极值点：1((|))2[()(|1)]2()()0(())()(|1)()()TTtrPkkHkPkkKkSkKkKkPkkHkSk注：这里协方差矩阵式对称矩阵式（12）后验误差协方差矩阵简化：()()()(|1)()()(|)(|1)()()(|1)(|1)()()(|1)()()(|1)()()(|1)[()()](|1)TTTTTTTTKkSkKkPkkHkKkPkkPkkKkHkPkkPkkHkKkPkkHkKkPkkKkHkPkkIKkHkPkk将式（12）两边同乘S(k)K(k)得：式（13）将式（13）代入式（11）得：式（14）这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公式仅在最优卡尔曼增益时它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用上面导出的后验误差协方差公式。式（5）、（6）、（9）、（10）、（12）这几个方程便构成了卡尔曼滤波器的5个基本方程，其中式（10）可以简化为式（14）。5、卡尔曼滤波的计算框图