变量：
1、预积分量： α 、 θ 、 β 、 b k a 、 b k g \alpha、\theta、\beta、b^a_k、b^g_k α、θ、β、bka、bkg
2、前一次优化后的状态： q k 、 p k 、 v k 、 b k g 、 b k a q_k、p_k、v_k、b^g_k、b^a_k qk、pk、vk、bkg、bka，优化过程 b k g 、 b k a b^g_k、b^a_k bkg、bka设为常数
3、预积分预测的后一次的位姿： q k + 1 p r e 、 p k + 1 p r e 、 v k + 1 p r e 、 b k + 1 g 、 b k + 1 a q^{pre}{k+1}、p^{pre}{k+1}、v^{pre}{k+1}、b^g{k+1}、b^a_{k+1} qk+1pre、pk+1pre、vk+1pre、bk+1g、bk+1a
4、点云匹配得到的后一次的位姿： q k + 1 p c 、 p k + 1 p c q^{pc}{k+1}、p^{pc}{k+1} qk+1pc、pk+1pc
残差的求取：
1、imu自身的约束：
（1）每次计算残差之前需要使用零偏更新一次预积分
（2）
E α = q k ∗ ( p k + 1 p r e − p k − v k ∗ d e l t a t − 1 2 g ∗ δ t 2 ) − α E θ = ln ( exp ( − [ θ ] × ) ∗ q k ∗ ( q k + 1 p r e ) − 1 ) ∨ E β = q k ∗ ( v k + 1 p r e − v k − g ∗ δ t ) − β E b a = b k + 1 a − b k a E b g = b k + 1 g − b k g \begin{align*} E_\alpha &= q_k * (p^{pre}{k+1} - p_k - v_k * delta t - \frac{1}{2}g*\delta t^2) - \alpha \\ E\theta &= \ln(\exp(-[\theta]\times) * q_k * (q^{pre}{k+1})^{-1})^\vee \\ E_\beta &= q_k * (v^{pre}{k+1} - v_k - g*\delta t) - \beta \\ E{b^a} &= b^a_{k+1} - b^a_k \\ E_{b^g} &= b^g_{k+1} - b^g_k \\ \end{align*} EαEθEβEbaEbg=qk∗(pk+1pre−pk−vk∗deltat−21g∗δt2)−α=ln(exp(−[θ]×)∗qk∗(qk+1pre)−1)∨=qk∗(vk+1pre−vk−g∗δt)−β=bk+1a−bka=bk+1g−bkg
（3）在ceres中没有提供直接添加信息矩阵的接口（g2o有），因此需要手动添加信息矩阵到残差中。
在最小二乘法问题中，当我们讨论带信息矩阵的最小二乘法时，通常是指加权最小二乘法（Weighted Least Squares, WLS），其中权重矩阵通常是根据测量误差的逆协方差矩阵来选择的。这种情况下，信息矩阵就是测量误差的逆协方差矩阵。
假设我们要最小化的残差向量为 r ( x ) = y − f ( x ) \mathbf{r}(\mathbf{x}) = \mathbf{y} - f(\mathbf{x}) r(x)=y−f(x)，其中 y \mathbf{y} y 是观测值向量， f ( x ) f(\mathbf{x}) f(x) 是由参数向量 (\mathbf{x}) 定义的模型预测值。如果观测数据的误差服从已知分布且其协方差矩阵为 Σ \Sigma Σ，则我们可以定义一个加权的残差向量，使得每个观测值的重要性与其误差的大小成反比。这样可以得到如下目标函数：
min x 1 2 r ( x ) T Σ − 1 r ( x ) \min_{\mathbf{x}} \frac{1}{2} \mathbf{r}(\mathbf{x})^T \Sigma^{-1} \mathbf{r}(\mathbf{x}) xmin21r(x)TΣ−1r(x)
在这个公式中， Σ − 1 \Sigma^{-1} Σ−1就是我们所说的信息矩阵，它反映了观测值的精度信息。如果误差是独立同分布的，那么 Σ \Sigma Σ 可能是单位矩阵或对角矩阵，此时加权最小二乘就退化为普通最小二乘。
Σ − 1 \Sigma^{-1} Σ−1有两个作用：
1、对残差加权，衡量不同残差之间的重要性，避免某个残差过大，导致难以优化
2、使不同单位之间可以比较，例如面积和体积是不可比的，因此需要引入信息矩阵
当 r ( x ) \mathbf{r}(\mathbf{x}) r(x) 是 x \mathbf{x} x 的非线性函数时，我们可以用泰勒级数展开来近似 r ( x ) \mathbf{r}(\mathbf{x}) r(x)，从而得到线性化后的最小二乘问题。如果当前参数估计为 x k \mathbf{x}_k xk，则线性化后的残差可以表示为：
r ( x k + Δ x ) ≈ r ( x k ) + J ( x k ) Δ x \mathbf{r}(\mathbf{x}_k + \Delta \mathbf{x}) \approx \mathbf{r}(\mathbf{x}_k) + J(\mathbf{x}_k) \Delta \mathbf{x} r(xk+Δx)≈r(xk)+J(xk)Δx
其中 J ( x k ) J(\mathbf{x}_k) J(xk) 是雅可比矩阵。将此代入目标函数中，我们得到：
min Δ x 1 2 ( r ( x k ) + J ( x k ) Δ x ) T Σ − 1 ( r ( x k ) + J ( x k ) Δ x ) \min_{\Delta \mathbf{x}} \frac{1}{2} (\mathbf{r}(\mathbf{x}_k) + J(\mathbf{x}_k) \Delta \mathbf{x})^T \Sigma^{-1} (\mathbf{r}(\mathbf{x}_k) + J(\mathbf{x}_k) \Delta \mathbf{x}) minΔx21(r(xk)+J(xk)Δx)TΣ−1(r(xk)+J(xk)Δx)
对上式关于 Δ x \Delta \mathbf{x} Δx 求导并令导数为零，可以得到正规方程：
J ( x k ) T Σ − 1 J ( x k ) Δ x = − J ( x k ) T Σ − 1 r ( x k ) J(\mathbf{x}_k)^T \Sigma^{-1} J(\mathbf{x}_k) \Delta \mathbf{x} = -J(\mathbf{x}_k)^T \Sigma^{-1} \mathbf{r}(\mathbf{x}_k) J(xk)TΣ−1J(xk)Δx=−J(xk)TΣ−1r(xk)
现在需要手动把 Σ − 1 \Sigma^{-1} Σ−1加入到残差中：
min x 1 2 r ( x ) T Σ − 1 r ( x ) = min x 1 2 r ( x ) T L L T r ( x ) = min x 1 2 ( L T r ( x ) ) T ( L T r ( x ) ) \min_{\mathbf{x}} \frac{1}{2} \mathbf{r}(\mathbf{x})^T \Sigma^{-1} \mathbf{r}(\mathbf{x}) = \min_{\mathbf{x}} \frac{1}{2} \mathbf{r}(\mathbf{x})^T LL^T \mathbf{r}(\mathbf{x}) = \min_{\mathbf{x}} \frac{1}{2} (L^T\mathbf{r}(\mathbf{x}))^T (L^T \mathbf{r}(\mathbf{x})) xmin21r(x)TΣ−1r(x)=xmin21r(x)TLLTr(x)=xmin21(LTr(x))T(LTr(x))
因此可以使用Cholesky分解（https://blog.csdn.net/ergevv/article/details/139260380）将 Σ − 1 \Sigma^{-1} Σ−1分解成 L L T LL^T LLT，则残差 r ( x ) \mathbf{r}(\mathbf{x}) r(x)变成了 L T r ( x ) L^T \mathbf{r}(\mathbf{x}) LTr(x)。
重新推导高斯牛顿验证一下：
r ( x ) ′ = L T r ( x ) \mathbf{r}(\mathbf{x}) '=L^T \mathbf{r}(\mathbf{x}) r(x)′=LTr(x)
L T r ( x k + Δ x ) ≈ L T r ( x k ) + L T J ( x k ) Δ x L^T\mathbf{r}(\mathbf{x}_k + \Delta \mathbf{x}) \approx L^T\mathbf{r}(\mathbf{x}_k) + L^TJ(\mathbf{x}_k) \Delta \mathbf{x} LTr(xk+Δx)≈LTr(xk)+LTJ(xk)Δx
则
min x k 1 2 ( L T r ( x k ) + L T J ( x k ) Δ x ) T ( L T r ( x k ) + L T J ( x k ) Δ x ) \min_{\mathbf{x_k}} \frac{1}{2} (L^T\mathbf{r}(\mathbf{x}_k) + L^TJ(\mathbf{x}_k) \Delta \mathbf{x})^T (L^T\mathbf{r}(\mathbf{x}_k) + L^TJ(\mathbf{x}_k) \Delta \mathbf{x}) xkmin21(LTr(xk)+LTJ(xk)Δx)T(LTr(xk)+LTJ(xk)Δx)
对上式关于 Δ x \Delta \mathbf{x} Δx 求导并令导数为零，可以得到正规方程：
( L T J ( x k ) ) T L T J ( x k ) Δ x = − ( L T J ( x k ) ) T L T r ( x k ) (L^TJ(\mathbf{x}_k))^T L^TJ(\mathbf{x}_k) \Delta \mathbf{x} = -(L^TJ(\mathbf{x}_k))^T L^T\mathbf{r}(\mathbf{x}_k) (LTJ(xk))TLTJ(xk)Δx=−(LTJ(xk))TLTr(xk)
则
J ( x k ) T L L T J ( x k ) Δ x = − J ( x k ) T L L T r ( x k ) J(\mathbf{x}_k)^T LL^TJ(\mathbf{x}_k) \Delta \mathbf{x} = -J(\mathbf{x}_k)^T LL^T\mathbf{r}(\mathbf{x}_k) J(xk)TLLTJ(xk)Δx=−J(xk)TLLTr(xk)
即
J ( x k ) T Σ − 1 J ( x k ) Δ x = − J ( x k ) T Σ − 1 r ( x k ) J(\mathbf{x}_k)^T \Sigma^{-1} J(\mathbf{x}_k) \Delta \mathbf{x} = -J(\mathbf{x}_k)^T \Sigma^{-1} \mathbf{r}(\mathbf{x}_k) J(xk)TΣ−1J(xk)Δx=−J(xk)TΣ−1r(xk)
2、点云和imu之间的约束：
E θ p c = ( q k + 1 p c ) − 1 ∗ q k + 1 p r e E p p c = p k + 1 p r e − p k + 1 p c \begin{align*} E^{pc}\theta &= (q^{pc}{k+1})^{-1} * q^{pre}{k+1}\\ E^{pc}p &= p^{pre}{k+1} - p^{pc}{k+1}\\ \end{align*} EθpcEppc=(qk+1pc)−1∗qk+1pre=pk+1pre−pk+1pc
这里就不分析残差对变量的雅可比矩阵了，因为ceres和g2o都支持自动求导