>

## VIO 残差函数的构建

VIO 中残差总共由三部分组成：先验误差、视觉误差和 IMU 误差，这里不同的方差之间用各自的协方差矩阵来统一单位和权重，即各自的残差为：

\begin{aligned} r &= ||\boldsymbol{f}(x)||^2_\Sigma\\ &= \boldsymbol{f}^T(x)\Sigma^{-1}\boldsymbol{f}(x) \end{aligned}


$$||\boldsymbol{r}_p + \boldsymbol{J}_p\chi||_{\Sigma_p}^2$$


\begin{aligned} ||\boldsymbol{r}_b(\boldsymbol{z}_{b_ib_{i+1}}, \chi)||_{\Sigma_{b_ib_{i+1}}}^2 \end{aligned}


\begin{aligned} r_{f_j}^{c_i}= ||\boldsymbol{r}_f(\boldsymbol{z}_{\boldsymbol{f}_j}^{c_i}, \chi)||^2_{\Sigma_{\boldsymbol{f}_j^{c_i}}} \end{aligned}


\begin{aligned} \min_{\chi}\boldsymbol{F}(x) =& \min_{\chi}(\rho(||\boldsymbol{r}_p - \boldsymbol{J}_p\chi||_{\Sigma_p}^2)\\ &+ \sum_{i\in B}\rho(||\boldsymbol{r}_b(\boldsymbol{z}_{b_ib_{i+1}}, \chi)||_{\Sigma_{b_ib_{i+1}}}^2) \\ &+ \sum_{(i,j)\in F} \rho(||\boldsymbol{r}_f(\boldsymbol{z}_{\boldsymbol{f}_j}^{c_i}, \chi)||^2_{\Sigma_{\boldsymbol{f}_j^{c_i}}})) \end{aligned}


\begin{aligned} \chi &= \left[\boldsymbol{x}_n, \boldsymbol{x}_{n+1}, ... , \boldsymbol{x}_{n+N}, \lambda_m, \lambda_{m+1}, ... , \lambda_{m+M}\right] \\ \boldsymbol{x}_i &= \left[\boldsymbol{p}_{wb_i}, \boldsymbol{q}_{wb_i}, \boldsymbol{v}_i^w, \boldsymbol{b}_a^{b_i}, \boldsymbol{b}_g^{b_i} \right], \qquad i \in [n, n + N] \end{aligned}


• 载体的状态量包括其在惯性坐标系中的位置、速度、姿态以及 IMU 中加速度计和陀螺仪在载体坐标系下的零偏
• 路标的状态量则用逆深度来表示，关于逆深度会在后面进行分析

## 视觉重投影误差

$$\boldsymbol{r}_c = \begin{bmatrix} \frac{x}{z} - u\\ \frac{y}{z} - v \end{bmatrix}$$


$$\begin{bmatrix} x\\y\\z \end{bmatrix} = \frac{1}{\lambda} \begin{bmatrix} u\\v\\1 \end{bmatrix}$$


\begin{aligned} \boldsymbol{p}_{c_j} &= \boldsymbol{T}_{c_jc_i}\boldsymbol{p}_{c_i}\\ \begin{bmatrix} x_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1 \end{bmatrix} &= \boldsymbol{T}_{bc}^{-1}\boldsymbol{T}_{wb_j}^{-1}\boldsymbol{T}_{wb_i}\boldsymbol{T}_{bc} \begin{bmatrix} \frac{1}{\lambda} u_{c_i}\\ \frac{1}{\lambda} v_{c_i}\\ \frac{1}{\lambda}\\ 1\\ \end{bmatrix} \end{aligned}


$$\boldsymbol{r}_c = \begin{bmatrix} \frac{x_{c_j}}{z_{c_j}} - u_{c_j}\\ \frac{y_{c_j}}{z_{c_j}} - v_{c_j} \end{bmatrix}$$


## IMU 误差

### 计算方法

\begin{aligned} \boldsymbol{q}_{wb_j} &=\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix})}\delta t\\ \boldsymbol{v}^w_j &= \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t\\ \boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_i}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t^2 \end{aligned}


• 在描述一个待优化的状态量时，会使用四元数，这样做的好处是我们只需要更新三个参数（四元数的独立参数为 3 个，而旋转矩阵 为 8 个）
• 在使用姿态来对点或者向量进行旋转时，以及部分求导场景时，会使用旋转矩阵来描述姿态，这样做的好处是旋转矩阵的旋转操作遵循通常的矩阵运算，在计算或者求导时相对方便
• 在需要旋转向量时将作为状态量的四元数进行转换即可。

### IMU 预积分

$$\boldsymbol{q}_{wb_{100}} =\int_{t\in[0,100]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix})}\delta t$$


