状态估计和残差
测量
在任何一次测量中，测量值满足$z=r+\epsilon$的关系，即测量值等于实际值加物理残差，表示观测数据和实际数据之间存在随机变量r作为噪声。在随机噪声分布满足高斯分布时，$\epsilon\sim\mathcal{N}(0,\sigma^2)$，表示残差在没有系统误差的时候，符合标准差为$\sigma$的正态分布。当然实际过程中，误差并不严格遵守正态分布，故存在别的计算路径。
在这里我们需要理清楚一些核心的概念和运算中会使用到的变量：
状态向量：表示最终需要求的所有关键参数，是一个很长很长的向量，在当前项目中表现为一个320维的向量，也就是320*1的矩阵。具体来说就是位姿啊速度加速度等，具体如下图实际内容更复杂，反正大致是这个意思，状态向量就包括了所有想要获取的参数，其中不包含观测矩阵内容！！
20 帧 × 15 + 20 = 320（其中逐帧状态20 × 15 = 300）类别 每帧 / 全局 维度 内容 逐帧 每帧 15 速度(3) + 加计零偏(3) + 陀螺零偏(3) + 位置(3) + 姿态增量(3) 全局 — 20 尺度(1) + 世界对齐(6) + 时延(1) + 辅助位姿(6) + 地图变换(6) 观测向量：传感器得到的数据矩阵， 一般来说会经过一些简单运算，比如预积分等操作。维度由观测器或者最终决定传入的维度决定，比如说视觉观测向量就是2维，imu帧率高，需要对若干帧进行预积分，得到进一步的数据后传入，最终应该是15维。
代价函数：以状态向量为自变量的函数，通过预测向量与观测向量做差得到的物理残差白化后求和构建，至于白化是什么后面会详细介绍。总而言之，就是一个由观测和预测构建的标量函数，根据概率论推导，能够一定程度上描述该自变量取值的概率，其概率最大点，就是代价函数导数为0的点。当然这个导数为零的表述并不标准，实际上并不能直接进行求解，也很难有解析解。一般的工程实践是采用梯度下降或者高斯牛顿法对最低点进行逼近，在本仓库中反映为“Dogleg”法，也就是通过特定规范，对下降距离进行控制，后面也会详细介绍。
预测矩阵：预先写死的转换矩阵，通过固定数学计算得到的矩阵，负责从状态矩阵映射到观测矩阵，具体使用后面详细介绍。
残差：分为物理残差和白化残差，前者是预测向量减去测量向量得到的向量，表示当前预测和测量值的差距，包含单位；后者是使用物理残差除以残差协方差得到的向量，通过除以标准偏差，得到该测量值和实际值的偏差比例，比如说我一般来说的测量偏差为0.1m，也就是这个测量工具的误差标准差（协方差在一维的时候退化为方差），然后某一次测量的误差为3.25m，计算出来的偏差（白化残差）就是32.5个标准差，可以看出来这次测量非常的不准确，在后面的运算中就会转换成对估计值的纠正系数，反映为对状态向量的影响，也就是系统到底更相信测量值还是预测值。实际参与计算的残差来源可能很多，所以需要对所有残差都进行白化，得到单纯描述大小的白化残差才能加减。
残差协方差：表示该残差的标准偏差，对于互相影响的变量来说（比如imu传入的旋转变量，与xyz三轴的数据变量都相关，残差也存在相关性），传入的是协方差。
代价函数构建
正如上面提到的那样，我们需要根据测量值和预测值构建一个标量函数，对其求导，得到该函数取最小值时的自变量取值，就是目标变量状态向量的取值。
一般来说，根据不同观测变量满足的概率模型需要采用不同的构建形式：
| 噪声分布 | 概率密度 ∝ | $-\ln p$ | 代价形式 |
|---|---|---|---|
| 高斯 | $\exp(-r^2/2\sigma^2)$ | $r^2/2\sigma^2$ | 平方和（最小二乘） |
| 拉普拉斯 | $\exp(-\lvert r\rvert / b)$ | $\lvert r\rvert / b$ | 绝对值和（L1） |
| 柯西 | $1/(1+r^2/a^2)$ | $\ln(1+r^2/a^2)$ | 对数惩罚（鲁棒核） |
从原理上看，一般出现的残差分布可以视为中心的高斯分布加上尾部的重尾分布，测量噪声本身是高斯（中心部分）。需要”重尾”是为了容纳那些根本不属于测量噪声的东西——误匹配、遮挡、尖峰。它们不是”噪声大”，而是数据错了。
我们可以借用纯粹的高斯分布去进行理论的简单推导，得出上图中提到的：负对数满足二分之白化残差平方和的形式，然后使用这个进行后续的推导。在本仓库中，实际上对视觉和imu观测采用的都是柯西噪声的模型，让在核心数据之外的错误点对预测模型影响小一点。
项目中的完整代价函数是这样的：
$$F(x) = \underbrace{\sum_{\text{视觉}} \frac{1}{2} \ln\left(1 + \|r_{\text{vis}}\|^{2}\right)}_{\text{柯西负对数似然}} + \underbrace{\sum_{\text{IMU}} 50 \ln\left(1 + \frac{\|r_{\text{imu}}\|^{2}}{100}\right)}_{\text{柯西负对数似然}} + \underbrace{\frac{1}{2} \|r_{\text{prior}}\|^{2}}_{\text{高斯负对数先验}}$$
可以看到，对于视觉和imu的观测向量，我们都采用的是柯西负对数似然，而后面还跟着一个高斯负对数先验，这个我在后面也会简单做一个介绍，大致来说就是一个之前状态得到的一组信息以及其残差，用于描述删帧后之前状态向量的可信度。
这里提到了删帧的操作，这就不得不提及项目最关键的一点了，也就是通过滑动窗口进行预测，每一次都对往期一定窗口内的帧进行计算，可以在较小的运算复杂度内对数据进行反复线性化，避免传统batch思路堆积上下文，进而导致计算量骤增。
边缘化（marginalization）正是这里的关键操作。窗口满了要丢掉最老的帧时，不能直接删——那样这些帧带来的约束连同证据一起丢了，位姿会逐渐漂移，而协方差却虚假地变小。正确做法是把要移除的状态 $x_m$ 从联合分布里积分掉，用Schur 补把它的信息压缩成剩余状态 $x_r$ 上的一条稠密先验：
$$H_{prior}=H_{rr}-H_{rm}H_{mm}^{-1}H_{mr},\qquad b_{prior}=b_r-H_{rm}H_{mm}^{-1}b_m$$
这条先验在概念上等价于一次「虚拟观测」：它告诉优化器，这些变量应当取边缘化发生那一刻的值，而信心的度量就是 $H_{prior}$。所以总而言之，这里的操作通过一些运算保留了往期的状态信息，同时完全清除了之前帧在运算中的滞留。代价是矩阵会因填充效应（fill-in）变稠密——原本稀疏的 $H$ 在消元后会在被消变量的所有邻居之间两两产生新耦合。
代价函数的下降
本质上是求最小值，从路径上反映为高度的逐级下降，所以这一系列算法都在解决下降问题。
Dogleg下降法
狗腿法是该定位算法采用的下降思路，能够一定程度上减少迭代次数并智能更新步进距离。