视觉SLAM十四讲-第十讲笔记

本讲开始进入后端模块。后端优化目前分为基于滤波器的方法和基于非线性优化的方法，本讲讲述以EKF为代表的滤波器法，以及稀疏性。

一、 后端优化概述

1. 后端模块的作用

前端估计得到：

• $k$时刻相机的位姿，$x_k$
• 地图中路标$j$的世界坐标，$y_j$

前端估计只基于$k$的前一时刻或前几时刻。后端优化所做的，则是根据到目前为止观测到的所有信息，优化之前的估计结果。
后端优化中，把位姿$x$和路标$y$都看做服从某种概率分布的随机变量。已知相机运动数据$u$，观测数据$z$，问题转化为确定其概率分布，进行最大似然估计。

后端优化主要有两种方法：

1. 假设马尔科夫性，$k$时刻状态只与$k-1$时刻有关。即本讲的EKF法。
2. 假设$k$时刻状态与之前所有状态有关，为基于非线性优化的方法。将在下一讲详细阐释。

2. 问题描述

变量：
$x_k$：$k$时刻下所有未知量，包含相机和此时刻m个可见路标。
已知：
$u_{1:k}$：前$k$帧的运动数据。
$z_{1:k}$：前$k$帧的观测数据。
估计：
$P(x_k|x_0,u_{1:k},z_{1:k})$：根据所有已知，估计现在的位姿和路标的状态分布。

根据贝叶斯法则：
$$P(x_k|x_0,u_{1:k},z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1})$$
即：后验正比于似然乘先验。
其中，先验指前$k-1$帧的观测。根据马尔科夫假设，对先验展开得：
$$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})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}$$
即：$x_k$受$x_{k-1}$影响，考虑所有$k-1$时刻的$x$可能，对每一种可能，其概率是$x_{k-1}$下$x_k$的概率，乘以$x_{k-1}$的概率。
问题的最终目标是：求状态变量的最大后验概率形式。

二、 卡尔曼滤波器

1. 线性系统和卡尔曼滤波器(KF)

根据马尔科夫性，当前时刻状态只和上一时刻有关。我们来化简先验概率部分，把这个公式挪下来：
$$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})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}$$
等式右边第一项可以化简为：
$$P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})=$$