状态估计与卡尔曼滤波理解分析

补充材料：高斯系统的均值和协方差（待补充）

参考视觉SLAM十四讲中第六讲和第十讲

一.状态估计

1.贝叶斯法则

一个联合概率密度可以分解成一个条件概率密度和一个非条件概率密度的乘积：
$p(x,y)=p(x|y)\cdot p(y)=p(y|x)\cdot p(x)$

将上面的公式进行整理，就可以得到贝叶斯公式：
$p(x|y)=\frac{p(y|x)\cdot p(x)}{p(y)}$

在贝叶斯公式中，$p(x)$被称为先验，$p(y|x)$被称为似然，也是传感器模型，$p(x|y)$是后验概率。我们一般要求的就是这个后验概率，值得注意的是，在求解这个后验概率的时候一般不考虑$p(y)$，这是因为该部分与待估计的状态x无关，因此可以直接忽略。

2.SLAM中的状态估计问题

我们只讨论最简单的系统： 线性高斯系统，如果不是线性高斯系统，我们会吧非线性非高斯系统近似转换成线性高斯系统。
假设我们现在有一个运动方程和一个观测方程，公式如下
$$\begin{gathered} x_k=f(x_{k-1},u_k)+{\omega }_k \\ {z}_k=h(x_k)+{\nu }_k \end{gathered}$$
需要注意的几点：
1.运动方程是由IMU等传感器获得，观测方程是由相机等传感器获得。所有的状态量都统一到了$x_k$中，包含有当前k时刻的载体的一个pose和大量的特征点的landmark。对于运动方程，当前时刻的状态量一般只有载体的一个pose（运动方程不存在特征点，对于IMU，可能还有速度v和偏置bias）。不管包含有多少的状态量，最后都统一到一个$x_k$中。
2.假设两个方程的噪声都是满足零均值的高斯分布（也就是正态分布）：
${\omega }_k\sim N(0,R_k),{\nu }_k\sim N(0,Q_k)$

2.1 如何由概率对状态进行估计

前面的方程可以用下面的条件概率分布表示，即在已知k时刻运动数据u和观测数据z的条件下，求状态量的条件概率分布：
$P(x_k|z_k,u_k)$
首先简单一点理解，不存在运动方程，只有观测方程，即条件概率变成了
$P(x_k|z_k)$
根据贝叶斯公式，可以分解该公式
$P(x_k|z_k)=\frac{P(z_k|x_k)\cdot P(x_k)}{P(z_k)}\approx P(z_k|x_k)\cdot P(x_k)$

这里先验$P(x_k)$相当于一个初始值，比如说是初始的pose和初始的landmark的位置（亦或者VIO中的先验残差那一部分，就是这里的先验概率对应的最小二乘模型），这个先验可有可无。这里似然$P(z_k|x_k)$就是相机的观测模型（重投影模型，对应VIO中的相机重投影残差，就是这里的似然对应的最小二乘模型）。这里后验概率$P(x_k|z_k)$相当于通过最新k时刻的观测和先验得到的最优的当前k时刻的状态量的结果。求后端概率最大化即（MAP， maximize a posterior） 就是求解最优的状态量，直接求解据说比较困难，所以转换成求似然和先验乘积的最大化。更加简单，如果没有先验（其实有先验也一样，就是在残差方程中多加一个先验的残差项）。那就是求最大似然估计（MLE，Maximize likelihood estimation）。转换关系是：
求最优状态量–》求最大后验概率–》求最大似然估计

最大似然估计的求解比较简单，就是传感器模型，这里就是相机的重投影残差方程。

如何求最大似然估计详见视觉SLAM十四讲的6.1.2.
核心思想是对最大似然估计做-log运算，最后发现，只要计算出传感器模型的残差和协方差就可以对状态量进行估计。

2.2 考虑先验概率对状态进行估计

前面2.1是没有考虑先验对状态进行估计，现在我们需要考虑了，我们希望用过去1到k中所有的数据来估计现在的状态分布：
$P(x_k|x_0,u_{1:k},z_{1:k})$
稍微解释下这个概率分布，和2.1的$P(x_k|z_k,u_k)$不同，这里想要估计k时刻的状态量$x_k$,是在已知初始的$x_0$,1到k时刻所有的运动数据u和观测数据z的条件下，求状态量的条件概率分布，
还是根据贝叶斯公式，可以分解该公式：
$P(x_k|x_0,u_{1:k},z_{1:k})\approx P(z_k|x_k)\cdot P(x_k|x_0,u_{1:k},z_{1:k-1})$
这个公式的是按照$P(x_k|z_k)=\frac{P(z_k|x_k)\cdot P(x_k)}{P(z_k)}\approx P(z_k|x_k)\cdot P(x_k)$直接把$z_k$和$x_k$交换了下位置，其余的条件变量位置不变，简单点，你直接把其余的条件变量去掉，发现和$P(x_k|z_k)=\frac{P(z_k|x_k)\cdot P(x_k)}{P(z_k)}\approx P(z_k|x_k)\cdot P(x_k)$就会一模一样。

