IMU预积分推导（三）

IMU预积分推导系列文章
IMU预积分推导（一）
IMU预积分推导（二）
IMU预积分推导（三）

5.1、先验残差对状态增量的雅可比矩阵

$(31)$中，只有先验旋转残差对增量$\delta {\mathbf{\varphi }}_{i_0}$的雅可比矩阵不能直接看出，需要求解，其他雅可比容易看出
$$\begin{array}{rl}{\mathbf{r}}_{{\mathbf{R}}_{i_0}}({\mathbf{R}}_{i_0}\mathrm{Exp}(\delta {\mathbf{\varphi }}_{i_0}))& =\mathrm{Log}\left({\check{\mathbf{R}}}_{i_0}^T{\mathbf{R}}_{i_0}\mathrm{Exp}(\delta {\mathbf{\varphi }}_{i_0})\right)\\ & =\mathrm{Log}\left({\check{\mathbf{R}}}_{i_0}^T{\mathbf{R}}_{i_0}\right)+{\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left({\check{\mathbf{R}}}_{i_0}^T{\mathbf{R}}_{i_0}\right)\right)\delta {\mathbf{\varphi }}_{i_0}\end{array}$$
故先验残差对状态增量的雅可比矩阵为
$$\begin{array}{lllllc}& \frac{\partial {\mathbf{r}}_{{\mathbf{R}}_{i_0}}}{\partial \delta {\mathbf{\varphi }}_{i_0}}={\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left({\check{\mathbf{R}}}_{i_0}^T{\mathbf{R}}_{i_0}\right)\right)& \frac{\partial {\mathbf{r}}_{{\mathbf{R}}_{i_0}}}{\partial \delta {\mathbf{p}}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{R}}_{i_0}}}{\partial \delta {\mathbf{v}}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{R}}_{i_0}}}{\partial \delta {\mathbf{b}}_{g,i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{R}}_{i_0}}}{\partial \delta {\mathbf{b}}_{a,i_0}}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{{\mathbf{p}}_{i_0}}}{\partial \delta {\mathbf{\varphi }}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{p}}_{i_0}}}{\partial \delta {\mathbf{p}}_{i_0}}=\mathbf{I}& \frac{\partial {\mathbf{r}}_{{\mathbf{p}}_{i_0}}}{\partial \delta {\mathbf{v}}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{p}}_{i_0}}}{\partial \delta {\mathbf{b}}_{g,i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{p}}_{i_0}}}{\partial \delta {\mathbf{b}}_{a,i_0}}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{{\mathbf{v}}_{i_0}}}{\partial \delta {\mathbf{\varphi }}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{v}}_{i_0}}}{\partial \delta {\mathbf{p}}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{v}}_{i_0}}}{\partial \delta {\mathbf{v}}_{i_0}}=\mathbf{I}& \frac{\partial {\mathbf{r}}_{{\mathbf{v}}_{i_0}}}{\partial \delta {\mathbf{b}}_{g,i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{v}}_{i_0}}}{\partial \delta {\mathbf{b}}_{a,i_0}}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{g,i_0}}}{\partial \delta {\mathbf{\varphi }}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{g,i_0}}}{\partial \delta {\mathbf{p}}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{g,i_0}}}{\partial \delta {\mathbf{v}}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{g,i_0}}}{\partial \delta {\mathbf{b}}_{g,i_0}}=\mathbf{I}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{g,i_0}}}{\partial \delta {\mathbf{b}}_{a,i_0}}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{a,i_0}}}{\partial \delta {\mathbf{\varphi }}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{a,i_0}}}{\partial \delta {\mathbf{p}}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{a,i_0}}}{\partial \delta {\mathbf{v}}_{i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{a,i_0}}}{\partial \delta {\mathbf{b}}_{g,i_0}}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{a,i_0}}}{\partial \delta {\mathbf{b}}_{a,i_0}}=\mathbf{I}\end{array}$$
5.2、递推残差对状态增量的雅可比矩阵

