1. 简化最大似然估计

当假设了马尔可夫性，从数学角度会发生哪些变化呢？
首先，当前时刻状态只和上一个时刻有关，式中等式右侧第一部分
在这里插入图片描述
可进一步简化：

这里，由于 k 时刻状态与 k − 1 之前的无关
所以就简化成只与 x_k−1 和 u_k 有关的形式，与 k 时刻的运动方程对应

第二部分可简化为：
在这里插入图片描述
这是考虑到 k 时刻的输入量 u_k 与 k − 1 时刻的状态无关，所以把 u_k 拿掉

可以看到，这一项实际是 k − 1 时刻的状态分布
于是，这一系列方程说明了，实际在做的是“如何把 k − 1 时刻的状态分布推导至 k 时刻”这样一件事
也就是说，在程序运行期间，只要维护一个状态量，对它进行不断地迭代和更新即可

2. 线性高斯系统

进一步，如果假设状态量服从高斯分布，那只需考虑维护状态量的均值和协方差即可

从形式最简单的线性高斯系统开始，最后会得到卡尔曼滤波器
线性高斯系统是说，运动方程和观测方程可以由线性方程来描述：
在这里插入图片描述
并假设所有的状态和噪声均满足高斯分布
记这里的噪声服从零均值高斯分布：

为了简洁省略了 R 和 Q 的下标
现在，利用马尔可夫性，假设知道了 k − 1时刻的后验（在 k − 1 时刻看来）状态估计 $\hat{x}$ _k−1 和它的协方差 $\hat{P}$ _k−1
现在要根据 k 时刻的输入和观测数据，确定 x_k 的后验分布

为区分推导中的先验和后验，在记号上作一点区别：
以尖帽子 $\hat{x}$ _k 表示后验，以横线 $\bar{x}$ 表示先验分布

3. 卡尔曼滤波器

卡尔曼滤波器的第一步，通过运动方程确定 x_k 的先验分布
根据高斯分布的性质，显然有：

在这里插入图片描述
这一步称为预测
它显示了如何从上一个时刻的状态，根据输入信息（但是有噪声），推断当前时刻的状态分布
这个分布也就是先验
记这里的：

另一方面，由观测方程，可以计算在某个状态下，应该产生怎样的观测数据：

在这里插入图片描述

为了得到后验概率，想要计算它们的乘积
然而，虽然知道最后会得到一个关于 x_k 的高斯分布，但计算上是有一丁点儿麻烦的
先把结果设为 x_k ∼ N( $\hat{x}$ _k, $\hat{P}$ _k)，那么：
在这里插入图片描述

4. 推导KF

这里稍微用点讨巧的方法
既然已经知道等式两侧都是高斯分布
那就只需比较指数部分即可，而无须理会高斯分布前面的因子部分
指数部分很像是一个二次型的配方，来推导一下

首先把指数部分展开，有：
在这里插入图片描述
为了求左侧的 $\hat{x}$ _k 和 $\hat{P}$ _k，把两边展开，并比较 x_k 的二次和一次系数
对于二次系数，有式1：

该式给出了协方差的计算过程
为了便于后边列写式子，定义一个中间变量，则式2：

则在式1中左右各乘 $\hat{P}$ _k，有：
在这里插入图片描述
于是有：

然后再比较一次项的系数，有：

整理（取系数并转置）得：

两侧乘以 $\hat{P}$ _k 并代入式2，得：

于是又得到了后验均值的表达
总而言之，上面的两个步骤可以归纳为“预测”（Predict）和“更新”（Update）两个步骤：

在这里插入图片描述
至此，推导了经典的卡尔曼滤波器的整个过程
事实上卡尔曼滤波器有若干种推导方式，而使用的是从概率角度出发的最大后验概率估计的形式
在线性高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计
而且，由于高斯分布经过线性变换后仍服从高斯分布，所以整个过程中没有进行任何的近似
可以说，卡尔曼滤波器构成了线性系统的最优无偏估计

参考：

《视觉SLAM十四讲》

视觉SLAM笔记（50） 线性系统和 KF