vins 预积分中值法公式详细推导
关键符号说明
w m w_m wm vins中中值法， [ w t , w t + 1 ] [w_t , w_{t+1}] [wt,wt+1]区间的测量值
w m = w t + w t + 1 2 (0) \tag{0} w_{m} = \frac {w_t+w_{t+1}} {2} wm=2wt+wt+1(0)
a m a_m am vins中中值法，[ a t , a t + 1 ] a_t , a_{t+1}] at,at+1]区间的测量值
a m = q t ( a t + n a 0 − b a t ) + q t + 1 ( a t + 1 + n a 1 − b a t ) 2 (1) \tag{1} a_{m} = {\frac {q_t(a_t + n_{a0}-b_{at}) + q_{t+1}{(a_{t+1}+n_{a1}-b_{at})}} {2}} am=2qt(at+na0−bat)+qt+1(at+1+na1−bat)(1)
a t a_t at t t t 时刻的加速度测量值
v t v_t vt t t t 时刻的角速度测量值
b a t b_{at} bat 加速度在 t t t时刻的偏置
b w t b_{wt} bwt 角速度在 t t t时刻的偏置
n a t n_{at} nat 加速度在 t t t时刻的高斯噪声
n w t n_{wt} nwt 角速度在 t t t时刻的高斯噪声
0、离散系统的状态更新方程
δ p t + 1 = δ p t + δ v t δ t + 1 2 a m δ 2 t (2) \delta p_{t+1} = \delta p_t + \delta v_t \delta t + \frac 1 2 a_m\delta^2 t \tag{2} δpt+1=δpt+δvtδt+21amδ2t(2)
δ q t + 1 = δ q t ⊗ { w m δ t } (3) \delta q_{t+1} = \delta q_t\otimes\big\{w_m\delta t\big\}\tag{3} δqt+1=δqt⊗{
wmδt}(3)
δ v t + 1 = δ v t + a m δ t (4) \delta v_{t+1} = \delta v_t + a_m \delta t \tag{4} δvt+1=δvt+amδt(4)
b a ( t + 1 ) = b a t (5) b_{a(t+1)} = b_{at} \tag{5} ba(t+1)=bat(5)
b w ( t + 1 ) = b w t (6) b_{w(t+1)} = b_{wt} \tag{6} bw(t+1)=bwt(6)
预积分过程中， b a b_{a} ba是不变的。
1、 δ θ t + 1 \delta\theta_{t+1} δθt+1 的推导：
由参考文献2中的5.33节The error-state kinematics可知：
δ θ t ′ = − [ w m − b w ] × δ θ t − δ b w − n w (7) \tag{7}\delta\theta_t^{'}=-[w_m - b_w]_{\times}\delta\theta_t - \delta b_w - n_w δθt′=−[wm−bw]×δθt−δbw−nw(7)
公式 ( 0 ) (0) (0)带入公式 ( 7 ) (7) (7)可得：
δ θ t ′ = − [ w t + w t + 1 2 − b w t ] × δ θ t − δ b w − n w 0 + n w 1 2 (8) \tag{8}\delta\theta_t^{'}=-[\frac {w_t+w_{t+1}} {2}-b_{wt}]_{\times}\delta\theta_t - \delta{b_w} - \frac {n_{w0} + n_{w1}} {2} δθt′=−[2wt+wt+1−bwt]×δθt−δbw−2nw0+nw1(8)
由泰勒公式得：
δ θ t + 1 = δ θ t + δ θ t ′ δ t (9) \tag{9} \delta\theta_{t+1} = \delta\theta_t + \delta\theta_t^{'}\delta{t} δθt+1=δθt+δθt′δt(9)
δ θ t + 1 = δ θ t + ( − [ w t + w t + 1 2 − b w t ] × δ θ t − δ b w − n w 0 + n w 1 2 ) δ t = ( I − [ w t + w t + 1 2 − b w t ] × δ t ) δ θ t − δ b w δ t − n w 0 + n w 1 2 δ t (10) \begin{aligned} \delta\theta_{t+1} = \delta\theta_t + (-[\frac {w_t+w_{t+1}} {2}-b_{wt}]_{\times}\delta\theta_t - \delta{b_w} - \frac {n_{w0} + n_{w1}} {2})\delta{t}\\ =(I-[\frac {w_t+w_{t+1}} {2}-b_{wt}]_{\times}\delta{t})\delta\theta_t - \delta{b_w}\delta{t} - {\frac {n_{w0} + n_{w1}} {2}\delta}{t}\end{aligned}\tag{10} δθt+1=δθt+(−[2wt+wt+1−bwt]×δθt−δbw−2nw0+nw1)δt=(I−[2wt+wt+1−bwt]×δt)δθt−δbwδt−2nw0+nw1δt(10
最低0.47元/天 解锁文章
扫一扫
494
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
