Kalman Filter
X(k|k-1)=A X(k-1|k-1)+B U(k)
X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。
P(k|k-1)=A P(k-1|k-1) A’+Q
P(k|k-1)是X(k|k-1)对应的covariance,P(k-1|k-1)是X(k-1|k-1)对应的covariance,A’表示A的转置矩阵,Q是系统过程的covariance。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
Kg为卡尔曼增益(Kalman Gain):
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
P(k|k)=(I-Kg(k) H)P(k|k-1)
其中I 为1的矩阵,对于单模型单测量,I=1。当系统进入k+1状态时,P(k|k)就是式子(2)的P(k-1|k-1)。这样,算法就可以自回归的运算下去。
http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf
Linear Filtering and Prediction
最新推荐文章于 2022-09-02 21:19:15 发布