slam公式总结

卡尔曼滤波

原方程:

• 状态方程: $x_k=A_k\cdot x_{k-1}+B\cdot u_k+w_k$
• 观测方程: $z_k=C_k\cdot x_k+v_k$

预测步骤:

• 预测状态: ${\hat{x}}_k,\text{pred}=A_k\cdot {\hat{x}}_{k-1}+B\cdot u_k$；其中，$A_k$为转移矩阵，$B$为控制输入矩阵，slam一般忽略$B$。
• 预测协方差:$P_k,\text{pred}=A_k\cdot P_{k-1}\cdot A_k^T+R_k$；P为估计协方差矩阵，目标是最小化P值，$R_k$为过程噪声的协方差矩阵。

更新步骤:

• 卡尔曼增益计算: $K_k=P_k,\text{pred}\cdot C_k^T\cdot (C_k\cdot P_k,\text{pred}\cdot C_k^T+Q_k{)}^{-1}$；$K_k$为卡尔曼增益，$C_k$为观测矩阵，$Q_k$为观测噪声的协方差矩阵。
• 更新状态:${\hat{x}}_k={\hat{x}}_k,\text{pred}+K_k\cdot (z_k-C_k\cdot {\hat{x}}_k,\text{pred})$；此步调整预测量，基于观测值的偏差确定其有效性。
• 更新协方差: $P_k=(I-K_k\cdot C_k)\cdot P_k,\text{pred}$；$I$为单位矩阵，此式反映经过校正后协方差矩阵的更新。

以上即为卡尔曼滤波的基本流程，通过预测与更新两个循环步骤，不断优化系统状态的估计。

高斯-牛顿（Gauss-Newton）方法是一种迭代优化算法，用于求解非线性最小二乘问题。在SLAM和其他工程领域中，它被广泛应用于参数估计问题，如优化传感器测量与模型预测之间的残差。高斯-牛顿法的核心思想是每一步迭代都通过线性化非线性函数来近似求解，并逐步减小目标函数值。下面是高斯-牛顿方法的基本公式和步骤概述：

1、最小二乘问题

最小二乘法（又称最小平方法）是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。最小二乘问题可以表述为：
$$\underset{x}{\min }F(x)=||f(x)|{|}^2$$

2、高斯牛顿法原理

当 x 在 $x_k$ 处，具有增量 $\Delta x_k$，优化目标函数取值为：
$$\underset{x}{\min }F(x_k+\Delta x_k)=||f(x_k+\Delta x_k)|{|}^2$$
不同于牛顿法，我们不对优化目标函数进行泰勒展开，我们对优化目标函数中的一部分，即 $f(x)$ 进行一阶泰勒展开。展开后优化目标函数为：
$$F(x_k+\Delta x_k)\approx ||f(x_k)+J(x_k)\Delta x_k|{|}^2$$

该式中，只有$\Delta x_k$是变量，而 $x_k$ 是一个确定的数值，$f(x_k)$，$J(x_k{)}^T$均是一个确定的数值，即这是一个以 $\Delta x_k$为变量的二次函数。且二次项的系数为正，因此该函数拥有最小值。其取最小值的条件就是该式对 Δx_k 的导数等于零，即：
$$\frac{\partial ||f(x_k)+J(x_k)\Delta x_k|{|}^2}{\partial \Delta x_k}=0$$
解得：
$$J(x_k{)}^{T}J(x_k)\Delta x_k=-J(x_k{)}^{T}f(x_k)$$
记：
$$H(x_k)=J(x_k{)}^{T}J(x_k),g(x_k)=-J(x_k{)}^{T}f(x_k)$$
则高斯牛顿法的增量方程为：
$$H(x_k)\Delta x_k=g(x_k)$$

罗德里格斯公式（Rodrigues’s Formula），也称为罗德里格斯旋转公式。这个公式在SLAM和其他涉及三维空间旋转计算的领域非常重要。罗德里格斯公式提供了一种方法，用于将一个围绕任意轴的旋转角度转换为一个3x3的旋转矩阵。这对于从IMU（惯性测量单元）数据中恢复姿态、在视觉里程计中处理相机姿态变化等SLAM关键步骤至关重要。

罗德里格斯公式的基本形式如下：
$$\mathbf{R}=\mathbf{I}+\sin (\theta )\mathbf{K}+(1-\cos (\theta )){\mathbf{K}}^2$$
其中：