递推陀螺仪零偏残差对增量雅的可比矩阵为
$$\begin{array}{rlr}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{g,j}}}{\partial \delta {\mathbf{b}}_{g,i}}=-\mathbf{I}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{g,j}}}{\partial \delta {\mathbf{b}}_{g,j}}=\mathbf{I}\end{array}$$
递推加速度计零偏残差对增量的雅可比矩阵为
$$\begin{array}{rlr}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{a,j}}}{\partial \delta {\mathbf{b}}_{a,i}}=-\mathbf{I}& \frac{\partial {\mathbf{r}}_{{\mathbf{b}}_{a,j}}}{\partial \delta {\mathbf{b}}_{a,j}}=\mathbf{I}\end{array}$$
5.3、预积分旋转残差对状态增量的雅可比矩阵

预积分旋转残差和${\mathbf{R}}_i,{\mathbf{R}}_j,{\mathbf{b}}_{g,i}$相关，因此对增量$\delta {\mathbf{\varphi }}_i,\delta {\mathbf{\varphi }}_j,\delta {\mathbf{b}}_{g,i}$的雅可比矩阵需要求解，对其余增量$\delta {\mathbf{p}}_i,\delta {\mathbf{v}}_i,\delta {\mathbf{p}}_j,\delta {\mathbf{v}}_j,\delta {\mathbf{b}}_{a,i}$的雅可比矩阵为零矩阵

展开求解预积分旋转残差对$\delta {\mathbf{\varphi }}_i$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i\mathrm{Exp}(\delta {\mathbf{\varphi }}_i))& =\mathrm{Log}\left[(\Delta {\tilde{\mathbf{R}}}_{ij}{)}^T({\mathbf{R}}_i\mathrm{Exp}(\delta {\mathbf{\varphi }}_i){)}^T{\mathbf{R}}_j\right]\\ & =\mathrm{Log}\left[(\Delta {\tilde{\mathbf{R}}}_{ij}{)}^T\mathrm{Exp}(-\delta {\mathbf{\varphi }}_i{)}^T{\mathbf{R}}_i^T{\mathbf{R}}_j\right]\\ & =\mathrm{Log}\left[(\Delta {\tilde{\mathbf{R}}}_{ij}{)}^T{\mathbf{R}}_i^T{\mathbf{R}}_j\mathrm{Exp}(-{\mathbf{R}}_j^T{\mathbf{R}}_i\delta {\mathbf{\varphi }}_i)\right]\\ & =\mathrm{Log}\left\{\mathrm{Exp}\left[\mathrm{Log}\left((\Delta {\tilde{\mathbf{R}}}_{ij}{)}^T{\mathbf{R}}_i^T{\mathbf{R}}_j\right)\right]\mathrm{Exp}(-{\mathbf{R}}_j^T{\mathbf{R}}_i\delta {\mathbf{\varphi }}_i)\right\}\\ & =\mathrm{Log}\left[\mathrm{Exp}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i))\mathrm{Exp}(-{\mathbf{R}}_j^T{\mathbf{R}}_i\delta {\mathbf{\varphi }}_i)\right]\\ & ={\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i)-{\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i)){\mathbf{R}}_j^T{\mathbf{R}}_i\delta {\mathbf{\varphi }}_i\end{array}$$
展开求解预积分旋转残差对$\delta {\mathbf{\varphi }}_j$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_j\mathrm{Exp}(\delta {\mathbf{\varphi }}_j))& =\mathrm{Log}\left[(\Delta {\tilde{\mathbf{R}}}_{ij}{)}^T{\mathbf{R}}_i{\mathbf{R}}_j\mathrm{Exp}(\delta {\mathbf{\varphi }}_j)\right]\\ & =\mathrm{Log}\left\{\mathrm{Exp}\left[\mathrm{Log}\left((\Delta {\tilde{\mathbf{R}}}_{ij}{)}^T{\mathbf{R}}_i^T{\mathbf{R}}_j\right)\right]\mathrm{Exp}(\delta {\mathbf{\varphi }}_i)\right\}\\ & =\mathrm{Log}\left[\mathrm{Exp}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i))\mathrm{Exp}(\delta {\mathbf{\varphi }}_i)\right]\\ & ={\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i)+{\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i))\delta {\mathbf{\varphi }}_i\end{array}$$
展开求解预积分旋转残差对$\delta {\mathbf{b}}_{g,i}$的雅可比矩阵，其中预积分观测量通过$(17a)$进行修正
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i}))& =\mathrm{Log}\left\{{\left[\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i})\mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)\right]}^T\Delta {\mathbf{R}}_{ij}\right\}\\ & =\mathrm{Log}\left[\mathrm{Exp}\left(-\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}{)}^T\Delta {\mathbf{R}}_{ij}\right]\\ & =\mathrm{Log}\left[\mathrm{Exp}\left(-\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)\mathrm{Exp}\left(\mathrm{Log}\left(\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}{)}^T\Delta {\mathbf{R}}_{ij}\right)\right)\right]\\ & =\mathrm{Log}\left[\mathrm{Exp}\left(-\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)\mathrm{Exp}\left({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i})\right)\right]\\ & =\mathrm{Log}\left[\mathrm{Exp}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i}))\mathrm{Exp}\left(-\mathrm{Exp}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i}){)}^T\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)\right]\\ & ={\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i}))-{\mathbf{J}}_r({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i}){)}^{-1}\mathrm{Exp}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i}){)}^T\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\end{array}$$
综上可得到预积分旋转残差对状态增量的雅可比矩阵
$$\begin{array}{llc}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{\varphi }}_i}=-{\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i)){\mathbf{R}}_j^T{\mathbf{R}}_i& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{\varphi }}_j}={\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{R}}_i))\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{p}}_i}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{p}}_j}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{v}}_i}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{v}}_j}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{b}}_{g,i}}=-{\mathbf{J}}_r({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i}){)}^{-1}\mathrm{Exp}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}({\mathbf{b}}_{g,i}){)}^T\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial \delta {\mathbf{b}}_{a,i}}=\mathbf{O}\end{array}$$
5.4、预积分速度残差对状态增量的雅可比矩阵

