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

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

---

P(ose)，V(elocity)，Q(uaternion)某一时刻$t$的导数为：
$$\begin{array}{rl}{\dot{p}}_{w{b}_t}& =v_t^w\\ {\dot{v}}_t^w& =a_t^w\\ {\dot{q}}_{w{b}_t}& =q_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }_t^{b_t}\end{array}\right]\end{array}$$
那么，已知第$i$帧的姿态、位置（相对于世界坐标系），根据IMU的输出，可以递推求得第$j$帧的姿态、位置（相对于世界坐标系）：
$$\begin{array}{rl}{p}_{w{b}_j}& =p_{w{b}_i}+v_i^w\Delta t+\int {\int }_{t\in [i,j]}{a}_t^{w}d{t}^2\\ v_j^w& =v_i^w+{\int }_{t\in [i,j]}{a}_t^{w}dt\\ q_{w{b}_j}& ={\int }_{t\in [i,j]}{q}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }_t^{b_t}\end{array}\right]dt\end{array}$$
【问题】姿态的积分？
另一种求得第$j$帧的姿态、位置：
应用 $q_{w{b}_t}=q_{w{b}_i}\otimes q_{b_i{b}_t}$，转而去求相对于第$i$帧，第$j$帧的姿态、位置变化。
p w b j = p w b i + v i w Δ t − 1 2 g w Δ t 2 + q w b i ∫ ∫ t ∈