新的贝叶斯公式的理解：
这里先验 $P(x_k|x_0,u_{1:k},z_{1:k-1})$包含了前面所有时刻的信息，这些信息是： 初始的状态量$x_0,1到k时刻的运动数据u,以及1到k-1时刻的观测方程z。这里似然 $P(z_k|x_k)$和2.1里的似然是一样的，都是相机的观测模型。这里后验概率 $P(x_k|x_0,u_{1:k},z_{1:k})$和2.1不同，是包含了k时刻以及之前所有数据，还有初始的状态$x_0$。

对于先验的考虑，现在有两种理论：

（1）一阶马尔可夫，认为k时刻的状态至于k-1时刻的状态有关，而与之前所有的状态没有关系，这种就是滤波的方法进行状态估计，在滤波的方法中，我们会从某一时刻的状态估计，推导下一个时刻的状态。

（2）考虑k时刻状态与之前所有状态的估计有关系，即非线性优化的方法。

3.线性系统与KF

我们再把2.2的后验概率公式拿过来：

$P(x_k|x_0,u_{1:k},z_{1:k})\approx P(z_k|x_k)\cdot P(x_k|x_0,u_{1:k},z_{1:k-1})$

为了来求解这个概率公式，需要单独先求出似然和先验的概率，然后再相乘得到xk时刻的后验概率。

在此之前我们有下面两个重点：

（1）我们讨论简单的线性高斯系统。那么运动方程和观测方程可以使用下面的线性方程来描述：

$\begin{array}{rl}{x}_k& =A_k{x}_{k-1}+u_k+w_k\\ z_k& =C_k{x}_k+v_k\end{array}$

还是假设两个方程的噪声都是满足零均值的高斯分布（也就是正态分布）：
${\omega }_k\sim N(0,R_k),{\nu }_k\sim N(0,Q_k)$

（2）对于KF来说，高斯系统只需要考虑和维护均值和协方差即可，均值是迭代更新的状态量具体结果，而协方差是该时刻的状态量分布情况。

3.1 先看似然的概率

似然概率比较简单，由于观测方程是是线性高斯系统，所以似然概率为：

$P(z_k|x_k)=N(C_k{x}_k,Q_k)$

$C_k{x}_k$是观测的均值，$Q_k$是观测的协方差

3.2 再单独把先验拿出来-预测

$P(x_k|x_0,u_{1:k},z_{1:k-1})=\int P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})\cdot P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}$

根据一阶马尔可夫性，可以把上面的公式改写：

$\begin{array}{rl}P(x_k|x_0,u_{1:k},z_{1:k-1})& =\int P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})\cdot P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}\\ & =\int P(x_k|x_{k-1},u_k)\cdot P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}\\ & =\int P(x_k|x_{k-1},u_k)d{x}_{k-1}\cdot P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}\end{array}$

其中$\int P(x_k|x_{k-1},u_k)d{x}_{k-1}$是运动方程的概率，$P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}$是k-1时刻的后验概率。那么k时刻的先验概率的意义就是k时刻的运动方程概率乘以k-1时刻的后验概率。

这里插一句，k时刻的后验概率就是k时刻的观测方程概率，乘以k时刻的运动方程概率，再乘上k-1时刻后验概率。而k时刻的后验概率就包含了k时刻的状态和协方差

假设k-1时刻的后验概率为：

$P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}=N({\hat{x}}_{k-1},{\hat{P}}_{k-1})$

那么k时刻的先验概率
$P(x_k|x_0,u_{1:k},z_{1:k-1})=N(A_k{\hat{x}}_{k-1}+u_k,{\hat{A}}_k{P}_{k-1}{A}_k^T+R_k)$

而再卡尔曼滤波框架下，k时刻的先验概率就是预测，其物理意义就是运动方程的概率乘上k-1时刻的后验概率。

3.3 更新

更新就是计算k时刻的后验概率。其物理意义就是k时刻的观测方程概率乘上k时刻的先验概率。具体的计算方法推导详见视觉slam十四讲的10.1.2公式10.14-10.23，最后卡尔曼滤波的模型间视觉slam十四讲的公式10.24-10.26.

简单的线性高斯系统的卡尔曼滤波过程分为两个步骤，第一是预测过程，预测过程是考虑当前时刻的运动方程概率和前一时刻的后验概率，从而得到预测的当前概率，也就是当前的先验概率。第二是更新，更新是考虑当前时刻的观测和当前时刻的先验概率，得到当前时刻的后验概率，而后验概率就是当前时刻的概率，包含有当前时刻状态量的均值和协方差。