速度残差和${\mathbf{R}}_i,{\mathbf{v}}_i,{\mathbf{v}}_j,{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}$和相关，因此对增量$\delta {\mathbf{\varphi }}_i,\delta {\mathbf{v}}_i,\delta {\mathbf{v}}_j,\delta {\mathbf{b}}_{g,i},\delta {\mathbf{b}}_{a,i}$的雅可比矩阵需要求解，对其余增量$\delta {\mathbf{\varphi }}_j,\delta {\mathbf{p}}_i,\delta {\mathbf{p}}_j$的雅可比矩阵为零矩阵

展开求解预积分速度残差对$\delta {\mathbf{b}}_{g,i},\delta {\mathbf{b}}_{a,i}$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})& =\Delta {\mathbf{v}}_{ij}-\left(\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\right)\\ & ={\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})-\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}-\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\end{array}$$
展开求解预积分速度残差对$\delta {\mathbf{v}}_i$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{v}}_i+\delta {\mathbf{v}}_i)& ={\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\delta {\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})-\Delta {\tilde{\mathbf{v}}}_{ij}\\ & ={\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{v}}_i)-{\mathbf{R}}_i^T\delta {\mathbf{v}}_i\end{array}$$
展开求解预积分速度残差对$\delta {\mathbf{v}}_j$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{v}}_j+\delta {\mathbf{v}}_j)& ={\mathbf{R}}_i^T({\mathbf{v}}_j+\delta {\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})-\Delta {\tilde{\mathbf{v}}}_{ij}\\ & ={\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{v}}_j)+{\mathbf{R}}_i^T\delta {\mathbf{v}}_j\end{array}$$
展开求解预积分速度残差对$\delta {\mathbf{\varphi }}_i$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{v}}_j+\delta {\mathbf{v}}_j)& =({\mathbf{R}}_i\mathrm{Exp}(\delta {\mathbf{\varphi }}_i){)}^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})-\Delta {\tilde{\mathbf{v}}}_{ij}\\ & =\mathrm{Exp}(-\delta {\mathbf{\varphi }}_i){\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})-\Delta {\tilde{\mathbf{v}}}_{ij}\\ & \approx \left(\mathbf{I}-(\delta {\mathbf{\varphi }}_i{)}^{\wedge }\right){\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})-\Delta {\tilde{\mathbf{v}}}_{ij}\\ & ={\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})-\Delta {\tilde{\mathbf{v}}}_{ij}-(\delta {\mathbf{\varphi }}_i{)}^{\wedge }{\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})\\ & ={\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{v}}_j)+{\left[{\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})\right]}^{\wedge }\delta {\mathbf{\varphi }}_i\end{array}$$
综上可得到预积分速度残差对状态增量的雅可比矩阵
$$\begin{array}{llc}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{\varphi }}_i}={\left[{\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})\right]}^{\wedge }& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{\varphi }}_j}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{p}}_i}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{p}}_j}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{v}}_i}=-{\mathbf{R}}_i^T& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{v}}_j}={\mathbf{R}}_i^T\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{b}}_{g,i}}=-\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial \delta {\mathbf{b}}_{a,i}}=-\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\end{array}$$
5.5、预积分位移残差对状态增量的雅可比矩阵

