【VIO】第2讲 基于优化的IMU

第2讲 基于优化的 IMU 与视觉信息融合

1.最小二乘问题求解

（1）最小二乘基础概念

​ 1 定义：找到一个n维的变量 $x\in R^n$ ，使得损失函数 $F(x)$ 取得局部最小值：
$$F(x)=\frac{1}{2}\sum _{i=1}^m(f_i(x){)}^2$$

2 cost function 泰勒展开

$$F(x+\Delta x)=F(x)+J\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x+O(||\Delta x|{|}^3)$$

​ 3 迭代法

​ 找一个下降方向使得损失函数随x的迭代逐渐减小，直到x收敛到 $x^{\ast }$：
$$F(x_{k+1})<F(x_k)$$
​ 第一步：找下降方向单位向量 d，第二步：确定下降步长 $\alpha$

​ 假设 $\alpha$ 足够小，对 $F(x)$ 进行一阶泰勒展开：
$$F(x+\alpha d)\approx F(x)+\alpha Jd$$
​ 所以只需要寻找下降方向，满足：
$$Jd<0$$

（2）最速下降法和牛顿法

​ 1）最速下降法

​ 下降方向的条件：$Jd = ||J||cos\theta $ ，$\theta$ 表示下降方向和梯度方向的夹角。当 $\theta =\pi$时：
$$d=\frac{-J^T}{||J||}$$
​ 即梯度的负方向为最速下降方向。

​ 2）牛顿法
$$\begin{gathered} J+H\Delta x=0 \\ =>H\Delta x=-J \end{gathered}$$
​ 求解线性方程，便得到了增量。

​ 3）阻尼法

​ 记：
$$F(x+\Delta x)\approx L(\Delta x)\equiv F(x)+J\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x$$
​ 求以下函数的最小化：
Δx≡arg  min{L(Δx)+12μΔxTΔx} \Delta x \equiv arg\;min\{L(\Delta x) + \frac{1}{2}\mu \Delta x^T \Delta x \} Δx≡argmin{L(Δx)+21​μΔxTΔx}

求导，并整理：

$$(H+\mu I)\Delta x=-J^T$$

（3）高斯牛顿法

$$\begin{gathered} (J^{T}J)\Delta x=-J^{T}f \\ =>H\Delta x=b \end{gathered}$$

（4）LM

​ 对高斯牛顿法进行了改进，求解过程中引入了阻尼因子：
$$(J^{T}J+\mu I)\Delta x=-J^{T}f\mu >0$$
​ 阻尼因子 $\mu$ 的作用：

​ 1 $\mu >0$ 保证迭代朝着下降方向

​ 2 $\mu$ 非常大，则 $\Delta x=-\frac{1}{\mu }{J}^{T}f=-\frac{1}{\mu }{F}^{\prime }(x{)}^T$，接近最速下降法

​ 3 $\mu$ 非常小，则 接近高斯牛顿法

​ 简单的 ${\mu }_0$ 初始值选取策略：
$${\mu }_0=\tau max\{(J^{T}J{)}_{ii}\}$$
​ 通常，$\tau \in [10^{-8},1]$

​ 阻尼因子 $\mu$ 的更新策略：

​ 定义比例因子
$$\begin{gathered} \rho =\frac{F(x)-F(x+\Delta x)}{L(0)-L(\Delta x)} \\ L(0)-L(\Delta x)=\frac{1}{2}\Delta x^T(\mu \Delta x-J^{T}f) \end{gathered}$$
​ Marquardt 策略
$$\begin{gathered} if\rho <0.25 \\ \mu :=u\ast 2 \\ elseif\rho >0.75 \\ \mu :=\mu /3 \end{gathered}$$
​ Nielsen策略
if    ρ>0μ:=u∗max{13,1−(2ρ−1)3};    v:=2else                  μ:=μ∗3;v:=2∗v if \;\; \rho > 0\\ \mu :=u * max\{\frac{1}{3}, 1-(2\rho-1)^3 \}; \;\; v :=2\\ else\;\;\;\;\;\;\;\;\;\\ \mu :=\mu * 3; v:=2*v ifρ>0μ:=u∗max{31​,1−(2ρ−1)3};v:=2elseμ:=μ∗3;v:=2∗v
​ Nielsen策略实现代码：

