高翔【自动驾驶与机器人中的SLAM技术】学习笔记（七）卡尔曼滤波器三：卡尔曼滤波器公式推导【转载】

卡尔曼滤波器三：卡尔曼公式推导

转载来源：卡尔曼滤波：从入门到精通

简述

考虑一个SLAM 问题，它由一个运动方程：
$${\mathbf{x}}_t=f({\mathbf{x}}_{t-1},{\mathbf{u}}_t)+{\omega }_t\text{\hspace{1em}(1)}$$
和一个观测方程组成：
$${\mathbf{z}}_{t,j}=h({\mathbf{y}}_j,{\mathbf{x}}_t)+v_{t,j}\text{\hspace{1em}(2)}$$
就把它当作一个线性系统吧（非线性系统请看下一讲扩展卡尔曼滤波），并且为了简化推导，忽略路标的下标j，并把路标y并入到状态向量一起优化，那么运动方程就可以写为：
$${\mathbf{x}}_t={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\omega }_t\text{\hspace{1em}(3)}$$
其中，

• ${\mathbf{x}}_t$为 t 时刻的状态向量，包括了相机位姿、路标坐标等信息，也可能有速度、朝向等信息；
• ${\mathbf{u}}_t$为运动测量值，如加速度，转向等等；
• ${\mathbf{F}}_t$为状态转换方程，将 t-1 时刻的状态转换至 t 时刻的状态；
• ${\mathbf{B}}_t$是控制输入矩阵，将运动测量值${\mathbf{u}}_t$的作用映射到状态向量上；
• ${\omega }_t$是预测的高斯噪声，其均值为 0，协方差矩阵为 $Q_t$ 。

这一步在卡尔曼滤波中也称为预测 (predict)。

类似地，测量方程可以写为：
$${\mathbf{z}}_t={\mathbf{H}}_t{\mathbf{x}}_t+{\mathbf{v}}_t\text{\hspace{1em}(4)}$$
其中，

• ${\mathbf{z}}_t$为传感器的测量值；
• ${\mathbf{H}}_t$为转换矩阵，它将状态向量映射到测量值所在的空间中；
• ${\mathbf{v}}_t$为测量的高斯噪声，其均值为0，协方差矩阵为 ${\mathbf{R}}_t$。

而卡尔曼滤波就是预测 - 测量之间不断循环迭代。当然，对于某些情况，如GPS + IMU，由于IMU 测量频率远比GPS 高，在只有IMU 测量值时，只执行运动更新，在有GPS 测量值时再进行测量更新。

运动更新

一个小例子

用一个在解释卡尔曼滤波时最常用的一维例子：小车追踪。如下图所示：

状态向量为小车的位置和速度：
$${\mathbf{x}}_t=\left[\begin{array}{c}{x}_t\\ {\dot{x}}_t\end{array}\right]\text{\hspace{1em}(5)}$$
而司机要是踩了刹车或者油门，小车就会具有一个加速度，
$${\mathbf{u}}_t=\frac{f_t}{m}$$
假设 t 和 t-1 时刻之间的时间差为 Δ𝑡 。根据物理知识，有：
$$\{\begin{array}{llcc}{x}_t=x_{t-1}+& {\dot{x}}_{t-1}\Delta t& +& \frac{1}{2}\frac{f_t}{m}\Delta t^2\\ {\dot{x}}_t=& {\dot{x}}_{t-1}& +& \frac{f_t}{m}\Delta t\end{array}\text{\hspace{1em}(6)}$$
写成矩阵形式：(注意变量对齐，提取系数构建矩阵，变量构建成向量)。
$$\left[\begin{array}{c}{x}_t\\ {\dot{x}}_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t-1}\\ {\dot{x}}_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]\frac{f_t}{m}\text{\hspace{1em}(7)}$$
跟之前的运动方程对比，就知道
$${\mathbf{F}}_t=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]，{\mathbf{B}}_t=\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]\text{\hspace{1em}(8)}$$
公式（3）的估计形式写为：
$${\hat{\mathbf{x}}}_{t\mid t-1}={\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t\text{\hspace{1em}(9)}$$

${\hat{\mathbf{x}}}_{t-1}$表示 t-1 时刻卡尔曼滤波的状态估计； ${\hat{\mathbf{x}}}_{t\mid t-1}$则表示中 t-1 到 t 时刻，预测更新所得的预测值。

再利用运动模型对状态向量进行更新后，还要继续更新状态向量的协方差矩阵$P$，公式为：
$${\mathbf{P}}_{t\mid t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1}{\mathbf{F}}_t^T+{\mathbf{Q}}_t\text{\hspace{1em}(10)}$$
假设${\mathbf{x}}_t$为 t 时刻下状态向量的真值（自然是永远未知的），由之前的现形运动方程（式(3)）给出，将式(3) 与式(9) 相减可得：
$${\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t\mid t-1}={\mathbf{F}}_t({\mathbf{x}}_t$$