卡尔曼滤波（五）——协方差矩阵
模型公式：
Xk=AXk-1+BUk+Wk-1
Zk = HXk + Vk
卡尔曼增益：Kk = P-kHT / (HP-kHT + R)
观测值协方差阵： R = E(VVT) = E( [v1,v2]T [v1,v2] )
模型协方差阵：Q =E(WWT)= E( [w1,w2]T [w1,w2] )。
先验误差协方差矩阵：P-k = E(e-k e-k T)
先验误差：e-k = Xk - X-k（Xk为真值不可知）
先验值：X-k = AX^k-1 + BUk-1
后验值：X^k = X-k + Kk (Zk - HX-K)
研究P-k = E(e-k e-k T)
e-k = Xk - X-k
= AXk-1 + BUk+ Wk-1 - AX^k-1 - BUk-1
=A Xk-1 + Wk-1 - AX^k-1
= A(Xk-1 - X^k-1) + Wk-1
后验误差：ek-1 = Xk-1 - X^k-1
ek = Aek-1 + Wk-1
代入P-k = E[e-k e-k T]
P-k = E[ (Aek-1 + Wk-1) (Aek-1 + Wk-1) T]
= E[ (Aek-1 + Wk-1) ((Aek-1 )T+ (Wk-1) T) ]
= E[ (Aek-1 + Wk-1) (ek-1 TAT+ Wk-1 T) ]
= E[ (Aek-1ek-1 TAT + Aek-1Wk-1 T + Wk-1 ek-1 TAT + Wk-1Wk-1 T ]
P-k = E[ (Aek-1ek-1 TAT] + E[Aek-1Wk-1 T] + E[Wk-1 ek-1 TAT ] + E[ Wk-1Wk-1 T ]
第二项：E[Aek-1Wk-1 T] , 因为ek-1 、Wk-1 相互独立，所以E[Aek-1Wk-1 T] = A E[ ek-1 ]E[ Wk-1 T] = A* 0 *0 =0
（Wk-1 是算第k次的Xk，ek-1 = Xk-1 - X^k-1 是算第k-1次的）
第三项：同理 E[Wk-1 ek-1 TAT ] = 0
P-k = E[ (Aek-1ek-1 TAT] + E[ Wk-1Wk-1 T ]
= AE[ ek-1ek-1 T]AT + Qk-1
又【后验协方差阵】：Pk-1 = E[ ek-1ek-1 T]
P-k = A Pk-1 AT + Qk-1
卡尔曼滤波分为两个过程：
1. 预测；
2. 校正；
预测：
先验值：X-k = AX^k-1 + BUk-1
先验协方差：P-k = A Pk-1 AT + Qk-1
校正：
卡尔曼增益：Kk = P-kHT / (HP-kHT + R)
后验值：X^k = X-k + Kk (Zk - HX-K)
后验协方差：Pk = (I - KkH)P-k
后验协方差：Pk = E(ekekT) = E[(Xk - X^k )(Xk - X^k )T]
最后推得 ：
Pk = P-k - KkHP-k - P-k HTKkT + KkHP-k HTKkT + KkRKkT
= P-k - KkHP-k - P-k HTKkT + Kk(HP-k HT + R)KkT
将 Kk = P-kHT / (HP-kHT + R) 代入，得到：
Pk = P-k - KkHP-k - P-k HTKkT + P-kHT / (HP-kHT + R) * (HP-k HT + R)KkT
Pk = P-k - KkHP-k - P-k HTKkT + P-kHTKkT
Pk = P-k - KkHP-k = (I - KkH)P-k
Pk = (I - KkH)P-k
遗留的问题：
Pk-1 当k等于1时，就是P0 ，需要一个初始值，应该如何赋初始值？？