\begin{aligned} \boldsymbol{q}_{wb_{100}} &= \boldsymbol{q}_{wb_{0}}\otimes\boldsymbol{q}_{b_0b_{100}}\\ &= \boldsymbol{q}_{wb_{0}}\otimes\int_{t\in[0,100]}{(\boldsymbol{q}_{b_0b_t}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix})}\delta t \end{aligned}


\begin{aligned} \boldsymbol{q}_{wb_j} &=\boldsymbol{q}_{wb_i}\otimes\int_{t\in[i,j]}{(\boldsymbol{q}_{b_ib_t}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix})}\delta t \end{aligned}


\begin{aligned} \boldsymbol{v}^w_j &= \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a}^w - \boldsymbol{g}^w)}\delta t\\ &= \boldsymbol{v}^w_i - \boldsymbol{g}^w\Delta t + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a}^w)}\delta t \\ &= \boldsymbol{v}^w_i - \boldsymbol{g}^w\Delta t + \boldsymbol{R}_{wb_i}\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t \end{aligned}


\begin{aligned} \boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_i}\boldsymbol{a}^w - \boldsymbol{g}^w)}\delta t^2\\ &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t - \frac{1}{2}\boldsymbol{g}^w\Delta t^2 + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a}^w)}\delta t^2 \\ &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t - \frac{1}{2}\boldsymbol{g}^w\Delta t^2 + \boldsymbol{R}_{wb_i}\int\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t^2 \end{aligned}


\begin{aligned} \boldsymbol{q}_{wb_j} &=\boldsymbol{q}_{wb_i}\otimes\int_{t\in[i,j]}{(\boldsymbol{q}_{b_ib_t}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix})}\delta t\\ \boldsymbol{v}^w_j &= \boldsymbol{v}^w_i - \boldsymbol{g}^w\Delta t + \boldsymbol{R}_{wb_i}\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t\\ \boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t - \frac{1}{2}\boldsymbol{g}^w\Delta t^2 + \boldsymbol{R}_{wb_i}\int\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t^2 \end{aligned}


\begin{aligned} \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_j} &= \int_{t\in[i,j]}{(\boldsymbol{q}_{b_ib_t}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix})}\delta t\\ \boldsymbol{{\boldsymbol{\beta}}}_{b_ib_j} &= \int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t\\ \boldsymbol{{\boldsymbol{\alpha}}}_{b_ib_j} &= \int\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t^2 \end{aligned}


$$\begin{bmatrix} \boldsymbol{p}_{wb_j}\\ \boldsymbol{v}^w_j\\ \boldsymbol{q}_{wb_j}\\ \boldsymbol{b}_j^a\\ \boldsymbol{b}_j^g \end{bmatrix} = \begin{bmatrix} \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t - \frac{1}{2}\boldsymbol{g}^w\Delta t^2 + \boldsymbol{R}_{wb_i}\boldsymbol{{\boldsymbol{\alpha}}}_{b_ib_j}\\ \boldsymbol{v}^w_i - \boldsymbol{g}^w\Delta t + \boldsymbol{R}_{wb_i}\boldsymbol{{\boldsymbol{\beta}}}_{b_ib_j}\\ \boldsymbol{q}_{wb_i}\otimes\boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_j}\\ \boldsymbol{b}_i^a\\ \boldsymbol{b}_i^g \end{bmatrix}$$


### 计算 IMU 预积分量

• 预积分的计算方法：如何对一个时间段内的原始测量值进行累积（积分）得到预积分量
• 预积分的观测模型：我们使用预积分作为 IMU 的观测量，因此需要像别的观测一样构建观测模型（方程），构建形如 $z = h(x, \omega)$ 的方程
• 使用预积分构建约束：利用观测模型和实际观测的预积分构建残差方程（约束）

#### 预积分计算方法

\begin{aligned} \boldsymbol{\omega} &= \frac{1}{2}((\boldsymbol{\omega}^{b_k}-\boldsymbol{b}^g_k)+(\boldsymbol{\omega}^{b_{k+1}}-\boldsymbol{b}^g_{k+1}))\\ \boldsymbol{a} &= \frac{1}{2}(\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k} -\boldsymbol{b}^g_k) + \boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}} -\boldsymbol{b}^g_{k+1})) \end{aligned}


\begin{aligned} {\boldsymbol{\alpha}}_{b_ib_{k+1}} &= {\boldsymbol{\alpha}}_{b_ib_k} + {\boldsymbol{\beta}}_{b_ib_k}\delta t + \frac{1}{2}\boldsymbol{a}\delta t^2\\ {\boldsymbol{\beta}}_{b_ib_{k+1}} &= {\boldsymbol{\beta}}_{b_ib_k} + \boldsymbol{a}\delta t\\ \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_{k+1}}&= \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_k} \otimes\begin{bmatrix} 1\\ \frac{1}{2}\boldsymbol{\omega}\delta t \end{bmatrix} \end{aligned}