位移残差和${\mathbf{R}}_i,{\mathbf{v}}_i,{\mathbf{p}}_i,{\mathbf{p}}_j,{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}$相关，因此对增量$\delta {\mathbf{\varphi }}_i,\delta {\mathbf{v}}_i,\delta {\mathbf{p}}_i,\delta {\mathbf{p}}_j,\delta {\mathbf{b}}_{g,i},\delta {\mathbf{b}}_{a,i}$的雅可比矩阵需要求解，对其余增量$\delta {\mathbf{\varphi }}_j,\delta {\mathbf{v}}_j$的雅可比矩阵为零矩阵

展开求解预积分位移残差对$\delta {\mathbf{b}}_{g,i},\delta {\mathbf{b}}_{a,i}$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})& =\Delta {\mathbf{p}}_{ij}-\left(\Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\right)\\ & ={\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})-\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}-\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\end{array}$$
展开求解预积分位移残差对${\mathbf{p}}_j$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{p}}_j+{\mathbf{R}}_j\delta {\mathbf{p}}_j)& ={\mathbf{R}}_i^T({\mathbf{p}}_j+{\mathbf{R}}_j\delta {\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)-\Delta {\tilde{\mathbf{p}}}_{ij}\\ & ={\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{p}}_j)+{\mathbf{R}}_i^T{\mathbf{R}}_j\delta {\mathbf{p}}_j\end{array}$$
展开求解预积分位移残差对${\mathbf{p}}_i$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{p}}_i+{\mathbf{R}}_i\delta {\mathbf{p}}_i)& ={\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{R}}_i\delta {\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)-\Delta {\tilde{\mathbf{p}}}_{ij}\\ & ={\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{p}}_i)-\delta {\mathbf{p}}_i\end{array}$$
展开求解预积分位移残差对${\mathbf{v}}_i$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{v}}_i+\delta {\mathbf{v}}_i)& ={\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\delta {\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)-\Delta {\tilde{\mathbf{p}}}_{ij}\\ & ={\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}({\mathbf{v}}_i)-{\mathbf{R}}_i^T\Delta t_{ij}\delta {\mathbf{v}}_i\end{array}$$
展开求解预积分位移残差对${\mathbf{\varphi }}_i$的雅可比矩阵
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{R}}_i\mathrm{Exp}(\delta {\mathbf{\varphi }}_i))& =({\mathbf{R}}_i\mathrm{Exp}(\delta {\mathbf{\varphi }}_i){)}^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)-\Delta {\tilde{\mathbf{p}}}_{ij}\\ & =\mathrm{Exp}(-\delta {\mathbf{\varphi }}_i){\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)-\Delta {\tilde{\mathbf{p}}}_{ij}\\ & \approx (\mathbf{I}-(\delta {\mathbf{\varphi }}_i{)}^{\wedge }){\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)-\Delta {\tilde{\mathbf{p}}}_{ij}\\ & ={\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)-\Delta {\tilde{\mathbf{p}}}_{ij}-(\delta {\mathbf{\varphi }}_i{)}^{\wedge }{\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)\\ & ={\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}({\mathbf{R}}_i)+{\left[{\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)\right]}^{\wedge }\delta {\mathbf{\varphi }}_i\end{array}$$
综上可得到预积分位移残差对状态增量的雅可比矩阵
$$\begin{array}{llc}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{\varphi }}_i}={\left[{\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)\right]}^{\wedge }& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{\varphi }}_j}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{p}}_i}=-\mathbf{I}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{p}}_j}={\mathbf{R}}_i^T{\mathbf{R}}_j\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{v}}_i}=-{\mathbf{R}}_i^T\Delta t_{ij}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{v}}_j}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{b}}_{g,i}}=-\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial \delta {\mathbf{b}}_{a,i}}=-\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\end{array}$$

