【SLAM】卡尔曼滤波（Kalman Filter）

卡尔曼滤波（Kalman filter）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

---

卡尔曼滤波器的原理解释如下：
首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值：
Z(k)=H X(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（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。

首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
推导过程：
e(k|k-1)=X(k)-X(k|k-1)=A( x(k)-x(k-1|k-1) ) + W(k)
P(k|k-1)=E{ ( X(k) - X(k|k-1) ) ( X(k) - X(k|k-1) )’}
=E{ ( AX(k-1)+BU(k)+W(k)-AX(k-1|k-1)-BU(k) ) ( AX(k-1) + BU(k) + W(k) - AX(k-1|k-1) - BU(k) )’}
=E{ ( A( X(k-1) - X(k-1|k-1) ) + W(k) ) ( A( X(k-1) - X(k-1|k-1) )+W(k) )’}
=E{ A( X(k-1) - X(k-1|k-1) ) ( X(k-1)-X(k-1|k-1) )’A’+ W(k)W(k)’}
=A E{ ( X(k-1) - X(k-1|k-1) ) ( X(k-1) - X(k-1|k-1) )’} A’+ E{ W(k)W(k)’}
=A P(k-1|k-1) A’+ Q