\begin{aligned} {\boldsymbol{\alpha}}_{b_ib_{k}} &= \bar{{\boldsymbol{\alpha}}}_{b_ib_k} + J^{\boldsymbol{\alpha}}_{b^a_{i}}\delta\boldsymbol{b}_{b_i}^a + J^{\boldsymbol{\alpha}}_{b^g_{i}}\delta\boldsymbol{b}_{b_i}^g\\ {\boldsymbol{\beta}}_{b_ib_{k}} &= \bar{{\boldsymbol{\beta}}}_{b_ib_k} + + J^{\boldsymbol{\beta}}_{b^a_{i}}\delta\boldsymbol{b}_{b_i}^a + J^{\boldsymbol{\beta}}_{b^g_{i}}\delta\boldsymbol{b}_{b_i}^g\\ \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_{k}}&= \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_k} \otimes\begin{bmatrix} 1\\ \frac{1}{2}J^{\boldsymbol{\gamma}}_{b^g_{i}}\delta\boldsymbol{b}_{b_i}^g \end{bmatrix} \end{aligned}


\begin{aligned} J^{\boldsymbol{\alpha}}_{b^a_{i}} &= \frac{\partial{\boldsymbol{\alpha}}_{b_ib_j}}{\partial\boldsymbol{b}_i^a}\\ J^{\boldsymbol{\alpha}}_{b^g_{i}} &= \frac{\partial{\boldsymbol{\alpha}}_{b_ib_j}}{\partial\boldsymbol{b}_i^g}\\ J^{\boldsymbol{\beta}}_{b^a_{i}} &= \frac{\partial{\boldsymbol{\beta}}_{b_ib_j}}{\partial\boldsymbol{b}_i^a}\\ J^{\boldsymbol{\beta}}_{b^g_{i}} &= \frac{\partial{\boldsymbol{\beta}}_{b_ib_j}}{\partial\boldsymbol{b}_i^g}\\ J^{\boldsymbol{\gamma}}_{b^g_{i}} &= \frac{\partial{\boldsymbol{\gamma}}_{b_ib_j}}{\partial\boldsymbol{b}_i^g}\\ \end{aligned}


#### 预积分观测模型

• 我们已经建立了离散时间下的状态方程，借鉴扩展卡尔曼滤波（EKF）的思路，直接通过一阶泰勒展开离散时间下的状态方程，得到离散时间下的误差递推关系
• 在连续时间下建立微分方程，推导误差的递推关系，在对其进行离散化

$$\begin{bmatrix} \tilde{\boldsymbol{\alpha}}_{{b_k}b_{k+1}}\\ \tilde{\boldsymbol{\beta}}_{{b_k}b_{k+1}}\\ \tilde{\boldsymbol{\gamma}}_{{b_k}b_{k+1}}\\ \boldsymbol{0}\\ \boldsymbol{0}\\ \end{bmatrix} = \begin{bmatrix} \boldsymbol{R}_{b_kw}(\boldsymbol{p}_{wb_{k+1}} - \boldsymbol{p}_{wb_{k}} + \frac{1}{2}\boldsymbol{g}^w\Delta t^2 - \boldsymbol{v}^w_{b_k}\Delta t)\\ \boldsymbol{R}_{b_kw}(\boldsymbol{v}^w_{b_{k+1}} + \boldsymbol{g}^w\Delta t - \boldsymbol{v}^w_{b_k})\\ \boldsymbol{q}_{wb_{k}}^{-1}\otimes\boldsymbol{q}_{wb_{k+1}}\\ \boldsymbol{b}^a_{b_{k+1}} - \boldsymbol{b}^a_{b_{k}}\\ \boldsymbol{b}^g_{b_{k+1}} - \boldsymbol{b}^g_{b_{k}}\\ \end{bmatrix}$$


#### IMU 预积分误差构建

\begin{aligned} \begin{bmatrix} \boldsymbol{r}_p\\ \boldsymbol{r}_v\\ \boldsymbol{r}_q\\ \boldsymbol{r}_{ba}\\ \boldsymbol{r}_{bg}\\ \end{bmatrix}_{15\times 1} &= \begin{bmatrix} \delta\boldsymbol{p}_{b_ib_j}\\ \delta\boldsymbol{v}_{b_ib_j}\\ \delta\boldsymbol{\theta}_{b_ib_j}\\ \delta\boldsymbol{b}^a_{b_ib_j}\\ \delta\boldsymbol{b}^g_{b_ib_j}\\ \end{bmatrix}\\ &= \begin{bmatrix} \boldsymbol{R}_{b_iw}(\boldsymbol{p}_{wb_j} - \boldsymbol{p}_{wb_i} - \boldsymbol{v}^w_i\Delta t + \frac{1}{2}\boldsymbol{g}^w\Delta t^2) - {\boldsymbol{\alpha}}_{b_ib_j}\\ \boldsymbol{R}_{b_iw}(\boldsymbol{v}_j^w - \boldsymbol{v}^w_i + \boldsymbol{g}^w\Delta t) - {\boldsymbol{\beta}}_{b_ib_j}\\ 2\left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes(\boldsymbol{q}_{b_iw}\otimes\boldsymbol{q}_{wb_j})\right]_{xyz}\\ \boldsymbol{b}_j^a - \boldsymbol{b}_i^a\\ \boldsymbol{b}_j^g - \boldsymbol{b}_i^g \end{bmatrix} \end{aligned}