实验：预积分实现GINS系统定位

本实验通过预积分方案实现GINS系统定位

观测残差${\mathbf{r}}_{{\mathbf{z}}_i}={\mathbf{r}}_{{\mathbf{T}}_i}^G={\left[{{\mathbf{r}}_{{\mathbf{R}}_i}^G}^T,{{\mathbf{r}}_{{\mathbf{p}}_i}^G}^T\right]}^T$，其中
$$\begin{array}{rl}{\mathbf{r}}_{{\mathbf{R}}_i}^G& \stackrel{\mathrm{def}}{=}\mathrm{Log}\left({{\tilde{\mathbf{R}}}_i^G}^T{\mathbf{R}}_i\right)\\ {\mathbf{r}}_{{\mathbf{p}}_i}^G& \stackrel{\mathrm{def}}{=}{\mathbf{p}}_i-{\tilde{\mathbf{p}}}_i^G\end{array}$$
${\tilde{\mathbf{T}}}_i^G=[{\tilde{\mathbf{R}}}_i^G,{\tilde{\mathbf{p}}}_i^G]$为GNSS在第$i$时刻观测到的位姿。暂不讨论$\mathbf{\Sigma }({\mathbf{z}}_i)$求解。

GNSS旋转残差对增量$\delta {\mathbf{\varphi }}_i$的雅可比矩阵不能直接看出，需要求解，其余雅可比容易看出
$$\begin{array}{llc}& \frac{\partial {\mathbf{r}}_{{\mathbf{R}}_i}^G}{\partial \delta {\mathbf{\varphi }}_i}={\mathbf{J}}_r^{-1}\left(\mathrm{Log}\left({\check{\mathbf{R}}}_i^T{\mathbf{R}}_i\right)\right)& \frac{\partial {\mathbf{r}}_{{\mathbf{R}}_i}^G}{\partial \delta {\mathbf{p}}_i}=\mathbf{O}\\ & \frac{\partial {\mathbf{r}}_{{\mathbf{p}}_i}^G}{\partial \delta {\mathbf{\varphi }}_i}=\mathbf{O}& \frac{\partial {\mathbf{r}}_{{\mathbf{p}}_i}^G}{\partial \delta {\mathbf{p}}_i}=\mathbf{I}\end{array}$$
本实验采用两帧优化模型，当IMU数据到来时，更新预积分协方差矩阵，预积分观测对零偏的雅可比等预积分相关量，并将预测结果在窗口显示，通过GNSS触发优化，即当RTK数据到来时，对当前帧与上一帧的系统状态进行优化，并将当前帧的优化结果在窗口显示，最后重置预积分相关量。在优化过程中不考虑某状态变量间更新对其他状态变量的影响，优化目标是使残差尽量小，本实验的图优化结构如下图

实验结果如下图，可以看到缺少GNSS观测时，单纯靠IMU积分会导致位移局部发散。在预积分方案中，可以选择优化迭代次数，并且可以调整与边关联的顶点。

附录

§1

任意$\mathbf{\varphi }\in {\mathbb{R}}^3$，$\mathbf{R}\in SO(3)$，满足
$$\mathbf{R}\mathrm{Exp}(\mathbf{\varphi }){\mathbf{R}}^T=\exp (\mathbf{R}{\mathbf{\varphi }}^{\wedge }{\mathbf{R}}^T)=\mathrm{Exp}(\mathbf{R}\mathbf{\varphi })$$
由此可以推出
$$\mathrm{Exp}(\mathbf{\varphi })\mathbf{R}=\mathbf{R}\mathrm{Exp}({\mathbf{R}}^T\mathbf{\varphi })\text{\hspace{1em}(A1)}$$
§2

