SLAM--位姿估计 (扩展卡尔曼滤波EKF/高斯牛顿非线性优化)

一、扩展卡尔曼滤波EKF求解

1.矩阵伴随基础

李代数的伴随矩阵：
$$ad({\xi }^{\wedge })={\xi }^{⋏}={\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]}^{⋏}=\left[\begin{array}{c}{\varphi }^{\wedge }{\rho }^{\wedge }\\ 0{\varphi }^{\wedge }\end{array}\right]$$
李群的伴随：
$$\begin{gathered} Ad(\exp ({\xi }^{\wedge }))=\exp (ad({\xi }^{\wedge }))=Ad(\left[\begin{array}{c}Cr\\ 0^{T}1\end{array}\right]) \\ =\left[\begin{array}{c}C{r}^{\wedge }C\\ \\ 0{\varphi }^{\wedge }\end{array}\right] \end{gathered}$$

2.卡尔曼滤波基础

卡尔曼滤波

3.位姿推导

针对离散模型：

$$\begin{array}{rl}标称运动公式& ：T_k=\exp (\Delta t_k{\varpi }_k^{\wedge })T_{k-1}\\ & \\ 测量模型& ：y_{jk}=D^T{T}_k{p}_j\end{array}$$
其中${\varpi }_k$为广义的速度；

扰动运动：
$$\begin{array}{rl}\delta {\check{\xi }}_k& =\exp (\Delta t_k{\varpi }_k^{⋏})\delta {\hat{\xi }}_{k-1}+{\omega }_k\\ & \\ \delta y_{jk}& =D^T({\check{T}}_k{p}_j{)}^{\odot }\delta {\check{\xi }}_k+n_{jk}\end{array}$$
其中：
$$\begin{gathered} F_{k-1}=\exp (\Delta t_k{\varpi }_k^{⋏}) \\ {G}_{jk}=D^T({\check{T}}_k{p}_j{)}^{\odot } \end{gathered}$$
则由卡尔曼滤波：
预测：
$$\begin{gathered} {\check{P}}_k=F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+Q_k \\ {\check{T}}_k=\exp (\delta {\xi }^{\wedge }){\hat{T}}_{k-1} \end{gathered}$$
更新：
$$\begin{array}{rl}{K}_k& ={\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{G}}_k^T({\mathbf{G}}_k{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{G}}_k^T+{\mathbf{R}}_k{)}^{-1}\\ & \\ {\hat{\mathbf{P}}}_{\mathbf{k}}& =(1-K_k{\mathbf{G}}_k)\check{{\mathbf{P}}_{\mathbf{k}}}\\ & \\ {\hat{\mathbf{T}}}_{\mathbf{k}}& =\exp ((K_k(y_k-{\check{y}}_k){)}^{\wedge }){\check{T}}_k\end{array}$$

二、非线性优化（高斯牛顿迭代）

1.高斯牛顿迭代基础

高斯牛顿迭代推导

2.推导

对于运动方程和观测方程：
$$\begin{gathered} {\check{T}}_k=\exp (\delta {\xi }^{\wedge }){\hat{T}}_{k-1} \\ {\check{y}}_{jk}=u_m({\check{T}}_k{p}_j) \end{gathered}$$
优化的代价函数：
$$L({\check{T}}_k)=\frac{1}{2}{||y_{jk}-u_m({\check{T}}_k{p}_j)||}^2$$
令 $e({\check{T}}_k)=y_{jk}-u_m({\check{T}}_k{p}_j)$

计算雅可比矩阵，由链式求导法则：

$$J({\check{T}}_k)=\frac{\partial e({\check{T}}_k)}{\partial \delta \xi }=\frac{\partial u_m({\check{T}}_k{p}_j)}{\partial {\check{T}}_k{p}_j}\cdot \frac{\partial {\check{T}}_k{p}_j}{\partial \delta \xi }$$
扰动模型的导数：
$$\frac{\partial {\check{T}}_k{p}_j}{\partial \delta \xi }=({\check{T}}_k{p}_j{)}^{\odot }$$
所以雅可比矩阵为：
$$J=\frac{\partial u_m(x)}{\partial x}\cdot ({\check{T}}_k{p}_j{)}^{\odot }$$
求解$\Delta x$:
$$J({\check{T}}_k{)}^{T}J({\check{T}}_k)\delta ϵ=-J({\check{T}}_k{)}^T\cdot e({\check{T}}_k)$$
迭代更新：
$$T_{op}=\exp (\delta {ϵ}^{\wedge })T_{op}$$