我所理解的卡尔曼滤波——公式推导与应用

1.什么是卡尔曼滤波

先举个例子说一下什么是卡尔曼滤波。

假如有一个小机器人可以在草地上自由的运动。为了让它实现导航，机器人需要知道自己所处的位置。那么这个小机器人如何才能知道自己在什么位置呢？有两种方式：

1. 根据起始位置及自身的运动进行运动学计算，从而得到自身的位置信息。
2. 根据自身携带的传感器测量自身的位置信息，如GPS。

那么，现在就存在一个问题。不论是根据运动学计算还是利用传感器信息，得到的位置信息都不可避免的存在误差。那机器人到底该相信哪个数据呢？有或者，如何根据这两种数据来确定自身的位置呢？最简单的方式就是取平均值，但是这种方式合理吗？凭什么每一项的权重都是0.5？显然这种方式不合适。

既然，取均值的方式不合理，那又该怎么做呢？卡尔曼滤波解决的就是这个问题！

卡尔曼滤波的本质就是根据”测量值”（如GPS数据） 和 “预测量”（如运动学计算结果） 及 “误差”，计算得到当前状态的最优量。

2.卡尔曼滤波的数学推导

2.1 状态方程和测量方程

首先假设我们知道一个线性系统的状态差分方程为$$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}\text{\hspace{1em}(1)}$$ 其中 $x$ 是系统的状态向量，大小为n*1列。$A$为转换矩阵，大小为 $n\ast n$。$u$为系统输入，大小为$k\ast 1$。$B$是将输入转换为状态的矩阵，大小为$n\ast k$。随机变量$w$为系统噪声。

这里说句题外话，什么是状态方程？说的通俗一点，就是预测方程。根据上一时刻的状态及控制量来预测当前时刻的状态，当然，这个过程中不可避免的存在误差。

系统的测量方程为$$z_k=H{x}_k+v_k\text{\hspace{1em}(2)}$$ $z$是测量值，大小为$m\ast 1$，$H$也是状态变量到测量的转换矩阵。大小为$m\ast n$。随机变量$v$是测量噪声。

那什么又是测量方程呢？就是根据此刻的状态量及噪声得到的测量值的过程。另一方面测量方程也表明，测量值是由我们的状态值确定的。

对于状态方程中的系统噪声$w$和测量噪声$v$，假设服从如下多元高斯分布，并且$w$,$v$是相互独立的。其中$Q$,$R$为噪声变量的协方差矩阵。

2.2 卡尔曼滤波过程

在此，我们设${\widehat{x_k}}^{\prime }$为预测值（根据上一时刻的状态及控制量由状态方程得到的当前时刻的状态）、$\widehat{x_k}$为估计值（即卡尔曼滤波的最终的结果）、$\widehat{z_k}$是测量值的预测（即，根据预测的状态值${\widehat{x_k}}^{\prime }$得到的测量值）。则三者的关系为$$\widehat{x_k}={\widehat{x_k}}^{\prime }+K(z_k-\widehat{z_k})={\widehat{x_k}}^{\prime }+K(z_k-H{\widehat{x_k}}^{\prime })\text{\hspace{1em}(3)}$$

其中，$(z_k-H{\widehat{x_k}}^{\prime })$称之为残差，也就是预测的和你实际测量值之间的差距。如果这项等于0，说明预测和测量出的完全吻合。

这个公式说明什么呢？他的意思就是最终的估计值等于预测值加上乘以系数$K$的测量残差。

从这个公式可以看出，我们可以根据状态方程得到预测值，根据测量方程可以得到测量值的预测，然后在根据公式3就可以得到最终的估计值。还有一个问题就是系数$K$的确定。

××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××

接下来说一下参数$K$的确定。

首先来看一下真实值与估计值之间协方差
$$P_K=E(e_{}$$