kalman滤波计算过程

对于方程：

dot X=A*X(k-1)+B*u(k)

 Y = C*X

卡尔曼滤波器的递归过程：

1)   估计时刻k 的状态：

          

X(k) = A*X(k-1) + B*u(k)

这里,   u(k) 是系统输入

2)   计算误差相关矩阵P, 度量估计值的精确程度：

 P(k) = A*P(k-1)*A’+ Q

 这里,   Q = E{ Wj^2 } 是系统噪声的协方差阵，即系统框图中的Wj的协方差阵, Q 应该是不断变化的，为了简

化，当作一个常数矩阵。

3)计算卡尔曼增益, 以下略去 (k),  即 P = P(k), X = X(k)：

K = P *C’ * (C * P * C’ + R) ^-1 

这里 R = E{ Vj^2 }, 是测量噪声的协方差(阵),  即系统框图中的 Vj 的协方差, 为了简化，也当作一个常数矩阵。由于

我们的系统一般是单输入单输出，所以 R是一个 1x1的矩阵，即一个常数，上面的公式可以简化为:

             K = P *C’ / (C * P * C’ + R) 

4)状态变量反馈的误差量：

e = Z(k) – C*X(k)

这里的 Z(k) 是带噪声的测量量

5)更新误差相关矩阵P

P = P – K * C * P

6)   更新状态变量：

X =X + K*e = X + K* (Z(k) – C*X(k))

7)最后的输出：

 

Y = C*X