对Kalman Filter的理解

1. 卡尔曼滤波理论回顾

      对于一个动态系统，我们首先定义一组状态空间方程

     状态方程：     

     测量方程：      

        x_k是状态向量，z_k是测量向量，A_k是状态转移矩阵，u_k是控制向量，B_k是控制矩阵，w_k是系统误差（噪声），H_k是测量矩阵，v_k是测量误差（噪声）。w_k和v_k都是高斯噪声，即

                             

    整个卡尔曼滤波的过程就是个递推计算的过程，不断的“预测——更新——预测——更新……”

预测

     预测状态值：