第一部分:残差函数
约定符号:
IMU的真实值: ω b , a b \omega^b,a^b ωb,ab
IMU的测量值: ω ~ b , a ~ b \tilde{\omega}^b,\tilde{a}^b ω~b,a~b
某个时刻 t t t,二者的关系:
ω ~ t b t = ω t b t + b t g + n t g 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 \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} ω~tbta~tbt=ωtbt+btg+ntg=atbt+bta+nta=qbtw(atw+gw)+bta+nta
ps. IMU机体系即 b b b系, w w w系表示世界坐标系。
不考虑高斯白噪声项,有:
ω t b t ≈ ω ~ t b t − b t g a t w ≈ q w b t ( a ~ t b t − b t a ) − g w a t b t ≈ a ~ t b t − b t a \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} ωtbtatwatbt≈ω~tbt−btg≈qwbt(a~tbt−bta)−gw≈a~tbt−bta
考虑高斯白噪声项,有:
ω t b t = ω ~ t b t − b t g − n t g a t w = q w b t ( a ~ t b t − b t a − n t a ) − g w a t b t = a ~ t b t − b t a − n t a \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} ωtbtatwatbt=ω~tbt−btg−ntg=qwbt(a~tbt−bta−nta)−gw=a~tbt−bta−nta
【注意】:此处考虑与不考虑高斯噪声,记为了同一个符号
P(ose),V(elocity),Q(uaternion)某一时刻 t t t的导数为:
p ˙ w b t = v t w v ˙ t w = a t w q ˙ w b t = q w b t ⊗ [ 0 1 2 ω t b 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} p˙wbtv˙twq˙wbt=vtw=atw=qwbt⊗[021ωtbt]
那么,已知第 i i i帧的姿态、位置(相对于世界坐标系),根据IMU的输出,可以递推求得第 j j j帧的姿态、位置(相对于世界坐标系):
p w b j = p w b i + v i w Δ t + ∫ ∫ t ∈ [ i , j ] a t w d t 2 v j w = v i w + ∫ t ∈ [ i , j ] a t w d t q w b j = ∫ t ∈ [ i , j ] q w b t ⊗ [ 0 1 2 ω t b t ] d t \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} pwbjvjwqwbj=pwbi+viwΔt+∫∫t∈[i,j]atwdt2=viw+∫t∈[i,j]atwdt=∫t∈[i,j]qwbt⊗[021ωtbt]dt
【问题】姿态的积分?
另一种求得第 j j j帧的姿态、位置:
应用 q w b t = q w b i ⊗ q b i b t {q}_{w b_t}= {q}_{w b_i}\otimes {q}_{b_i b_t} qwbt=qwbi⊗qbibt,转而去求相对于第 i i i帧,第 j j j帧的姿态、位置变化。
p w b j = p w b i + v i w Δ t − 1 2 g w Δ t 2 + q w b i ∫ ∫ t ∈