IMU残差函数及雅可比公式推导（一）

最新推荐文章于 2022-10-19 21:57:41 发布

changshen_xu

最新推荐文章于 2022-10-19 21:57:41 发布

阅读量2.3k

点赞数 2

分类专栏： # ----------VIO原理系列文章标签： VINS

本文链接：https://blog.csdn.net/csdn_xcs/article/details/96356811

版权

----------VIO原理系列专栏收录该内容

14 篇文章 5 订阅

订阅专栏

第一部分：残差函数
约定符号：
IMU的真实值： $\omega^b,a^b$
IMU的测量值： $\tilde{\omega}^b,\tilde{a}^b$
某个时刻 $t$ ，二者的关系：
$\begin{aligned} \tilde{\omega}^{b_t}_t &=\omega^{b_t}_t+b^g_t+n^g_t \\ \tilde{a}^{b_t}_t &=a^{b_t}_t+b^a_t+n^a_t \\ &= q_{b_tw}(a^w_t+g^w)+b^a_t+n^a_t \end{aligned}$
ps. IMU机体系即 $b$ 系， $w$ 系表示世界坐标系。
不考虑高斯白噪声项，有：
$\begin{aligned} \omega^{b_t}_t &\approx \tilde{\omega}^{b_t}_t -b^g_t \\ a^w_t &\approx q_{wb_t} (\tilde{a}^{b_t}_t - b^a_t)- g^w\\ a^{b_t}_t &\approx \tilde{a}^{b_t}_t - b^a_t \end{aligned}$
考虑高斯白噪声项，有：
$\begin{aligned} \omega^{b_t}_t &= \tilde{\omega}^{b_t}_t -b^g_t -n^g_t\\ a^w_t &= q_{wb_t} (\tilde{a}^{b_t}_t - b^a_t-n^a_t)- g^w\\ a^{b_t}_t &= \tilde{a}^{b_t}_t - b^a_t-n^a_t \end{aligned}$
【注意】：此处考虑与不考虑高斯噪声，记为了同一个符号

P(ose)，V(elocity)，Q(uaternion)某一时刻 $t$ 的导数为：
$\begin{aligned} \dot{p}_{w b_t} &=v^w_t \\ \dot{v}^w_t &=a^w_t \\ \dot{q}_{w b_t}&= {q}_{w b_t}\otimes \begin{bmatrix} 0 \\ \frac{1}{2}\omega^{b_t}_t \end{bmatrix} \end{aligned}$
那么，已知第 $i$ 帧的姿态、位置（相对于世界坐标系），根据IMU的输出，可以递推求得第 $j$ 帧的姿态、位置（相对于世界坐标系）：
$\begin{aligned} {p}_{w b_j} &= {p}_{w b_i}+v^w_i\Delta t +\int \int_{t \in[i,j]}{a^w_t}dt^2 \\ {v}^w_j &=v^w_i+ \int_{t \in[i,j]}{a^w_t}dt \\ {q}_{w b_j}&= \int_{t \in [i,j]} {q}_{w b_t}\otimes \begin{bmatrix} 0 \\ \frac{1}{2}\omega^{b_t}_t \end{bmatrix}dt \end{aligned}$
【问题】姿态的积分？
另一种求得第 $j$ 帧的姿态、位置：
应用 ${q}_{w b_t}= {q}_{w b_i}\otimes {q}_{b_i b_t}$ ，转而去求相对于第 $i$ 帧，第 $j$ 帧的姿态、位置变化。
$\begin{aligned} {p}_{w b_j} &= {p}_{w b_i}+v^w_i\Delta t -\frac{1}{2}g^w\Delta t^2 + {q}_{w b_i} \int \int_{t \in[i,j]} ({q}_{b_i b_t}{a^{b_t}_t}) dt^2 \\ {v}^w_j &=v^w_i-g^w\Delta t +{q}_{w b_i} \int_{t \in[i,j]} ({q}_{b_i b_t}{a^{b_t}_t}) dt \\ {q}_{w b_j}&={q}_{w b_i}\otimes \int_{t \in [i,j]} {q}_{b_i b_t}\otimes \begin{bmatrix} 0 \\ \frac{1}{2}\omega^{b_t}_t \end{bmatrix}dt \end{aligned}$
【注】四元数之间的乘、四元数与矢量之间的乘
第二种积分的好处在于，即使优化过程使得第 $i$ 帧的姿态、位置发生变化，即 $q_{wb_i}$ 和 $p_{wb_i}$ 变化，但积分部分并不变化！！
IMU预积分以第 $i$ 帧为参考，将一段时间 $t\in [i,j]$ 的IMU数据积分起来，为了简化表达，定义三个符号（表示预积分量）：
$\begin{aligned} \alpha_{b_ib_j} &= \int \int_{t \in[i,j]} ({q}_{b_i b_t}{a^{b_t}_t}) dt^2 \\ \beta_{b_ib_j} &= \int_{t \in[i,j]} ({q}_{b_i b_t}{a^{b_t}_t}) dt \\ q_{b_ib_j} &= \int_{t \in [i,j]} {q}_{b_i b_t}\otimes \begin{bmatrix} 0 \\ \frac{1}{2}\omega^{b_t}_t \end{bmatrix}dt \end{aligned}$
【注】在纯惯导里，第三项被称为姿态变化四元数。
综上，整理一下（用新符号表示第 $j$ 帧的姿态、位置）：
$\begin{aligned} \begin{bmatrix} p_{wb_j} \\ v^w_j \\ q_{wb_j} \\ b^a_j \\ b^g_j \end{bmatrix} = \begin{bmatrix} {p}_{w b_i}+v^w_i\Delta t -\frac{1}{2}g^w\Delta t^2 + {q}_{w b_i} \alpha_{b_ib_j} \\ v^w_i-g^w\Delta t +{q}_{w b_i} \beta_{b_ib_j}\\ {q}_{w b_i}\otimes q_{b_ib_j} \\ b^a_i \\ b^g_i \end{bmatrix} \end{aligned}$