• $\mathbf{R}$ 是最终得到的$3\ast 3$旋转矩阵。
• $\theta$ 是绕单位旋转轴 $\mathbf{n}$的旋转角度。
• $\mathbf{K}=[\mathbf{n}{]}_{\times }$ 是由旋转轴 $\mathbf{n}$ 构成的反对称矩阵，$[\cdot {]}_{\times }$表示向量到反对称矩阵的转换操作。
• $\mathbf{I}$ 是单位矩阵。

反对称矩阵：

旋转轴 $\mathbf{n}$ 对应的反对称矩阵 $\mathbf{K}$ 定义为：
$$\mathbf{K}=[\mathbf{n}{]}_{\times }=\left(\begin{array}{ccc}0& -n_z& n_y\\ n_z& 0& -n_x\\ -n_y& n_x& 0\end{array}\right)$$

预测：
$$\begin{array}{rl}p(t+\Delta t)& =p(t)+v\Delta t+\frac{1}{2}(R(\tilde{a}-b_a))\Delta t^2+\frac{1}{2}g\Delta t^2,\\ v(t+\Delta t)& =v(t)+R(\tilde{a}-b_a)\Delta t+g\Delta t,\\ R(t+\Delta t)& =R(t)Exp((\omega -b_g)\Delta t),\\ b_g(t+\Delta t)& =b_g(t),\\ b_a(t+\Delta t)& =b_a(t),\\ g(t+\Delta t)& =g(t).\end{array}\text{\hspace{1em}(1)}$$
注：需要注意g的方向，这里g是当z轴朝上时，g(0,0,-9.8)朝下。

误差状态推导公式：
$$\begin{array}{rl}\delta p(t+\Delta t)& =\delta p+\delta v\Delta t,\\ \delta v(t+\Delta t)& =\delta v+(-R(\tilde{a}-b_a{)}^{\wedge }\delta \theta -R\delta b_a+\delta g)\Delta t-{\eta }_v,\\ \delta \theta (t+\Delta t)& =Exp(-(\omega -b_g)\Delta t)\delta \theta -\delta b_g\Delta t-{\eta }_{\theta },\\ \delta b_g(t+\Delta t)& =\delta b_g+{\eta }_g,\\ \delta b_a(t+\Delta t)& =\delta b_a+{\eta }_a,\\ \delta g(t+\Delta t)& =\delta g.\end{array}\text{\hspace{1em}(2)}$$

${\eta }_v$ 为 ${\eta }_a^{\prime }\ast \Delta t$，${\eta }_{\theta }$ 为${\eta }_g^{\prime }\ast \Delta t$，其中${\eta }_a^{\prime }$为 $R\ast 线加速度的测量噪声$，${\eta }_g^{\prime }$为角速度的测量噪声

${\eta }_g、{\eta }_a$ 为高斯过程，表示零偏的随机游走、零偏的导数，运动过程中零偏的变化，其均值为0，协方差函数为Cov($b_g$)δ(t−t′)、 Cov($b_a$)δ(t−t′)其中，δ 函数是一个单位脉冲函数，表示只有当时间 t 和 t′相同时，两个变量才有相关性，imu出厂会标定这两个值。

预积分:

1. $R_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}\left(p_{b_k}^w+v_{b_k}^w\Delta t_k+\frac{1}{2}{g}^w\Delta t_k^2\right)+{\alpha }_{b_{k+1}}^{b_k}$

2. $R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}(v_{b_k}^w+g^w\Delta t_k)+{\beta }_{b_{k+1}}^{b_k}$

3. $q_w^{b_k}\otimes q_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}$

注：需要注意g的方向，这里g是当z轴朝上时，g(0,0,-9.8)朝下。

其中，$\alpha 、\beta 、\gamma$就是预积分量，当$R_w^{b_k}$为幺元时，其退化为普通的预测公式，因此可以用作帧间约束。对应离散形式为：

${\hat{a}}_{i+1}^{b^k}={\hat{a}}_i^{b^k}+{\beta }_i^{b^k}\delta t+\frac{1}{2}(R({\hat{\gamma }}_{i+1}^{b^k})({\hat{a}}_{i+1}-b_a))\delta t^2$

