卡尔曼滤波入门

基本概念

卡尔曼滤波中，会涉及到真值、测量误差，预测误差等概念。真值描述的是客观世界上，物体具有的状态值，这个值通过仪器测量以后，会在真值范围内呈现正态分布，测量值和真值的误差就是测量误差。

真值空间： 通常指状态变量所在的空间。有可以直接测量的，可能也包含不可直接测量的变量。

测量空间：指可直接测量的状态变量所组成的空间。

预测空间：通常和真值空间等大，用于预测状态变量的下一时刻状态。

滤波算法

一般情况下，对于一个确定系统，我们可以通过上一时刻的状态推测下一时刻的状态，比如说一辆近乎匀速的小车，它的状态变量只有位置和速度：
$$X_t=\left[\begin{array}{c}s\\ v\end{array}\right]$$
在假设他匀速的情况下，下一时刻的状态为：
$$X_{t+1}^{\prime }=\left[\begin{array}{c}s+v\times \Delta t\\ v\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\ast \left[\begin{array}{c}s\\ v\end{array}\right]=F\ast X_t$$
由于是假设条件，所以计算得到的$X^{\prime }$是我们的预测值，或者说是估计值。

F 矩阵即我们用来做预测的状态转移矩阵，它将变量$X_t$从真值空间映射到了预测空间，得到$X_{t+1}^{\prime }$

等到下一时刻到来时，我们在对实际状态进行测量，可以得到一个观测值$Z$。观测值和预测值会产生一个误差，我们称为观测值的预测误差$Y$:

$$Y_{t+1}=Z_{t+1}-H\ast X_{t+1}^{\prime }$$
在此处H可以是一个单位矩阵，H实际上是将系统状态空间映射到观测空间的映射矩阵。

假设系统状态X中存在不可直接测量的量，则通过H矩阵映射，便可将无法直接测量的量剔除，从而使状态空间映射到观测空间。

然后我们将预测误差$Y$，乘以一个滤波系数$K$，加到预测值$X^{\prime }$上，便可得到我们滤波后的状态真值$X_{t+1}$：
$$X_{t+1}=X_{t+1}^{\prime }+K\ast Y_{t+1}^{\prime }$$

当然，这里的K实际上是一个矩阵，它的作用不仅仅是设置滤波系数，更重要的是它将位于观测空间的Y映射到了状态真值空间上 。

F映射矩阵的作用是： 真值空间 => 预测空间
H映射矩阵的作用是： 真值空间 => 观测空间
K映射矩阵的作用是： 观测空间 => 真值空间

滤波系数K估计

由于观测值Z和真值也会有误差(观测误差)，它的误差范围一般由仪器决定。对于不同的状态变量，测量仪器可能不一样，所以误差自然也不一样。所以这里我们引入一个量不确定度P来衡量这种误差的大小。

不确定度越大，说明误差有可能越大

另外我们还要考虑到，不确定度可能受到外界影响而改变，所以不确定度也需要迭代式更新。

由于我们要用不确定度来计算滤波系数矩阵K，所以不确定度也得表示为一个矩阵

假设状态空间中的 位置信息的不确定度较低，速度信息不确定度更高：
$$P_t=\left[\begin{array}{cc}1& 0\\ 0& 20\end{array}\right]$$

我们可以根据它，通过矩阵相似变换得到预测空间的不确定度：
$$P_{t+1}^{\prime }=F\ast P_t\ast F^T$$

然后根据它，再通过相似变换，得到测量空间的不确定度：
$$S_{t+1}=H\ast P_{t+1}^{\prime }\ast H^T$$

由此我们便可推算滤波系数$K$，在前面的公式中，我们知道K映射矩阵的作用是： 观测空间 => 真值空间。

所以我们得到了观测空间的测量误差后，可以乘上$S_{t+1}^{-1}$将其变换回观测变量的预测空间，由于观测空间和状态空间不一定一样大，所以我们再乘上$H^T$将其映射到状态空间，最后，通过$P_{t+1}^{\prime }$矩阵，将其从状态空间映射到了状态空间的预测空间
$$K=P_{t+1}^{\prime }\ast H^T\ast S_{t+1}^{-1}$$
所以K矩阵的历程是 :
1、观测空间 => 观测变量的预测空间        ~~~~~~       By        ~~~~~~       $S_{t+1}^{-1}$
2、观测变量的预测空间 => 状态空间        ~~~~~~       By        ~~~~~~       $H^T$
3、状态空间 => 状态空间的预测空间        ~~~~~~       By        ~~~~~~        $P_{t+1}^{\prime }$

得到K矩阵后，我们前面提到不确定度P是迭代更新的，所以最后我们再更新一下P矩阵，它与状态变量的更新有异曲同工之处：
$$P_{t+1}=P_{t+1}^{\prime }-K\ast H\ast P_{t+1}^{\prime }$$

式中H矩阵的作用，是将$P_{t+1}^{\prime }$从状态空间映射到观测空间，这样我们才能和状态空间更新方程一样使用系数矩阵K。

小结

预测部分：

$X_{t+1}^{\prime }=F\ast X_t+u$
$P_{t+1}^{\prime }=F\ast P_t\ast F^T+Q$

这里的u是外部扰动（噪声），Q是过程噪声

测量与更新部分：
$Y_{t+1}=Z_{t+1}-H\ast X_{t+1}^{\prime }$
$S_{t+1}=H\ast P_{t+1}^{\prime }\ast H^T+R$
$K=P_{t+1}^{\prime }\ast H^T\ast S_{t+1}^{-1}$

这里的R是测量噪声矩阵

$X_{t+1}=X_{t+1}^{\prime }+K\ast Y_{t+1}^{\prime }$
$P_{t+1}=P_{t+1}^{\prime }-K\ast H\ast P_{t+1}^{\prime }$

参考

无人驾驶技术入门（十三）| 手把手教你写卡尔曼滤波器