【注意】预积分量，既可以预测（即以递推形式得到）【记为】 $\hat{\alpha}_{b_ib_j}, \hat{\beta}_{b_ib_j}, \hat{q}_{b_ib_j},$ ，又可以被观测（即通过IMU数据的积分得到）【仍记为】 ${\alpha}_{b_ib_j}, {\beta}_{b_ib_j}, {q}_{b_ib_j},$ ，因此使用二者的差值构造IMU的残差函数：
$\begin{aligned} r_b= \begin{bmatrix} r_p \\ r_v \\ r_q \\ r_{b_a} \\ r_{b_g} \end{bmatrix} = \begin{bmatrix} \hat{\alpha}_{b_ib_j} -{\alpha}_{b_ib_j} \\ \hat{\beta}_{b_ib_j} - {\beta}_{b_ib_j}\\ \frac{1}{2}[\hat{q}_{b_jb_i} \otimes {q}_{b_ib_j}]_{xyz} \\ b^a_j - b^a_i \\ b^g_j - b^g_i \end{bmatrix} \end{aligned}$
其中，
$\begin{aligned} \begin{bmatrix} \hat \alpha_{b_ib_j} \\ \hat \beta_{b_ib_j} \\ \hat q_{b_jb_i} \end{bmatrix} = \begin{bmatrix} {q}_{ b_i w} (p_{wb_j}- {p}_{w b_i} - v^w_i\Delta t +\frac{1}{2}g^w\Delta t^2) \\ {q}_{b_iw} (v^w_j - v^w_i + g^w\Delta t )\\ {q}_{ b_j w}\otimes q_{wb_i} \end{bmatrix} \end{aligned}$

一个IMU量测数据的噪声强度通过Allan方差能够标定，现在我们更关心：目前我们使用的预积分量（一段时间内，即第 $i$ 帧到第 $j$ 帧多个IMU数据的积分结果）的方差。
【补充性质】：已知两个随机变量之间的线性关系 $y = A x$ ，若有 $x\in N(0,\Sigma_x)$ ，则可推导得 $y\in N(0,\Sigma_y)$ 。
$\mu_y=E(Ax)=A\mu_x=0$
$\begin{aligned} \Sigma_y &=E((Ax)(Ax)^T)\\ &= E(Axx^TA^T)\\ &= A\Sigma_xA^T \end{aligned}$

