ceres 残差 残差ei怎么算

第一部分：残差函数
约定符号：
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=Ax$，若有$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顺序依次求解。

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