${\beta }_{i+1}^{b^k}={\beta }_i^{b^k}+R({\hat{\gamma }}_{i+1}^{b^k})({\hat{a}}_{i+1}-b_a)\delta t$

${\hat{\gamma }}_{i+1}^{b^k}={\hat{\gamma }}_i^{b^k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}({\omega }_i-b_{\omega })\delta t\end{array}\right]$ ，这里运用了旋转向量转四元数时的等价无穷小

右扰动:
$$\begin{array}{rl}\frac{\partial (R\exp ({\varphi }^{\wedge })p+t)}{\partial \varphi }& =\underset{\varphi \to 0}{\lim }\frac{(R\exp ({\varphi }^{\wedge })p+t)-(Rp+t)}{\varphi }\\ & \approx \underset{\varphi \to 0}{\lim }\frac{(R(I+{\varphi }^{\wedge })p+t)-(Rp+t)}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{R{\varphi }^{\wedge }p}{\varphi }\\ & =-R{p}^{\wedge }\end{array}$$

使用四元数求解旋转量:
可以使用左乘矩阵和右乘矩阵来转成向量的求解
$$q_1\otimes q_2=[q_1{]}_L{q}_2=[q_2{]}_R{q}_1$$

SVD求解超定方程:
正交矩阵的保范性：乘一个正交矩阵，其模大小不变，可以想象成乘一个旋转矩阵。

$$\begin{array}{rl}\Vert Ax-b{\Vert }_2^2& ={\Vert U\left[\begin{array}{c}\Sigma \\ 0\end{array}\right]V^{T}x-b\Vert }_2^2（两边同时乘U^T，不改变模大小）\\ & ={\Vert \left[\begin{array}{c}\Sigma \\ 0\end{array}\right]V^{T}x-{\left[\begin{array}{cc}{\overline{U}}_n& \overline{U}\end{array}\right]}^{T}b\Vert }_2^2(U^T={\left[\begin{array}{cc}{\overline{U}}_n& \overline{U}\end{array}\right]}^T)\\ & ={\Vert \left[\begin{array}{c}\Sigma V^{T}x-{\overline{U}}_n^{T}b\\ -{\overline{U}}^{T}b\end{array}\right]\Vert }_2^2\\ & ={\Vert \Sigma V^{T}x-{\overline{U}}_n^{T}b\Vert }_2^2+{\Vert {\overline{U}}^{T}b\Vert }_2^2\ge {\Vert {\overline{U}}^{T}b\Vert }_2^2\end{array}$$
等号当且仅当$\Sigma V^{T}x-{\overline{U}}_n^{T}b=0$时成立，所以：
$$x=(\Sigma V^T{)}^{-1}{\overline{U}}_n^{T}b=V{\Sigma }^{-1}{\overline{U}}_n^{T}b$$这就是线性最小二乘问题的解。

特殊情况：齐次线性方程组$Ax=0$

$$min||Ax||st.||x||=1$$

则：$\Sigma V^{T}x=0$，
令$V^{T}x=y$，由正交矩阵的保范性知$||y||=1$

当仅当$y=[0,0,0,1{]}^T$时，$\Sigma y$取得最小值，则$x=V\ast [0,0,0,1{]}^T$

此时，最小二乘解为$A^{T}A$最小特征值对应的特征向量。

对于3维实向量$\vec{\varphi }$和一个小量$\delta \vec{\varphi }$，有下述近似性质：
$$\begin{array}{rl}& Exp(\vec{\varphi }+\delta \vec{\varphi })\approx Exp(\vec{\varphi })\cdot Exp(J_r(\vec{\varphi })\cdot \delta \vec{\varphi })\\ & Log(Exp(\vec{\varphi })\cdot Exp(\delta \vec{\varphi }))=\vec{\varphi }+J_r^{-1}(\vec{\varphi })\cdot \delta \vec{\varphi }\end{array}$$
式1可以这样理解：$R^3$中的向量$\vec{\varphi }$加上一个小量$\delta \vec{\varphi }$，对应到SO(3)中则是Exp($\vec{\varphi }$)右乘一个Exp($J_r(\vec{\varphi })\cdot \delta \vec{\varphi }$)；
式2则可这样理解：SO(3)中Exp($\vec{\varphi }$)右乘一个Exp($\delta \vec{\varphi }$)，对应到$R^3$中则是$\vec{\varphi }$加上一项$J_r^{-1}(\vec{\varphi })\cdot \delta \vec{\varphi }$。

