Kalman 滤波算法导论

        kalman 滤波在Gps惯性导航中应用达到极致，kalman滤波基础线性代数和隐马尔代夫链。

        基本模型，假设系统的状态方程为：X(k+1) = AX(k)+Bu(k)+w(k)

                            其中X 为系统的状态变量

                                    A为状态转移矩阵（矩阵设计和定义一般由物理意义产生）

                                    B为作用在控制器向量u(k)上的输入-控制模型

                                    w为过程噪声，并假定其符合均值为0，协方差矩阵为Qk的多元正态分布

                                   wk~N(0,Qk)

                          观测矩阵为： Z(k)=H(k)X(k) + V(k)

                         其中H(k)是观测矩阵，将状态空间映射成观测矩阵， V(k)为观测噪声矩阵，均值为0，协方差矩阵服从正态分布

                        V(k) ~N(0,Rk)

         

      kalman 滤波过程分为两个阶段：预测和更新

      预测阶段：

            X(k|k-1) = AX(k-1|k-1) + Bu(k)

            P(k|k-1) = AP(k-1|k-1)AT + Q(k)，其中AT为状态转移A的转置,

            Q(k)为过程协方差矩阵

     更新：

            Y(k) = Z(k) - H(k)X(k|k-1) ,(Y(K)为测量余量)

            S(k) = H(k)P(k|k-1)HT(k) +R (k) ,(其中HT为观测矩阵H的装置)

            K(k) = P(k|k-1)HT(k)S(k)(-1)  (S(k)(-1)表示S(k)的逆矩阵) K(k)是最优卡尔曼增益矩阵

      更新状态变量和P：

            X(k|k) = X(k|k-1) + K(k)Y(k)

             P(k|k)=(I-K(k)H(k))P(k|k-1) 

    

    总结：kalman滤波工程更加通俗的说为：预测阶段：拿上一时刻的状态根据状态方程预测当前时刻的状态，并且预测估计协方差矩阵，第一阶段是根据系自身的状态方程来计算下一时刻的状态，更新阶段：这一阶段的计算是基于误差均方差最小，计算得出的。在第一阶段我们根据状态方程预估了当前时刻的状态，第二阶段会加入观测方程，第一阶段是根据经验（或者物理模型）推导出来的值（先验值），而第二阶段是我们实际通过传感器测量出来的值（后验值），此时，我们就会问到底相信哪一个的问题，是相信我们的经验呢，还是相信我实际看到的呢，核心就在第二阶段的误差方差最小化这个得来的。古人有云，眼见为实，耳听为虚。应用kalman滤波算法后，应该改为唯眼见和经验综合起来才为实。因为眼睛看到的有时候有误差，而我们的经验有时候会因为环境的干扰不太准确，这时候kalman滤波算法可以得出一个比较真实的值，这个值是你综合了经验和观测使得与真实值得误差均方差最小。扯远了，第二阶段分别为计算观测余量，迭代测量余量协方差，计算最优kalman滤波增量，最后就是根据余量和最优卡尔曼增量计算当前时刻状态的估计，当然还有计算误差协方差矩阵了。（实际在导航中的应用将在后续中更新）