## IMU 预积分误差的方差的两种推导方法

### 基于离散系统推导线性递推关系

$$\boldsymbol{x}_{k+1} = f(\boldsymbol{x}_{k}, \boldsymbol{u}_{k})$$


\begin{aligned} \boldsymbol{x}_{k+1} &= f(\boldsymbol{x}_{k}, \boldsymbol{u}_{k})\\ \tilde{\boldsymbol{x}}_{k+1} + \delta \boldsymbol{x}_{k+1} &= f(\tilde{\boldsymbol{x}}_{k} + \delta\boldsymbol{x}_{k}, \tilde{\boldsymbol{u}}_{k} + \boldsymbol{n}_{k})\\ &= f(\tilde{\boldsymbol{x}}_{k}, \tilde{\boldsymbol{u}}_{k}) + \boldsymbol{F}\delta\boldsymbol{x}_{k} + \boldsymbol{G}\boldsymbol{n}_{k}\\ &= \tilde{\boldsymbol{x}}_{k+1} + \boldsymbol{F}\delta\boldsymbol{x}_{k} + \boldsymbol{G}\boldsymbol{n}_{k}\\ \Rightarrow \delta \boldsymbol{x}_{k+1} &= \boldsymbol{F}\delta\boldsymbol{x}_{k} + \boldsymbol{G}\boldsymbol{n}_{k} \end{aligned}


\begin{aligned} \boldsymbol{\omega} &= \frac{1}{2}((\bar{\boldsymbol{\omega}}^{b_k} + \boldsymbol{n}_k^g -\boldsymbol{b}^g_k)+(\bar{\boldsymbol{\omega}}^{b_{k+1}} + \boldsymbol{n}_{k+1}^g -\boldsymbol{b}^g_{k+1}))\\ \boldsymbol{a} &= \frac{1}{2}(\boldsymbol{R}_{b_ib_k}(\bar{\boldsymbol{a}}^{b_k} + \boldsymbol{n}_k^a -\boldsymbol{b}^a_k) + \boldsymbol{R}_{b_ib_{k+1}}(\bar{\boldsymbol{a}}^{b_{k+1}} + \boldsymbol{n}_{k+1}^a -\boldsymbol{b}^a_{k+1}))\\ {\boldsymbol{\alpha}}_{b_ib_{k+1}} &= {\boldsymbol{\alpha}}_{b_ib_k} + {\boldsymbol{\beta}}_{b_ib_k}\delta t + \frac{1}{2}\boldsymbol{a}\delta t^2\\ {\boldsymbol{\beta}}_{b_ib_{k+1}} &= {\boldsymbol{\beta}}_{b_ib_k} + \boldsymbol{a}\delta t\\ \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_{k+1}}&= \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_k} \otimes\begin{bmatrix} 1\\ \frac{1}{2}\boldsymbol{\omega}\delta t \end{bmatrix}\\ \boldsymbol{b}_{k+1}^a &= \boldsymbol{b}_{k}^a + \boldsymbol{n}_{b_k^a}\Delta t\\ \boldsymbol{b}_{k+1}^g &= \boldsymbol{b}_{k}^g + \boldsymbol{n}_{b_k^g}\Delta t \end{aligned}


### 基于连续系统推导线性递推关系

$$\dot{\boldsymbol{v}} = \boldsymbol{Ra}^b + \boldsymbol{g}$$