/// LM
void Problem::ComputeLambdaInitLM() {
    ni_ = 2.;
    currentLambda_ = -1.;
    currentChi_ = 0.0;
    // TODO:: robust cost chi2
    for (auto edge: edges_) {
        currentChi_ += edge.second->Chi2();
    }
    if (err_prior_.rows() > 0)
        currentChi_ += err_prior_.norm();

    stopThresholdLM_ = 1e-6 * currentChi_;          // 迭代条件为 误差下降 1e-6 倍

    double maxDiagonal = 0;
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    for (ulong i = 0; i < size; ++i) {
        maxDiagonal = std::max(fabs(Hessian_(i, i)), maxDiagonal);
    }
    double tau = 1e-5;
    currentLambda_ = tau * maxDiagonal;
}

void Problem::AddLambdatoHessianLM() {
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    for (ulong i = 0; i < size; ++i) {
        Hessian_(i, i) += currentLambda_;
    }
}

void Problem::RemoveLambdaHessianLM() {
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    // TODO:: 这里不应该减去一个，数值的反复加减容易造成数值精度出问题？而应该保存叠加lambda前的值，在这里直接赋值
    for (ulong i = 0; i < size; ++i) {
        Hessian_(i, i) -= currentLambda_;
    }
}

bool Problem::IsGoodStepInLM() {
    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);
    scale += 1e-3;    // make sure it's non-zero :)

    // recompute residuals after update state
    // 统计所有的残差
    double tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }

    double rho = (currentChi_ - tempChi) / scale;
    if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
    {
        double alpha = 1. - pow((2 * rho - 1), 3);
        alpha = std::min(alpha, 2. / 3.);
        double scaleFactor = (std::max)(1. / 3., alpha);
        currentLambda_ *= scaleFactor;
        ni_ = 2;
        currentChi_ = tempChi;
        return true;
    } else {
        currentLambda_ *= ni_;
        ni_ *= 2;
        return false;
    }
}

（5）鲁棒核函数

2. VIO残差函数的构建

（1）系统需要优化的状态量

$$\begin{gathered} \chi =[x_n,x_{n+1},\cdots ,x_{n+N},{\lambda }_m,{\lambda }_{m+1},{\lambda }_{m+M}] \\ {x}_i=[p_{w{b}_i},q_{w{b}_i},v_i^w,b_a^{b_i},b_g^{b_i}]，i\in [n,n+N] \end{gathered}$$

其中： $x_i$ 包含 $i$ 时刻IMU机体在惯性坐标系中的位置，姿态，速度以及IMU机体坐标系中的加速度和角速度的偏置量估计；

​ $n,m$ 分别是机体状态量，路标在滑动窗口里的起始时刻；

​ N 滑动窗口中关键帧数量；

​ M 被滑动窗口内所有关键帧观测到的；路标数量。

（2）基于逆深度的重投影误差

​ 特征点逆深度在第 $i$ 帧中初始化得到，在第 $j$ 帧又被观测到，预测其在第 $j$ 帧中的坐标：
$$[x_{c_j},y_{c_j},z_{c_j},1{]}^T=T_{bc}^{-1}{T}_{w{b}_j}^{-1}{T}_{w{b}_i}{T}_{bc}[\frac{1}{\lambda }{u}_{c_i},\frac{1}{\lambda }{v}_{c_i},\frac{1}{\lambda },1{]}^T$$
​ 思考公式的含义：本质上还是相机那一套，只不过IMU的 body 坐标系充当了一个桥梁的作用。（上个时刻的计算得到的像素坐标通过上个时刻的逆深度来估计此时刻的3维世界坐标值）

（3）IMU预积分

$${\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}\underset{\alpha }{\underbrace{{\iint }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t^2}}$$

