Kalman滤波的多种实现方式浅析

一、卡尔曼滤波算法原理：

卡尔曼算法是个递推迭代的过程，若一个模型可以描述为线性过程，则可得到它的状态方程和观测方程。

建立状态方程：X(k)=F*X(k-1)+B*U(K-1)+W(k-1)

X(k)表示系统在k时刻的n维状态，U(K)是系统的控制量如若没有就设置为0，W(k)表示过程白噪声，符合正太分布，其方差为Q。

F是状态转移矩阵。

建立系统的观测方程：Z(K)=H*X(k)+V(k)

Z(K)表示系统在K时刻的系统的m维观测值，H是观测矩阵，V(k)表示观测白噪声，符合正太分布，其方差为R。

卡尔曼预测对象状态主要分为5个步骤：
1、根据上一个状态预测下一个状态：X(k+1|k)=F*X(k|k);

此时得到的状态仅仅是根据上一次的状态预测得到的，并没有利用此次的观测值对其进行校正，因此是不准确的。

2、协方差矩阵的预测：P(k+1|k)=F*P(k|k)*F'+Q; 

3、校正状态：X(k+1|k+1)=X(k+1|k)+Kg(k+1)*E(k+1);

利用观测值对其进行校正，Kg表示卡尔曼增益，E表示误差信息