\begin{aligned} \dot{\boldsymbol{v}} + \dot{\delta \boldsymbol{v}} &= \boldsymbol{R}(\boldsymbol{I} + (\delta\boldsymbol{\theta})^\wedge)(\boldsymbol{a}^b+\delta\boldsymbol{a}^b) + \boldsymbol{g}+ \delta\boldsymbol{g}\\ \dot{\boldsymbol{v}} + \dot{\delta \boldsymbol{v}} &= \boldsymbol{Ra}^b + \boldsymbol{g} + \boldsymbol{R}\delta\boldsymbol{a}^b + \boldsymbol{R}(\delta\boldsymbol{\theta})^\wedge(\boldsymbol{a}^b+\delta\boldsymbol{a}^b)+ \delta\boldsymbol{g}\\ \Rightarrow \dot{\delta \boldsymbol{v}} &= \boldsymbol{R}\delta\boldsymbol{a}^b +\boldsymbol{R}(\delta\boldsymbol{\theta})^\wedge(\boldsymbol{a}^b+\delta\boldsymbol{a}^b)+ \delta\boldsymbol{g}\\ &\approx\boldsymbol{R}\delta\boldsymbol{a}^b +\boldsymbol{R}(\delta\boldsymbol{\theta})^\wedge\boldsymbol{a}^b+ \delta\boldsymbol{g}\\ &= \boldsymbol{R}\delta\boldsymbol{a}^b - \boldsymbol{R}(\boldsymbol{a}^b)^\wedge\delta\boldsymbol{\theta}+ \delta\boldsymbol{g} \end{aligned}


$$\dot{\delta\boldsymbol{x}} = \boldsymbol{A}\delta\boldsymbol{\boldsymbol{x}} + \boldsymbol{B}\delta\boldsymbol{n}$$


\begin{aligned} \dot{\delta\boldsymbol{x}}_k &= \boldsymbol{A}\delta\boldsymbol{x}_k + \boldsymbol{B}\delta\boldsymbol{n}_k\\ \Rightarrow \delta\boldsymbol{x}_{k+1} &= \delta\boldsymbol{x}_k + \dot{\delta\boldsymbol{x}}_k\Delta t\\ &= \delta\boldsymbol{x}_k + \boldsymbol{A}\Delta t\delta\boldsymbol{x}_k + \boldsymbol{B}\Delta t\delta\boldsymbol{n}_k\\ &= (\boldsymbol{I} + \boldsymbol{A}\Delta t)\delta\boldsymbol{x}_k + \boldsymbol{B}\Delta t\delta\boldsymbol{n}_k \end{aligned}


\begin{aligned} \boldsymbol{F} &= \boldsymbol{I} + \boldsymbol{A}\Delta t\\ \boldsymbol{G} &= \boldsymbol{B}\Delta t \end{aligned}


## 状态方程雅可比矩阵的推导

\begin{aligned} \boldsymbol{\omega} &= \frac{1}{2}((\boldsymbol{\omega}^{b_k} -\boldsymbol{b}^g_k)+(\boldsymbol{\omega}^{b_{k+1}} -\boldsymbol{b}^g_{k+1}))\\ \boldsymbol{a} &= \frac{1}{2}(\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k} -\boldsymbol{b}^a_k) + \boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}} -\boldsymbol{b}^a_{k+1}))\\ {\boldsymbol{\alpha}}_{b_ib_{k+1}} &= f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k) = {\boldsymbol{\alpha}}_{b_ib_k} + {\boldsymbol{\beta}}_{b_ib_k}\delta t + \frac{1}{2}\boldsymbol{a}\delta t^2\\ {\boldsymbol{\beta}}_{b_ib_{k+1}} &= f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k) ={\boldsymbol{\beta}}_{b_ib_k} + \boldsymbol{a}\delta t\\ \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_{k+1}}&= f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k) =\boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_k} \otimes\begin{bmatrix} 1\\ \frac{1}{2}\boldsymbol{\omega}\delta t \end{bmatrix}\\ \boldsymbol{b}_{k+1}^a &= f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k) =\boldsymbol{b}_{k}^a + \boldsymbol{n}_{b_k^a}\Delta t\\ \boldsymbol{b}_{k+1}^g &= f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k) =\boldsymbol{b}_{k}^g + \boldsymbol{n}_{b_k^g}\Delta t \end{aligned}

• $\boldsymbol{F}$ 推导

$\boldsymbol{F}$ 为状态方程 $\boldsymbol{x}_{k+1} = f(\boldsymbol{x}_k, \boldsymbol{u}_k)$ 对上一时刻状态量 $\boldsymbol{x}_k$ 的 雅可比，下面先将其形式以及比较简单的部分列出来，对不太直观的部分单独进行推导：

\begin{aligned} \boldsymbol{F} &= \frac{\partial f(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{x}_k}\\ &= \begin{bmatrix} \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \\ \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \\ \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \\ \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \\ \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \end{bmatrix}\\ &= \begin{bmatrix} \boldsymbol{I} & \boldsymbol{I}\delta t & f_{13} & f_{14} & f_{15} \\ \boldsymbol{0} & \boldsymbol{I} & f_{23} & f_{24} & f_{25} \\ \boldsymbol{0} & \boldsymbol{0} & f_{33} & \boldsymbol{0} & f_{35} \\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I} \end{bmatrix} \end{aligned}


$f_{13}$

