• 从本节开始步入后端，对前端视觉里程计粗略得到的位姿以及空间点进一步优化.
• 共介绍两种方法：卡尔曼滤波器（EKF）和光束法平差（BA）.
• 建议在初学BA时多下些功夫了解什么是边缘化，不光本节用到，下一节的滑动窗口法也要用到边缘化的概念.（当然，本讲也给出了相应的理解）

一、后端状态估计问题的处理方法

（1）批量（Batch）:考虑一段长时间内的状态估计问题，不仅用过去的所有信息更新自己的状态，也会用未来的信息来更新。
（2）渐进（Incremental / recursive / 滤波）：当前状态只由前一个时刻决定。

• 线性系统+高斯噪声=卡尔曼滤波器（KF）
• 非线性系统+线性近似+高斯噪声=扩展卡尔曼滤波器（EKF）
• 非线性系统+非高斯噪声+非参数化=粒子滤波器

二、卡尔曼滤波器KF

希望用过去0到k中的数据更新现在的状态$x_k$（批量式处理），即估计$$P({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}).$$

（1）总体思路：从贝叶斯法则推导卡尔曼滤波（似然 x 先验）

$$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right).$$

附：贝叶斯公式$：P(A|B)=P(B|A)\times P(A)/P(B)$

（2）利用一阶马尔可夫性 预测（渐进式处理）

• 一阶马尔可夫性：k时刻状态只与k-1时刻状态有关
• 假设满足一阶马氏性的线性高斯系统为：$$\{\begin{array}{l}{\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}k=1,\dots ,N.$$$$w_k\sim N(0,\mathbf{R}).{\mathbf{v}}_k\sim N(0,\mathbf{Q}).$$假设所有的状态和噪声均满足高斯分布
• 通过运动方程确定x的先验：$$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=N\left({\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^{\top }+\mathbf{R}\right).$$$${\check{\mathbf{x}}}_k={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+\mathbf{R}.$$

附：高斯分布性质：$X\sim N({\mu }_X,{\sigma }_X^2)$，Y=AX+b，则 $Y\sim N(A{\mu }_X+b,A^2{\sigma }_X^2)$

• 由观测方程确定观测数据：$$P\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)=N\left({\mathbf{C}}_k{x}_k,\mathbf{Q}\right).$$

此处，笔者初学时不太理解，$z_k$是$x_k$的线性方程，$x_k$满足高斯分布，那$z_k$也应该服从高斯分布，为什么表示下来不符合上述附的高斯分布性质呢？

回头看，发现观测方程中的$x_k$在书中前文已经进行了重定义， x k = ⁡ d e f { x k , y 1 , … , y m } . \boldsymbol{x}_k\overset{\mathrm{def}}{\operatorname*{=}}\{\boldsymbol{x}_k,\boldsymbol{y}_1,\ldots,\boldsymbol{y}_m\}. xk​=def{ xk​,y1​,…,ym​}. 因此，观测方程中的$x_k$其实已经不满足高斯分布。

（3）利用贝叶斯法则 更新

$$N({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)=\eta N\left({\mathbf{C}}_k{\mathbf{x}}_k,\mathbf{Q}\right)\cdot N({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k).$$

附：高斯分布概率密度函数：$f(x)=\frac{1}{\sqrt{2\pi {\sigma }^2}}\exp (-\frac{(x-\mu {)}^2}{2{\sigma }^2})$

忽略系数，展开指数项，比较$x_k$的二次和一次系数，两边近似相等得：$$({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k{)}^{\mathrm{T}}{\hat{\mathbf{P}}}_k^{-1}\left({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k\right)=({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k{)}^{\mathrm{T}}{\mathbf{Q}}^{-1}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)+{\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)}^{\mathrm{T}}{\check{\mathbf{P}}}_k^{-1}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right).$$定义中间变量·卡尔曼增益K：$$K={\hat{P}}_k{C}_k^{\mathrm{T}}{Q}^{-1}.$$由二次系数相等可得：$${\hat{P}}_k=(I-K{C}_k){\check{P}}_k.$$由一次系数相等可得：$${\hat{x}}_k={\check{x}}_k+K\left({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right)$$

书中一共给出了K的两种表达式，本质都一样。$K={\hat{P}}_k{C}_k^{\mathrm{T}}{Q}^{-1}$（1）和$K={\check{P}}_k{\mathbf{C}}_k^{\mathrm{T}}({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}+{\mathbf{Q}}_k{)}^{-1}$（2），把${\hat{P}}_k=(I-K{C}_k){\check{P}}_k$代入式（1）即可得到式（2）.

三、扩展卡尔曼滤波器EKF

（1）整体的推导思路同上述KF，利用一阶泰勒展开把非线性系统转化为线性系统.$${\mathbf{x}}_k\approx f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right)+{\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1})+{\mathbf{w}}_k.$$$${\mathbf{z}}_k\approx h\left({\check{\mathbf{x}}}_k\right)+{\frac{\partial h}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k}({\mathbf{x}}_k-{\check{\mathbf{x}}}_k)+{\mathbf{n}}_k.$$记$F={\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}$、$H={\frac{\partial h}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k}$
（2）预测
运动方程根据高斯分布性质得 先验$$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{0:k-1}\right)=N(f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right),\mathbf{F}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^{\mathrm{T}}+{\mathbf{R}}_k).$$$${\check{\mathbf{x}}}_k=f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right),{\check{\mathbf{P}}}_k=F{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^{\mathrm{T}}+{\mathbf{R}}_k.$$

按照高斯分布性质，得${\check{x}}_k$=$H{\hat{x}}_{k-1}-H{\hat{x}}_{k-1}+f({\hat{x}}_{k-1},u_k)$=$f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right)$