$${\mathbf{v}}_j^w={\mathbf{v}}_i^w-{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}\underset{\beta }{\underbrace{{\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t}}$$

$${\mathbf{q}}_{w{b}_j}={\mathbf{q}}_{w{b}_i}\underset{\gamma }{\underbrace{{\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t}}$$

​ 预积分量：
$${\mathbf{\alpha }}_{b_i{b}_j}={\iint }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t^2$$

$${\mathbf{\beta }}_{b_i{b}_j}={\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t$$

$${\mathbf{q}}_{b_i{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t$$

​ 基于预积分量的导航状态更新公式：

$$\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{v}}_j^w\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{b}}_j^a\\ {\mathbf{b}}_j^g\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ {\mathbf{v}}_i^w-{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{b}}_i^a\\ {\mathbf{b}}_i^g\end{array}\right]$$

（4）预积分误差

（5）预积分的离散形式

​ 采用中值积分法
$$\mathbf{\omega }=\frac{1}{2}\left[\left({\mathbf{\omega }}^{b_k}-{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}-{\mathbf{b}}_k^g\right)\right]$$

$$\mathbf{a}=\frac{1}{2}\left[{\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]$$

$${\mathbf{\alpha }}_{b_i{b}_{k+1}}={\mathbf{\alpha }}_{b_i{b}_k}+{\mathbf{\beta }}_{b_i{b}_k}\delta t+\frac{1}{2}\mathbf{a}\delta t^2$$

$${\mathbf{\beta }}_{b_i{b}_{k+1}}={\mathbf{\beta }}_{b_i{b}_k}+\mathbf{a}\delta t$$

$${\mathbf{q}}_{b_i{b}_{k+1}}={\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\delta t\end{array}\right]$$

$$b_{k+1}^a=b_k^a+n_{b_k^a}\delta t$$

$$b_{k+1}^g=b_k^g+n_{b_k^g}\delta t$$

（6）预积分量的方差

误差的传递由两部分组成：当前时刻的误差传递给下一时刻，当前时刻的测量噪声传递给下一时刻。

假设已知相邻时刻误差的线性传递方程：
$${\eta }_{ik}=F_{k-1}{\eta }_{ik-1}+G_{k-1}{n}_{k-1}$$
其中：状态误差为 ${\eta }_{ik}=[\delta {\theta }_{ik},\delta v_{ik},\delta p_{ik}]$，测量噪声为 $n_k=[n_K^g,n_k^a]$

预积分量的协方差矩阵通过递推得到：
$${\mathbf{\Sigma }}_{i,k}={\mathbf{F}}_{k-1}{\mathbf{\Sigma }}_{i,k-1}{\mathbf{F}}_{k-1}^{\top }+{\mathbf{G}}_{k-1}{\mathbf{\Sigma }}_{\mathbf{n}}{\mathbf{G}}_{k-1}^{\top }$$
​ 其中：${\Sigma }_n$ 为测量噪声的协方差矩阵，方差从 $i$ 时刻开始递推，F,G是离散时间下的状态转移矩阵

​ 现在需要求F和G

（7）预积分微分方程

​ 基于误差时间变化的递推方程

​ 已知连续时间下的微分方程形式为：
$$\dot{\mathbf{X}}={\mathbf{F}}_t\mathbf{X}+{\mathbf{G}}_t\mathbf{N}$$
其中：
$$\mathbf{X}=\left[\begin{array}{l}\delta {\mathbf{\alpha }}_t^{b_k}\\ \delta {\mathbf{\theta }}_t^{b_k}\\ \delta {\mathbf{\beta }}_t^{b_k}\\ \delta {\mathbf{b}}_{a_t}\\ \delta {\mathbf{b}}_{w_t}\end{array}\right]$$

$$\mathbf{N}=\left[\begin{array}{l}{\mathbf{n}}_a\\ {\mathbf{n}}_w\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]$$

​ 不推了，暂时只记结论：
$$\delta \dot{\mathbf{\theta }}=-{\left[{\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{{\omega }_t}$$

$$\begin{array}{r}\delta \dot{\mathbf{\beta }}={\mathbf{R}}_t[\delta \mathbf{\theta }{]}_{\times }\left({\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right)+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_{a_t}\right)\\ =-{\mathbf{R}}_t{\left[{\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_{a_t}\right)\end{array}$$

$$\delta \dot{\mathbf{\alpha }}=\delta \mathbf{\beta }$$

（8）预积分离散时间递推方程

$${\mathbf{X}}_{k+1}={\mathbf{F}}_k{\mathbf{X}}_k+{\mathbf{G}}_k{\mathbf{N}}_k$$

其中：
$${\mathbf{X}}_{k+1}=\left[\begin{array}{c}\delta {\mathbf{\alpha }}_{k+1}\\ \delta {\mathbf{\theta }}_{k+1}\\ \delta {\mathbf{\beta }}_{k+1}\\ \delta {\mathbf{b}}_{a_{k+1}}\\ \delta {\mathbf{b}}_{{\omega }_{k+1}}\end{array}\right]{\mathbf{X}}_k=\left[\begin{array}{c}\delta {\mathbf{\alpha }}_k\\ \delta {\mathbf{\theta }}_k\\ \delta {\mathbf{\beta }}_k\\ \delta {\mathbf{b}}_{a_k}\\ \delta {\mathbf{b}}_{{\omega }_k}\end{array}\right]{\mathbf{N}}_k=\left[\begin{array}{c}{\mathbf{n}}_{a_k}\\ {\mathbf{n}}_{w_k}\\ {\mathbf{n}}_{a_{k+1}}\\ {\mathbf{n}}_{w_{k+1}}\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]$$
​ 结论：
$$\delta {\mathbf{\theta }}_{k+1}=\left[\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right]\delta {\mathbf{\theta }}_k+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_k}+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_{k+1}}-\delta t\delta {\mathbf{b}}_{{\omega }_k}$$

$$\begin{array}{rl}\delta {\mathbf{\alpha }}_{k+1}=& \delta {\mathbf{\alpha }}_k+\delta {\dot{\mathbf{\alpha }}}_k\delta t\\ =& \delta {\mathbf{\alpha }}_k+\delta t\delta {\mathbf{\beta }}_k\\ & -\frac{\delta t^2}{4}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t^3}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{\delta t^2}{4}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{\delta t^2}{4}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$

$$\begin{array}{rl}\delta {\mathbf{\beta }}_{k+1}=& \delta {\mathbf{\beta }}_k+\delta {\dot{\mathbf{\beta }}}_k\delta t\\ =& \delta {\mathbf{\beta }}_k\\ & -\frac{\delta t}{2}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t^2}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{\delta t}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{\delta t}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{\delta t}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$

（9）F，G

$${\mathbf{F}}_k=\left[\begin{array}{ccccc}\mathbf{I}& {\mathbf{f}}_{12}& \mathbf{I}\delta t& -\frac{1}{4}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta t^2& {\mathbf{f}}_{15}\\ 0& \mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t& 0& 0& -\mathbf{I}\delta t\\ 0& {\mathbf{f}}_{32}& \mathbf{I}& -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta t& {\mathbf{f}}_{35}\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]$$

$$\begin{array}{rl}& {\mathbf{f}}_{12}=-\frac{\delta t^2}{4}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\\ & {\mathbf{f}}_{15}=\frac{\delta t^3}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & {\mathbf{f}}_{32}=-\frac{\delta t}{2}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\\ & {\mathbf{f}}_{35}=\frac{\delta t^2}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\end{array}$$

$${\mathbf{G}}_k=\left[\begin{array}{cccccc}\frac{1}{4}{\mathbf{R}}_k\delta t^2& {\mathbf{g}}_{12}& \frac{1}{4}{\mathbf{R}}_{k+1}\delta t^2& {\mathbf{g}}_{14}& 0& amp;0\\ 0& \frac{1}{2}\mathbf{I}\delta t& 0& \frac{1}{2}\mathbf{I}\delta t& 0& 0\\ \frac{1}{2}{\mathbf{R}}_k\delta t& {\mathbf{g}}_{32}& \frac{1}{2}{\mathbf{R}}_{k+1}\delta t& {\mathbf{g}}_{34}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}\delta t& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\delta t\end{array}\right]$$

$$\begin{array}{rl}& {\mathbf{g}}_{12}=-\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{14}=-\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{32}=-\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{34}=-\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\end{array}$$

计算得到矩阵$F_k$和$G_k$后，即可按照上述公式来计算方差。

3.预积分残差关于待求状态量的雅可比

在优化时，需要求出残差对于求解的状态量的雅可比。

已知基于预积分量的状态更新如下：
$$\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{v}}_j^w\\ {\mathbf{b}}_j^a\\ {\mathbf{b}}_j^g\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{v}}_i^w-{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_i^a\\ {\mathbf{b}}_i^g\end{array}\right]$$
把上式左侧状态移到右侧，在理想的情况下左侧应该只剩下0，但是由于误差的存在，可以使用残差小量r代替，因此有：
$$\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{bg}\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}-{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2-{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t-{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_j^a-{\mathbf{b}}_i^a\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]$$
更加复杂。。。。

练习2

样例代码给出使用LM算法估计曲线 $y=e^{(a{x}^2+bx+c)}$ 参数a,b,c 的完整过程。

（1）绘制样例代码中LM阻尼因子 $\mu$ 随迭代变化的曲线图。

​ 运行代码，即可看到输出，将结果导入matlab即可绘制图象

（2）将曲线函数改成 $y=(a{x}^2+bx+c)$ 修改代码

（3）其他阻尼因子更新策略