\begin{aligned} f_{13} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}}\\ &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{R}_{b_ib_k}}\\ &= \frac{\partial (\frac{1}{2}\boldsymbol{a}\delta t^2)}{\partial\boldsymbol{R}_{b_ib_k}}\\ &= (\frac{1}{2}\delta t^2) \frac{\partial \boldsymbol{a}}{\partial\boldsymbol{R}_{b_ib_k}}\\ \frac{\partial \boldsymbol{a}}{\partial\boldsymbol{R}_{b_ib_k}} &= \frac{1}{2}(\frac{\partial\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})}{\partial\boldsymbol{R}_{b_ib_k}} + \frac{\partial\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\partial\boldsymbol{R}_{b_ib_{k}}}) \end{aligned}


$$\frac{\partial\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})}{\partial\boldsymbol{R}_{b_ib_k}} = -\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})^\wedge$$


\begin{aligned} \frac{\partial\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\partial\boldsymbol{R}_{b_ib_{k}}}) &= \frac{\partial\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\partial\boldsymbol{R}_{b_ib_{k}}}\\ &= \lim_{\boldsymbol{\psi} \rightarrow 0}\frac{\boldsymbol{R}_{b_ib_{k}}\exp{(\boldsymbol{\psi}^\wedge})\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})-\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\boldsymbol{\psi}}\\ &\approx \lim_{\boldsymbol{\psi} \rightarrow 0}\frac{\boldsymbol{R}_{b_ib_{k}}(\boldsymbol{I} + \boldsymbol{\psi}^\wedge)\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})-\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\boldsymbol{\psi}}\\ &= \lim_{\boldsymbol{\psi} \rightarrow 0}\frac{\boldsymbol{R}_{b_ib_{k}}\boldsymbol{\psi}^\wedge\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\boldsymbol{\psi}}\\ &= -\lim_{\boldsymbol{\psi} \rightarrow 0}\frac{\boldsymbol{R}_{b_ib_{k}}(\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))^\wedge\boldsymbol{\psi}}{\boldsymbol{\psi}}\\ &= -\boldsymbol{R}_{b_ib_{k}}(\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))^\wedge\\ &= -\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\exp{((\boldsymbol{\omega}\delta t)^\wedge})^T\\ &= -\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\exp{((-\boldsymbol{\omega}\delta t)^\wedge})\\ &\approx -\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge(\boldsymbol{I} -\boldsymbol{\omega}^\wedge\delta t) \end{aligned}


$$(\boldsymbol{Ra})^\wedge = \boldsymbol{Ra}^\wedge\boldsymbol{R}^T$$


\begin{aligned} f_{13} &= (\frac{1}{2}\delta t^2) \frac{\partial \boldsymbol{a}}{\partial\boldsymbol{R}_{b_ib_k}}\\ &= \frac{1}{4}(\frac{\partial\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})}{\partial\boldsymbol{R}_{b_ib_k}} + \frac{\partial\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\partial\boldsymbol{R}_{b_ib_{k}}})\delta t^2\\ &= -\frac{1}{4}(\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})^\wedge +\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k}})^\wedge(\boldsymbol{I} -\boldsymbol{\omega}^\wedge\delta t))\delta t^2 \end{aligned}


$f_{14}$

\begin{aligned} f_{14} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}}\\ &= \frac{1}{4}\delta t^2 \frac{\partial (-\boldsymbol{R}_{b_ib_k}\boldsymbol{b}_k^a)}{\partial\boldsymbol{b}^a_{b_k}} + \frac{\partial (-\boldsymbol{R}_{b_ib_{k+1}}\boldsymbol{b}_{k+1}^a)}{\partial\boldsymbol{b}^a_{b_k}}\\ &= -\frac{1}{4}\delta t^2 (\frac{\partial (\boldsymbol{R}_{b_ib_k}\boldsymbol{b}_k^a)}{\partial\boldsymbol{b}^a_{b_k}} + \frac{\partial (\boldsymbol{R}_{b_ib_{k+1}}\boldsymbol{b}_{k+1}^a)}{\partial\boldsymbol{b}^a_{b_k}})\\ &= -\frac{1}{4} (\boldsymbol{R}_{b_ib_k}+ \boldsymbol{R}_{b_ib_{k+1}})\delta t^2\\ \end{aligned}


$f_{15}$:

\begin{aligned} f_{15} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}}\\ &= \frac{1}{2}\delta t^2\frac{\partial\boldsymbol{a}}{\partial\boldsymbol{b}^g_{b_k}}\\ &= \frac{1}{4}\delta t^2 \frac{\partial(\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))}{\partial\boldsymbol{b}^g_{b_k}}\\ &= \frac{1}{4}\delta t^2\frac{\partial(\boldsymbol{R}_{b_ib_{k}}\exp{(\boldsymbol{\omega}^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))}{\partial\boldsymbol{b}^g_{b_k}}\\ \frac{\partial(\boldsymbol{R}_{b_ib_{k}}\exp{(\boldsymbol{\omega}^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))}{\partial\boldsymbol{b}^g_{b_k}} &= \lim_{\delta\boldsymbol{b}^g_{b_k}\rightarrow 0}\frac{ (\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}-\delta\boldsymbol{b}^g_{b_k})^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})) - (\boldsymbol{R}_{b_ib_{k}}\exp{(\boldsymbol{\omega}^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})) }{\delta\boldsymbol{b}^g_{b_k}}\\ \end{aligned}


