kalman滤波

卡尔曼滤波在工程应用中十分的广泛，最近一直在学习机器人的运动估计，其中卡尔曼理论十分重要，因此记下相关的学习心得。
卡尔曼滤波就是根据上一时刻的信息估计当前时刻的信息。解决了对之前所有时刻信息都需要保留的问题。仅仅考虑上一时刻的信息。一般有一个估计值，一个测量值，或者（两个传感器分别对一个问题 的测量值），你对估计值也不全相信，对测量值也不全相信，这个时候你就希望有没有个算法能够根据我的估计值和测量值，获得一个较为准确的结果。取权值。小伙子还是太嫩了。这个时候卡尔曼就出来了，说我可以啊。（记住这个系统最好是线性的，对阶跃变化的系统不好）。
先写公式吧，然后根据公式讲解。
**系统预测（1）**
    X(K|K-1) = AX(K-1|K-1) + B*U(K) 
    A,B为系统参数，A为状态转移矩阵，U(K)为现在状态的控制量，如果没有可以为0,W(k)为高斯白噪声，它的协方差为Q

**系统预测（2）**
    P(K|K-1) = AP(K|K-1)A' + Q
    Q为协方差矩阵

 最优估计
    X(K|K) = X(K|K-1) + Kg(Z(K) - H*X(K|K-1))
    H为在X到Z的一个映射矩阵

卡尔曼增益Kg

Kg（K) = P(K|K-1)H’/[HP(K|K-1)H’ - R]
R为观测噪声的协方差

协方差更新
P(K|k) = (1 - Kg(K)H)P(K|K-1)