卡尔曼滤波

统计知识

协方差

相关系数

多元高斯分布

多元高斯分布，变量之间不一定独立。关键参数：期望、协方差

卡尔曼滤波

参考链接

How a Kalman filter works, in pictures

场景

机器人做直线运动。这里只考虑机器人的速度$v$、位置$v$。运动状态矩阵表示如下：
$$\vec{x}=\left[\begin{array}{c}p\\ v\end{array}\right]$$

可以通过传感器，获得当前时刻的速度、位置、加速度。但是每次获取存在时间间隔，且存在噪声。

现在，想要利用机器人的运动状态不会突变的先验知识、牛顿运动学知识，获得更加精确的运动状态？
可以利用卡尔曼滤波。

建模

${\mathbf{B}}_k$：控制矩阵，$\overrightarrow{{\mathbf{u}}_k}$：控制向量。
${\mathbf{F}}_k$：预测矩阵，或者状态转移矩阵。

建模的时候，也可以 把加速度当做一个状态。这样运动状态维度是3，没有控制向量。
不过考虑到实际物理意义，一般都是通过油门/刹车，改变加速度，进而影响了速度、位置。所以，把加速度作为控制向量也合理。
如果把油门力度、刹车力度作为控制向量，则加速度更适合当做一个状态。
这里是简单的建模，所以，直接把加速度当做控制向量了。

预测的状态值

认为状态是不确定的，存在噪声。
不确定性用多元高斯分布来度量。期望就是建模公式所得值，协方差通过初始协方差${\mathbf{P}}_{\mathbf{k}}$、协方差性质5，进行计算更新。

${\mathbf{Q}}_k$是协方差噪声。

根据经验，这里的预测值的准确度和时间间隔成反比。
当时间间隔比较短的时候，预测值是具有参考意义的。
预测值可以当做额外的传感器的探测值。

预测的探测值

${\mathbf{H}}_k$：状态与探测器原始输出的转换矩阵。比如，传感器输出的是以m/s为单位的速度，而我们定义状态时，速度单位是km/h。这就需要一个转换。

探测值

也用高斯分布表示。
期望是传感器输出$\overrightarrow{{\mathbf{z}}_k}$，协方差是${\mathbf{R}}_k$。

滤波：探测值、探测值的预测值

现在，关于探测值的原始输出有两个分布，来自不同传感器。一个来自真实的传感器，一个来自“额外”的传感器。

上面的两个分布都是关于探测器原始输出值的。
当然，也可以转换到状态值上。

这两个多元高斯分布是相乘，会得到一个新的多元高斯分布。新的分布，便可以用来描述探测器原始值的分布。
新的高斯分布的期望、协方差如下：

“额外”的探测值：
$({\mu }_0,{\Sigma }_0)=({\mathbf{H}}_k{\hat{\mathbf{x}}}_k,{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T)$

真实的探测值：
$({\mu }_1,{\Sigma }_1)=(\overrightarrow{{\mathbf{z}}_k},{\mathbf{R}}_k)$

滤波后：


同时去掉${\mathbf{H}}_k$，精简下公式：

现在，${\hat{\mathbf{x}}}_k’$是最佳估计，${\mathbf{P}}_k’$是对应协方差。

总结

扩展卡尔曼滤波

参考
将非线性的运动方程和观测方程进行以切线代替的方式来线性化。其实就是在均值处进行一阶泰勒展开。

无迹卡尔曼滤波（UKF）

在原高斯分布中采样，很多采样点（该图中是500000个画出来的），将采样点通过非线性变换，再统计变换后的结果的均值与方差，近似用一个高斯分布表示。这种方法的缺点很明显，计算量巨大，特别是在高维时。
而在UKF的无迹变换中我们不再进行如此大量的采样，而只选取有限的采样点（下图中为3个），还是将其经过非线性变换，最后加权统计变换后结果的均值和方差。