非线性优化：SLAM模型建立

SLAM数学模型

运动方程

设机器人在行驶过程中，时刻$t$的位姿为$x_t$，则机器人的运动方程可用下式表达：
$$x_k=f(x_{k-1},u_k,w_k)$$
其中，$u_k$为控制量也即运动传感器的读数或输入。一般采用编码器获取机器人的行驶速度（速度运动模型）或采用里程计获取里程信息（里程运动模型）作为输入。$w_k$为运动噪声项。

运动方程的含义为，通过机器人上一时刻的位姿信息及控制量，能够估计得到机器人的当前时刻位姿。

观测方程

视觉SLAM的观察模型通常为基于特征的模型，则地图信息由一些列的路标组成。用$y_1,y_2,\cdots ,y_N$表示各个路标点。

在机器人运动过程中，传感器将得到周围路标的信息，则机器人的观测方程如下：
$$z_{k,j}=h(y_j,x_k,v_{k,j})$$
其中，$v_{k,j}$为观测噪声项。

观测方程的含义为，在路标信息$y_j$和机器人位姿$x_k$下，传感器的观测数据为$z_{k,j}$

状态估计问题

由此，得到SLAM的模型：
$$\{\begin{array}{l}{x}_k=f(x_{k-1},u_k,w_k)\\ z_{k,j}=h(y_j,x_k,v_{k,j})\end{array}$$
其中，运动方程中机器人当前位姿$x_k$可由$T_k\in SE(3)$进行描述。而观测方程则有相机针孔模型进行建模：
$$s{z}_{k,j}=K(R_k{y}_j+t_k)$$
也即在时刻$t$下，相机传感器处于机器人位姿$x_k$处，此时对路标$y_j$进行的观测对应到图像上的像素位置$z_{k,j}$处。其中，$s$为比例因子，$K$为相机内参，$R_k\in SO(3)$和平移$t_k$构成了相机的外参矩阵$T_k$。

通常，增加以零为均值$R_k、Q_{k,j}$为方差的高斯分布$w_k,v_{k,j}$作为运动噪声和观测噪声：
$$w_k\sim \mathcal{N}(0,R_k)v_{k,j}\sim \mathcal{N}(0,Q_{k,j})$$
根据模型，采用控制量$u$（通常为速度信息或里程计信息）和传感器观测$z$推断机器人的位姿$x$以及地图信息$y$

状态估计问题的处理有两种方式，一种叫做滤波器算法，一种则为批量估计算法。滤波器算法持有一个对当前时刻的估计，并用新的传感器数据更新估计。批量估计算法则将数据累计为一个小批量后，统一进行处理估计。

常用的滤波器算法有扩展卡尔曼算法（EKF）、无迹卡尔曼算法（UKF）等。相对而言，滤波器算法仅关心当前时刻下的状态估计量，而批量估计则在更大范围内得到最优。一般认为批量估计优于滤波器估计。

批量估计

对于机器人运动，考虑从1到N时刻下机器人的位姿$x_t$及对应的地标信息$y_j$：
x = { x 1 , ⋯   , x N } y = { y 1 , ⋯   , y M } x=\{x_1,\cdots,x_N\}\qquad\qquad y=\{y_1,\cdots,y_M\} x={x1​,⋯,xN​}y={y1​,⋯,yM​}
则在控制量输入$u$、传感器观测数据$z$的条件下对机器人状估计如下：
$$p(x,y|z,u)=\frac{p(z,u|x,y)p(x,y)}{p(z,u)}=\eta p(z,u|x,y)p(x,y)$$
贝叶斯法则展开，得到右侧内容，其中$\eta$为归一化因子；$p(z,u|x,y)$为似然（Likehood）；$p(x,y)$为先验（Prior）；$p(x,y|z,u)$为后验。

直接求解后验分布较为困难，转为求解一个状态最优估计，使得后验概率最大化：
$$(x,y{)}_{MAP}^{\ast }=arg\max p(x,y|z,u)=arg\max p(z,u|x,y)p(x,y)$$
归一化因子$\eta$同分布无关，归为常数项，忽略。由上述等式知：求解后验概率最大化就在于求解最大似然估计：
$$(x,y{)}_{MLE}^{\ast }=arg\max p(z,u|x,y)$$
似然的含义为：在当前机器人位姿下，可能产生的观测数据。

最大似然估计：在机器人位姿什么状态下，最可能得到当前观测到的数据。

最大似然估计

观测模型

$$(x,y{)}_{MLE}^{\ast }=argmaxp(z,u|x,y)$$

对于一次观测：
$$z_{k,j}=h(y_j,x_k)+v_{k,j}$$
以知用高斯分布建模观测噪声$v_{t,j}\sim \mathcal{N}(0,Q_{k,j})$，则可知：
$$p(z_{k,j}|x_k,y_j)\sim \mathcal{N}(h(y_j,x_k),Q_{k,j})$$
求解最大似然估计可以使用最小化负对数方式进行。

最小负对数

对于一个任意高纬度高斯分布$x\sim \mathcal{N}(\mu ,\Sigma )$：
$$p(x)=\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))$$
对其取负自然对数：
$$-\ln p(x)=\frac{1}{2}\ln ((2\pi {)}^{N}det(\Sigma ))+\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )$$
由对数函数单调递增性质，可以得到对$p(x)$求取最大化也即对负对数$-\ln p(x)$取最小化。

上述表达式右侧第一项为常数项，则对其右侧第二项进行最小项求解即可得到状态的最大似然估计。

最大似然估计

将观测方程带入上述模型：
$$\begin{array}{rl}(x_k,y_j{)}^{\ast }& =arg\max \mathcal{N}(h(y_j,x_k),Q_{k,j})\\ & =arg\min \left(\right(z_{k,j}-h(x_k,y_j){)}^T{Q}_{k,j}^{-1}(z_{k,j}-h(x_k,y_j)))\end{array}$$
称上述公式描述的为马哈拉诺比距离，简称马氏距离。该式等价于最小化噪声项，$Q_{k,j}^{-1}$叫做信息矩阵，也即高斯分布协方差矩阵的逆。

批量估计

考虑批量内时刻的数据，假设批量内各个时刻的控制量输入$u$、传感器观测数据$z$相互独立，也即控制量间相互独立，观测间相互独立，可得：
$$p(z,u|x,y)=\prod _{k}p(u_k|x_{k-1},x_k)\prod _{k,j}p(z_{k,j}|x_k,y_j)$$
由此，可独立处理各个时刻间的运动、观测。现定义运动控制量、传感器观测同模型的误差如下：
$$\begin{array}{rl}{e}_{u,k}& =x_k-f(x_{k-1},u_k)\\ e_{z,k,j}& =z_{k,j}-h(x_k,y_j)\end{array}$$
由此，可得目标函数如下：
$$\min J(x,y)=\sum _k{e}_{u,k}^T{R}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}$$
此处，实际采用最小负对数求解最大似然估计问题，故而累乘在负对数形式下变为累加。

求解目标函数最小化为非线性最小二乘问题。