如果，我们回答上述问题，需要明确任一IMU采样时刻 $k$ 到下一时刻 $k + 1$ 之间的随机变量，以及线性关系：
时刻 $k + 1$ 的随机变量记为： $\eta_{k+1}=[\delta \alpha_{b_{k+1}b'_{k+1}},\delta \theta_ {b_{k+1}b'_{k+1}},\delta \beta_{b_{k+1}b'_{k+1}},\delta b^a_{b_{k+1}},\delta b^g_{b_{k+1}}]^T$ ，
时刻 $k$ 的随机变量记为： $\eta_{k}=[\delta \alpha_{b_{k}b'_{k}},\delta \theta_ {b_{k}b'_{k}},\delta \beta_{b_{k}b'_{k}},\delta b^a_{b_{k}},\delta b^g_{b_{k}}]^T$ ，
噪声项： $n_{k}=[n^a_k,n^g_k,n^a_{k+1},n^g_{k+1},n_{b^a_k},n_{b^g_k}]^T$
三者之间的关系： $\eta_{k+1}=F\eta_{k}+Gn_k$
若 $\eta_k\in N(0,\Sigma_{\eta_k})$ ， $n_k\in N(0,\Sigma_{n_k})$ ，则 $\eta_{k+1}\in N(0,\Sigma_{\eta_{k+1}})$ 。其中，
$\Sigma_{\eta_{k+1}}=F\Sigma_{\eta_{k}}F^T+G\Sigma_{n_{k}}G^T$

待求取的是 $\delta \alpha_{b_{k+1}b'_{k+1}}$ 等和 $\delta \alpha_{b_{k}b'_{k}}$ 等之间的关系，
目前根据预积分连续形式得到的离散形式可以反应： $\alpha_{b_{i}b'_{k+1}}$ 等和 $\alpha_{b_{i}b'_{k}}$ 之间的关系，即：
已知，
$\begin{aligned} \alpha_{b_ib_j} &= \int \int_{t \in[i,j]} ({q}_{b_i b_t}{a^{b_t}_t}) dt^2 \\ \beta_{b_ib_j} &= \int_{t \in[i,j]} ({q}_{b_i b_t}{a^{b_t}_t}) dt \\ q_{b_ib_j} &= \int_{t \in [i,j]} {q}_{b_i b_t}\otimes \begin{bmatrix} 0 \\ \frac{1}{2}\omega^{b_t}_t \end{bmatrix}dt \end{aligned}$
则：
$\begin{aligned} \alpha_{b_ib_{k+1}} &= \alpha_{b_ib_{k}} + \beta_{b_ib_k}\delta t +\frac{1}{2}a\delta t^2\\ \beta_{b_ib_{k+1}} &= \beta_{b_ib_{k}} +a\delta t \\ q_{b_ib_{k+1}} &= q_{b_ib_{k}}\otimes \begin{bmatrix} 0 \\ \frac{1}{2}\omega \delta t \end{bmatrix} \end{aligned}$
使用中值积分法（考虑高斯噪声）：
$\begin{aligned} \omega &= \frac{1}{2}(\omega^{b_k}_k+\omega^{b_{k+1}}_{k+1}) \\ &= \frac{1}{2}[(\tilde{\omega}^{b_k}_k -b^g_k -n^g_k)+(\tilde{\omega}^{b_{k+1}}_{k+1} -b^g_{k+1} -n^g_{k+1})]\\ a &= \frac{1}{2}(a^{b_k}_k+a^{b_{k+1}}_{k+1}) \\ &=\frac{1}{2}[(q_{wb_k} (\tilde{a}^{b_k}_k - b^a_k-n^a_k)- g^w)+(q_{wb_{k+1}} (\tilde{a}^{b_{k+1}}_{k+1} - b^a_{k+1}-n^a_{k+1})- g^w)] \end{aligned}$
注意此处，1.噪声项可加可减，使用加；2.预积分时 $g^w$ 被提到了积分之外；3.预积分相对于 $b_i$ 系，故可写为：
$\begin{aligned} \omega &= \frac{1}{2}[(\tilde{\omega}^{b_k}_k -b^g_k +n^g_k)+(\tilde{\omega}^{b_{k+1}}_{k+1}-b^g_{k+1} +n^g_{k+1})]\\ a &=\frac{1}{2}[q_{b_ib_k} (\tilde{a}^{b_k}_k - b^a_k+n^a_k)+q_{b_ib_{k+1}} (\tilde{a}^{b_{k+1}}_{k+1}- b^a_{k+1}+n^a_{k+1})] \end{aligned}$

其中，
$b^a_{k+1}=b^a_k+n_{b^a_k}\delta t \\ b^g_{k+1}=b^g_k+n_{b^g_k}\delta t$

至此，和上一节视觉雅可比公式的推导类似了：即求
$F=\frac{ \partial [\alpha_{b_{i}b'_{k+1}},\theta_ {b_{i}b'_{k+1}},\beta_{b_{i}b'_{k+1}},b^a_{b_{k+1}},b^g_{b_{k+1}}]^T} { \partial [\delta \alpha_{b_{k}b'_{k}},\delta \theta_ {b_{k}b'_{k}},\delta \beta_{b_{k}b'_{k}},\delta b^a_{b_{k}},\delta b^g_{b_{k}}]^T}$
及
$G=\frac{ \partial [\alpha_{b_{i}b'_{k+1}},\theta_ {b_{i}b'_{k+1}},\beta_{b_{i}b'_{k+1}},b^a_{b_{k+1}},b^g_{b_{k+1}}]^T} { \partial [n^a_k,n^g_k,n^a_{k+1},n^g_{k+1},n_{b^a_k},n_{b^g_k}]^T}$
可以看出来，这里共有 $5\times 5 \times 2=50$ 个小块，故以F.1.1 ~ F.1.5 ~ … F.5.5 ~ … G.5.5顺序依次求解。

残差函数已经得到，下面明确需要对哪些状态求雅可比？

changshen_xu

关注

2
点赞
踩
8

收藏

觉得还不错? 一键收藏
1
评论
IMU残差函数及雅可比公式推导（一）

第一部分：残差函数约定符号：IMU的真实值：ωb,ab\omega^b,a^bωb,abIMU的测量值：ω~b,a~b\tilde{\omega}^b,\tilde{a}^bω~b,a~b某个时刻ttt，二者的关系：ω~tbt=ωtbt+btg+ntga~tbt=atbt+bta+nta=qbtw(atw+gw)+bta+nta\begin{aligned}\tilde{\omega}...
复制链接

扫一扫

专栏目录