(一)卡尔曼滤波算法简介

参考自B站MATLAB中国及《视觉SLAM十四讲》第9章9.1

1.卡尔曼滤波器–最优状态估计

假如有一个自动驾驶汽车比赛，要求参赛汽车根据GPS测量定位分别在100种地形上行驶1公里，在每种地形上都尽量停靠在1公里终点处。计算100次的平均最终位置，取位置方差最小且平均位置最接近1公里的队伍获胜。

GPS的定位是比较粗糙的，误差较大，为了赢得比赛，期望获得尽量准确的位置估计。一个汽车系统可以简化成如下形式，输入是油门，输出是位置。

上图所示的汽车系统有比较多的状态量，其模型可表示为：

只保留速度作为输入，输出是位置后的简化模型为：

上式中：${\mu }_k$是速度，$x_k$是预测量，$y_k$是测量量。
理想情况下测量值等于预测值，因此$y_k=x_k$,因此$C=1$。为了让汽车停靠位置尽量靠近终点，因此必须得到尽可能准确的$y$。

实际场景还需考虑噪声，对于测量量$y_k$，使用GPS测量时难免会有测量误差，记为$v_k\sim N(0,R),R={\sigma }_v^2$。同样对于预测变量$x_k$，因$x_k$与速度有关，速度有可能受风速等的影响，必须考虑这个过程噪声，记为${\omega }_k\sim N(0,Q),Q={\sigma }^{}$