\begin{aligned} & \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}-\delta\boldsymbol{b}^g_{b_k})^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}) \\ =& \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)\exp{(-(\boldsymbol{J}_r(\boldsymbol{\omega}\delta t)\delta\boldsymbol{b}^g_{b_k}\delta t)^\wedge)}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})\\ =& \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)(\boldsymbol{I} -(\boldsymbol{J}_r(\boldsymbol{\omega}\delta t)\delta\boldsymbol{b}^g_{b_k}\delta t)^\wedge))}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})\\ \approx& \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)(\boldsymbol{I} -(\boldsymbol{I}\delta\boldsymbol{b}^g_{b_k}\delta t)^\wedge))}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}) \end{aligned}


\begin{aligned} f_{15} &= \frac{1}{4}\delta t^2\lim_{\delta\boldsymbol{b}^g_{b_k}\rightarrow 0}\frac{ \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)(-(\delta\boldsymbol{b}^g_{b_k}\delta t)^\wedge))}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}) }{\delta\boldsymbol{b}^g_{b_k}}\\ &= \frac{1}{4}\delta t^2\lim_{\delta\boldsymbol{b}^g_{b_k}\rightarrow 0}\frac{ \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge(\delta\boldsymbol{b}^g_{b_k}\delta t)} }{\delta\boldsymbol{b}^g_{b_k}}\\ &= \frac{1}{4}\delta t^2 \boldsymbol{R}_{b_ib_{k}} \exp{((\boldsymbol{\omega}\delta t)^\wedge)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t\\ &= \frac{1}{4}\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^3 \end{aligned}


• $f_{23}$:

\begin{aligned} f_{23} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}}\\ &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{R}_{b_k}}\\ &= -\frac{1}{2}(\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})^\wedge +\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k}})^\wedge(\boldsymbol{I} -\boldsymbol{\omega}^\wedge\delta t))\delta t \end{aligned}

• $f_{24}$:

\begin{aligned} f_{24} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}}\\ &= -\frac{1}{2} (\boldsymbol{R}_{b_ib_k}+ \boldsymbol{R}_{b_ib_{k+1}})\delta t \end{aligned}

• $f_{25}$:

\begin{aligned} f_{25} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}}\\ &= \frac{1}{2}\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^2 \end{aligned}


• $f_{33}$:

\begin{aligned} \boldsymbol{R}_{b_ib_{k+1}}\exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge} &= \boldsymbol{R}_{b_ib_{k}}\exp{(\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\exp{(\boldsymbol{\omega}\delta t)^\wedge}\\ \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge}&= \boldsymbol{R}_{b_ib_{k+1}}^{-1}\boldsymbol{R}_{b_ib_{k}}\exp{(\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\exp{(\boldsymbol{\omega}\delta t)^\wedge}\\ \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge} &= \boldsymbol{R}_{b_{k+1}b_{k}}\exp{(\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\exp{(\boldsymbol{\omega}\delta t)^\wedge}\\ \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge} &=\boldsymbol{R}_{b_{k}b_{k+1}}^T\exp{(\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\boldsymbol{R}_{b_{k}b_{k+1}}\\ \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge} &=\exp{(\boldsymbol{R}_{b_{k}b_{k+1}}^T\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\\ \Rightarrow \delta\boldsymbol{\theta_{b_{k+1}}}^\wedge &= \boldsymbol{R}_{b_{k}b_{k+1}}^T\delta\boldsymbol{\theta_{b_{k}}}\\ \delta\boldsymbol{\theta_{b_{k+1}}} &= \exp{(-\boldsymbol{\omega}\delta t)^\wedge}\delta\boldsymbol{\theta_{b_{k}}}\\ \Rightarrow f_{33} &= \frac{\delta\boldsymbol{\theta_{b_{k+1}}}}{\delta\boldsymbol{\theta_{b_{k}}}} = \exp{(-\boldsymbol{\omega}\delta t)^\wedge}\\ &\approx \boldsymbol{I} - \boldsymbol{\omega}^\wedge\delta t \end{aligned}

• $f_{35}$:

\begin{aligned} \boldsymbol{R}_{b_ib_{k+1}}\exp{(\boldsymbol{\delta\theta_{b_{k+1}}})^\wedge} &= \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega} - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\ \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge}&=\boldsymbol{R}_{b_ib_{k+1}}^T\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega} - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\ \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge}&= \exp{(-\boldsymbol{\omega}\delta t)}\exp{((\boldsymbol{\omega} - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\ \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge}&\approx \exp{(-\boldsymbol{\omega}\delta t)}\exp{(\boldsymbol{\omega}\delta t)}\exp{(J_r(\boldsymbol{\omega}\delta t)( - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\ \exp{(\boldsymbol{\delta\theta_{b_{k+1}}})^\wedge}&\approx \exp{(( - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\ \Rightarrow \delta\boldsymbol{\theta_{b_{k+1}}} &= - \delta\boldsymbol{b}_{b_k}\delta t\\ f_{35} = \frac{\delta\boldsymbol{\theta_{b_{k+1}}}}{\delta\boldsymbol{b}^g_{b_k}} &= \boldsymbol{I}\delta t \end{aligned}

• $\boldsymbol{G}$ 推导

$\boldsymbol{G}$ 为状态方程 $\boldsymbol{x}_{k+1} = f(\boldsymbol{x}_k, \boldsymbol{u}_k)$ 对控制量 $\boldsymbol{u}_k$ 的 雅可比，同样下面先将其形式以及比较简单的部分列出来，对不太直观的部分单独进行推导：

\begin{aligned} \boldsymbol{G} &= \frac{\partial f(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{u}_k}\\ &= \begin{bmatrix} \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} & \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}\\ \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} & \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}\\ \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} & \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}\\ \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} & \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}\\ \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} & \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}} \end{bmatrix}\\ &= \begin{bmatrix} g_{11} & g_{12} & g_{13} & g_{14} & \boldsymbol{0} & \boldsymbol{0}\\ g_{21} & g_{22} & g_{23} & g_{24} & \boldsymbol{0} & \boldsymbol{0}\\ g_{31} & \boldsymbol{0} & g_{33} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I}\delta t & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I}\delta t \end{bmatrix} \end{aligned}