$J_r(\vec{\varphi })$是SO(3)的右Jacobian，将切空间的“加性项”和SO(3)中的右“乘性项”联系在一起。$J_r(\vec{\varphi })$及其逆$J_r^{-1}(\vec{\varphi })$的表达式如下：
$$\begin{array}{rl}{J}_r(\vec{\varphi })& =I-\frac{1-\cos (\Vert \vec{\varphi }\Vert )}{\Vert \vec{\varphi }{\Vert }^2}{\vec{\varphi }}^{\wedge }+\frac{\Vert \vec{\varphi }\Vert -sin(\Vert \vec{\varphi }\Vert )}{\Vert \vec{\varphi }{\Vert }^3}({\vec{\varphi }}^{\wedge }{)}^2\\ J_r^{-1}(\vec{\varphi })& =I+\frac{1}{2}{\vec{\varphi }}^{\wedge }+\left(\frac{1}{\Vert \vec{\varphi }{\Vert }^2}-\frac{1+\cos (\Vert \vec{\varphi }\Vert )}{2\cdot \Vert \vec{\varphi }\Vert \cdot sin(\Vert \vec{\varphi }\Vert )}\right)({\vec{\varphi }}^{\wedge }{)}^2\end{array}$$

cartographer：

缺点：没有使用预积分，因此为了较少计算量，使用了子地图，但是子地图生成时是直接使用预积分预测的，没考虑噪声，而子地图内部后端是不优化的，这里可能会存在误差，可以引入ESKF。

指数平均滤波对齐重力：https://blog.csdn.net/ergevv/article/details/136618411?spm=1001.2014.3001.5502

分支定界法计算后端约束：https://zhuanlan.zhihu.com/p/336807140

后端优化：前端计算的相对变换和后端计算的约束之差作为误差来优化

LIO-sam

点到线：在局部地图上，遍历当前帧点，找到5个最近的点，计算这5个点的方差，对其进行特征值分解（如果点云数据大致分布在一条直线上，那么数据变化最大的方向实际上是这条直线本身的方向。因为点云数据在该直线方向上是有组织、有规律地分布的，所以沿着直线方向的方差（或对应的最大特征值）实际上反映了数据的主要变化趋势，这是数据点分布最集中、而非最分散的方向。相反，在垂直于这条直线的维度上，理论上数据的变化应该是最小的，因为如果所有点完美地落在同一直线上，那么垂直方向上的方差应为0，对应特征值也应为0（表明没有变化）），最大特征值应远大于次大特征值，其对应特征向量是直线方向，从而求出当前点和局部地图对应的直线的两个点。使用外积计算三个点的面积，两点距离为底，则可求出高（残差），方向则是既垂直底，又垂直外积矢量，再归一化，即是雅可比

点到面：在局部地图上，遍历当前帧点，找到5个最近的点，根据Ax+By+Cz+1=0;来求解超定方程，之后再将5个点分别代进去，距离过大则舍弃。将当前点代入方程得到残差，A、B、C则是点到面的方向，即残差对位置的雅可比

因子图：使用gtsam里的因子图代替了以往的滑窗，随着新的观测数据到来，可以添加新的因子并重新优化整个图，利用已有的雅可比矩阵和新增的雅可比矩阵，减少计算量来优化全局位资，无需从头开始计算，大大提高了实时性和计算效率，因子图里面也包含边缘化操作，用于去除旧约束。

可优化：ICP使用的是欧拉角，可以改成四元数

VINS-mono：

零偏初始化：特征点匹配的旋转量和预积分的旋转量之差作为误差来约束得到零偏

初始化速度、重力和尺度因子：残差可定义为相邻两帧之间 IMU 预积分出的增量与预测值之间的误差

边缘化：如果不考虑旧帧的影响，后端优化就是预积分与滑窗求得的位姿、重投影误差、回环检测（重投影误差）。为了保留旧帧的影响，引入边缘化，通过舒尔补把滑窗里的第0帧对后面帧的约束保留下来，对新的约束关系求得J和e，把新的约束也加入后端优化中。

DRT-init:
零偏初始化：两帧之间的一个特征点与两相机光点构成的平面的法向量垂直平移t，则考虑所有特征点，他们的法向量共面。