观测的分布满足：$$P\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)=N(h\left({\check{\mathbf{x}}}_k\right)+\mathbf{H}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{Q}}_k).$$（3）更新
定义卡尔曼增益$K_k$：$$K_k={\check{P}}_k{H}^{\mathrm{T}}(H{\check{P}}_k{H}^{\mathrm{T}}+Q_k{)}^{-1}$$最终，得到：$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_k-h\left({\check{\mathbf{x}}}_k\right)\right),{\hat{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\check{\mathbf{P}}}_k.$$

EKF只做一次泰勒展开，可以认为EKF仅是优化中的一次迭代.

四、BA / 光束法平差 / 捆集调整

（1）运动方程、观测方程中参数详指

• x——相机位姿，指外参R，t（对应李群T，李代数ξ）
• 路标y——三维点P
• 观测值z——像素坐标$\mathbf{z}\stackrel{\mathrm{def}}{=}[u_s,v_s{]}^{\mathrm{T}}$

（2）以最小二乘角度考虑观测误差

$$e=z-h(\mathbf{T},\mathbf{p}).$$整体的代价函数：$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert e_{ij}{\Vert }^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert {\mathbf{z}}_{ij}-h({\mathbf{T}}_i,{\mathbf{p}}_j){\Vert }^2.$$同时把相机位姿和空间点作为优化变量$\mathbf{x}=[{\mathbf{T}}_1,\dots ,{\mathbf{T}}_m,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_n{]}^{\mathrm{T}}$，从视觉图像中提炼最优的3D模型和相机参数，即为BA

（3）BA的求解

定义优化函数：$$\frac{1}{2}{\Vert f(\mathbf{x}+\Delta \mathbf{x})\Vert }^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert {\mathbf{e}}_{ij}+{\mathbf{F}}_{ij}\Delta {\mathbf{\xi }}_i+{\mathbf{E}}_{ij}\Delta {\mathbf{p}}_j\Vert }^2.$$
把相机位姿变量、空间点变量各放到一起：$$x_{\mathrm{c}}=[{\xi }_1,{\xi }_2,\dots ,{\xi }_m{]}^{\mathrm{T}}\in {\mathbb{R}}^{6m}$$$${\mathbf{x}}_p=[{\mathbf{p}}_1,{\mathbf{p}}_2,\dots ,{\mathbf{p}}_n{]}^{\mathrm{T}}\in {\mathbb{R}}^{3n},$$则目标函数简化为：$$\frac{1}{2}{\Vert f(\mathbf{x}+\Delta \mathbf{x})\Vert }^2=\frac{1}{2}{\Vert \mathbf{e}+\mathbf{F}\Delta {\mathbf{x}}_c+\mathbf{E}\Delta {\mathbf{x}}_p\Vert }^2.$$利用G-N法（$H=J^{T}J$）或者L-M法（$H=J^{T}J+\lambda I$），最后最小二乘问题都将转化为增量线性方程：$$\text{HΔx=g}，此处J=[FE]$$其中，H具有以下稀疏特性
则增量线性方程化为：$$\left[\begin{array}{cc}B& E\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v\\ w\end{array}\right].$$

其中，$g=-J(x)f(x)=\left[\begin{array}{c}v\\ w\end{array}\right]$

利用舒尔消元 边缘化得：$$\left[\begin{array}{cc}I& -E{C}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}B& E\\ E^{\mathrm{T}}& C\end{array}\right]\left[\begin{array}{c}\Delta x_{\mathrm{c}}\\ \Delta x_p\end{array}\right]=\left[\begin{array}{cc}I& -E{C}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{c}v\\ w\end{array}\right]$$$$\left[\begin{array}{cc}B-E{C}^{-1}{E}^{\mathrm{T}}& 0\\ E^{\mathrm{T}}& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v-E{C}^{-1}w\\ w\end{array}\right].$$

方程组第一行变成和Δxp无关的项，因此可以先通过第一行求出Δxc，代入第二行求出Δxp。
实际上把求（Δxc Δxp）的问题，转化成了先固定Δxp，求出Δxc，再求Δxp的过程.

笔者认为此处可以理解为求 联合分布$P(X_c{X}_p)=P(X_p)P(X_c/X_p)$，先求Xc在Xp下的条件分布，再求Xp边缘分布，故称为边缘化。此处为路标点Xp的边缘化，还可以对位姿x进行边缘化.

利用 H 矩阵的稀疏性，使得线性方程求解变得简单（避免直接求$H^{-1}$） ，仅用求对角矩阵C的逆.

神奇的是：边缘化后的H矩阵即为协方差矩阵${\hat{p}}_k$

（4）Huber核

Huber核是鲁棒核函数的一种。为了应对误差很大时，二范数增长太快，抹平其他正确值得影响而存在。$$H(e)=\{\begin{array}{ll}\frac{1}{2}{e}^2& \text{当}|e|⩽\delta ,\\ & \\ \delta \left(|e|-\frac{1}{2}\delta \right)& \text{其他}\end{array}$$

可见，在误差较大时，Huber核函数增长明显低于二次函数.