• $g_{11}$:

\begin{aligned} g_{11} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}}\\ &= \frac{\partial{\frac{1}{2}\boldsymbol{a}\delta t^2}}{\partial\boldsymbol{\omega}^{b_k}}\\ &= \frac{1}{4}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}\exp{(\boldsymbol{\omega}\delta t)^\wedge}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}})}{\partial\boldsymbol{\omega}^{b_k}}\\ &= -\frac{1}{4}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}})^\wedge(\boldsymbol{\omega}\delta t)}{\partial\boldsymbol{\omega}^{b_k}}\\ &= -\frac{1}{4}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}})^\wedge(\boldsymbol{\omega}\delta t)}{\partial\boldsymbol{\omega}^{b_k}}\\ &= -\frac{1}{8}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}})^\wedge(\boldsymbol{\omega}^{b_k}\delta t)}{\partial\boldsymbol{\omega}^{b_k}}\\ &= -\frac{1}{8}\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^3 \end{aligned}

• $g_{12}$:

\begin{aligned} g_{12} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}}\\ &= \frac{\partial{\frac{1}{2}\boldsymbol{a}\delta t^2}}{\partial\boldsymbol{a}^{b_k}}\\ &= \frac{1}{4}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}\boldsymbol{a}^{b_k}}}{\partial\boldsymbol{a}^{b_k}}\\ &= \frac{1}{4}\boldsymbol{R}_{b_ib_k}\delta t^2 \end{aligned}

• $g_{13}$:

\begin{aligned} g_{13} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}}\\ &= -\frac{1}{8}\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^3\\ &= g_{11} \end{aligned}

• $g_{14}$:

\begin{aligned} g_{14} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}}\\ &= \frac{1}{4}\boldsymbol{R}_{b_ib_{k+1}}\delta t^2 \end{aligned}


• $g_{21}$:

\begin{aligned} g_{21} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}}\\ &= -\frac{1}{4}\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^2 \end{aligned}

• $g_{22}$:

\begin{aligned} g_{22} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} \\ &= \frac{1}{2}\boldsymbol{R}_{b_ib_k}\delta t \end{aligned}

• $g_{23}$:

\begin{aligned} g_{23} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}}\\ &= -\frac{1}{4}\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^2\\ &= g_{21} \end{aligned}

• $g_{24}$:

\begin{aligned} g_{24} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} \\ &= \frac{1}{2}\boldsymbol{R}_{b_ib_{k+1}}\delta t \end{aligned}


• $g_{31}$:

\begin{aligned} g_{31} &= \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}}\\ &= \frac{1}{2}\boldsymbol{I}\delta t \end{aligned}

• $g_{33}$:

\begin{aligned} g_{33} &= \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}}\\ &= \frac{1}{2}\boldsymbol{I}\delta t\\ &= g_{33} \end{aligned}


## 参考资料

Built with Hugo
Theme Stack designed by Jimmy