卡尔曼滤波学习

    首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述： X(k) = AX(k - 1) + BU(k) + W(k)再加上系统的测量值： Z(k) = HX(k) + V(k)

    上两式子中， X(k) 是 k 时刻的系统状态， U(k) 是 k  时刻对系统的控制量。 A  和  B 是系统参数，对于多模型系统，他们为矩阵。 Z(k) 是  k 时刻的测量值， H  是测量系统的参数，对于多测量系统，H   为矩阵。  W(k) 和  V(k) 分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的 covariance 分别是  Q ， R （这里我们假设他们不随系统状态变化而变化）。

(1)利用系统的过程模型，预测下一状态的系统：X(k|k - 1) = AX(k - 1|k - 1) + BU(k)其中：

X(k|k - 1) 是利用上一状态预测的结果

X(k - 1|k - 1)是上一状态最优的估计结果

U(k)是现在状态的控制量，如果没有控制量，可以为0

(2)更行对应于X(k|k - 1)的covariance：P表示covarianceP(k|k - 1) = AP(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， 系统过程W(k)的噪声协方差

(3)结合预测值和测量值，可以得到现在状态的最优估计X(k|k) = X(k|k - 1) + Kg(k)(Z(k) - HX(k|k - 1))                                                                               卡尔曼增益（Kalman Gain）Kg(k) = P(k|k - 1)H'/(HP(k|k - 1)H' + R)

(4)更新X(k|k)的covariance

P(k|k) = (I - Kg(k)H)P(k|k - 1)其中I为1的矩阵，对于单模型单测量，I=1.

posted on 2014-10-15 11:14  夏鸥易  阅读(172)  评论(0)  编辑  收藏  举报

导航