对于$\mathbf{S}={\sum }_{k=i}^j{\mathbf{A}}_k$，${\mathbf{A}}_k\in {\mathbb{R}}^{M\times N}$，若$j<i$，则$\mathbf{S}=\mathbf{O}$；若$j⩾i$，则$\mathbf{S}={\mathbf{A}}_i+\cdots +{\mathbf{A}}_j$

对于$\mathbf{P}={\prod }_{k=i}^j{\mathbf{A}}_k$，${\mathbf{A}}_k\in {\mathbb{R}}^{N\times N}$，若$j<i$，则$\mathbf{P}=\mathbf{I}$；若$j⩾i$，则$\mathbf{P}={\mathbf{A}}_i\cdots {\mathbf{A}}_j$

§3、$SO(3)+\mathbf{t}$的更新

$SO(3)+\mathbf{t}$的更新有两种方式

方式一：
$${\mathbf{R}}_{w{b}^{\prime }}\leftarrow {\mathbf{R}}_{wb}\delta \mathbf{R}{\mathbf{p}}_{w{b}^{\prime }}^w\leftarrow {\mathbf{p}}_{wb}^w+{\mathbf{R}}_{wb}\delta \mathbf{p}$$
此时$\delta \mathbf{R}={\mathbf{R}}_{b{b}^{\prime }}$表示摄动载体系（经摄动后的载体坐标系）的载体坐标系到载体系的旋转矩阵，$\delta \mathbf{p}={\mathbf{R}}_{bw}({\mathbf{p}}_{w{b}^{\prime }}^w-{\mathbf{p}}_{wb}^w)={\mathbf{R}}_{bw}{\mathbf{p}}_{b{b}^{\prime }}^w={\mathbf{p}}_{b{b}^{\prime }}^b$表示载体坐标系原点到摄动载体系原点的矢量在载体坐标系下的坐标。

方式二：
$${\mathbf{R}}_{w{b}^{\prime }}\leftarrow {\mathbf{R}}_{wb}\delta \mathbf{R}{\mathbf{p}}_{w{b}^{\prime }}^w\leftarrow {\mathbf{p}}_{wb}^w+\delta \mathbf{p}$$
此时$\delta \mathbf{R}={\mathbf{R}}_{b{b}^{\prime }}$仍表示摄动载体系（经摄动后的载体坐标系）的载体坐标系到载体系的旋转矩阵，$\delta \mathbf{p}={\mathbf{p}}_{w{b}^{\prime }}^w-{\mathbf{p}}_{wb}^w={\mathbf{p}}_{b{b}^{\prime }}^w$表示载体坐标系原点到摄动载体系原点的矢量在世界坐标系下的坐标。

上述两种更新方式在实际应用中均可采取，区别在于$\delta \mathbf{p}$表示的物理意义不同。

§4、非欧式空间上的不确定性

若
$$\mathbf{x}={\mathbf{x}}_0\oplus \delta \mathbf{x}\delta \mathbf{x}\sim \mathcal{N}(0,\mathbf{\Sigma })$$
其中$\mathbf{x}\in \mathrm{Dom}(\mathbf{x})$，$\mathrm{Dom}(\mathbf{x})$为非欧式空间，$\delta \mathbf{x}\in {\mathbb{R}}^N$，$\oplus :\mathrm{Dom}(\mathbf{x})\times {\mathbb{R}}^N\to \mathrm{Dom}(\mathbf{x})$，则$\mathbf{x}$的不确定性完全来源于$\delta \mathbf{x}$，可将$\mathbf{x}$视为$\delta \mathbf{x}$的函数$\mathbf{x}(\delta \mathbf{x})$

故$\mathbf{x}$的概率分布为
$$p(\mathbf{x})=p(\mathbf{x}(\delta \mathbf{x}))=p(\delta \mathbf{x})\propto \exp \left(-\frac{1}{2}\Vert \mathbf{x}\ominus {\mathbf{x}}_0{\Vert }_{\mathbf{\Sigma }}^2\right)$$
其中$\ominus :\mathrm{Dom}(\mathbf{x})\times \mathrm{Dom}(\mathbf{x})\to {\mathbb{R}}^N$