卡尔曼滤波学习

首页 > 技术文章 > 卡尔曼滤波学习

lovelifelovestudy 2014-10-15 11:14 原文

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（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表示covariance $P(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.

1
2

推荐阅读