卡尔曼滤波原理(一)

卡尔曼滤波是一种递归线性最小变量无偏估计原理：

1>无偏估计 期望值等于真值 E(g')=g   其中g'是g的无偏估计

2> 最小变量估计  D(g')=E((g'-g)^2)     if   D(g')  <= D(g~)   任意的g~   也就是方差任意小

协方差矩阵 表示随机变量间的关联  

Xk=φk-1Xk-1+uk-1+τk-1Wk-1                             (1)   Wk-1 和Vk  是过程噪声和观测噪声

Zk=HkXk+Vk

估计器：X'k|k=K'k*X'k-1|k-1+Kk*Zk+K''k*uk-1  (2)  计算 Kk'  Kk  Kk''的值使得X'k|k是Xk的最优线性估计

X''k|k=Xk-X'k|k   最优估计值与真值得偏差         (3)

联立(1) (2) (3)则有下面表达式：

X'k,Zk,X'k|k——> X''k|k=φk-1Xk-1+uk-1+τk-1Wk-1-X'k|k-K'k*X'k-1|k-1+Kk*Zk+K''k*uk-1

                   ——> X''k|k=φk-1Xk-1+uk-1+τk-1Wk-1-X'k|k-K'k*X'k-1|k-1+Kk*(HkXk+Vk)+K''k*uk-1

                   ——>X''k|k=