Vins 预积分中值法公式详细推导

最新推荐文章于 2024-12-09 23:36:26 发布

wangyiwei2011

最新推荐文章于 2024-12-09 23:36:26 发布

阅读量643

点赞数 1

CC 4.0 BY-SA版权

分类专栏：笔记

本文链接：https://blog.youkuaiyun.com/wangyiwei2011/article/details/110523781

vins 预积分中值法公式详细推导

关键符号说明

$w_m$ vins中中值法， $w_t , w_{t+1}]$ 区间的测量值
$wm=wt+wt+12(0)\tag{0} w_{m} = \frac {w_t+w_{t+1}} {2}$
$a_m$ vins中中值法，[ $a_t , a_{t+1}]$ 区间的测量值
$am=qt(at+na0−bat)+qt+1(at+1+na1−bat)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}}$
$a_t$ $t$ 时刻的加速度测量值
$v_t$ $t$ 时刻的角速度测量值
$b_{at}$ 加速度在 $t$ 时刻的偏置
$b_{wt}$ 角速度在 $t$ 时刻的偏置
$n_{at}$ 加速度在 $t$ 时刻的高斯噪声
$n_{wt}$ 角速度在 $t$ 时刻的高斯噪声

0、离散系统的状态更新方程

$δpt+1=δpt+δvtδt+12amδ2t(2)\delta p_{t+1} = \delta p_t + \delta v_t \delta t + \frac 1 2 a_m\delta^2 t \tag{2}$
$wmδt}(3)\delta q_{t+1} = \delta q_t\otimes\big\{w_m\delta t\big\}\tag{3}$
$δvt+1=δvt+amδt(4)\delta v_{t+1} = \delta v_t + a_m \delta t \tag{4}$
$ba(t+1)=bat(5)b_{a(t+1)} = b_{at} \tag{5}$
$bw(t+1)=bwt(6)b_{w(t+1)} = b_{wt} \tag{6}$
预积分过程中， $b_{a}$ 是不变的。

1、 $δθt+1\delta\theta_{t+1}$ 的推导：

由参考文献2中的5.33节The error-state kinematics可知：
$δθt′=−[wm−bw]×δθt−δbw−nw(7)\tag{7}\delta\theta_t^{'}=-[w_m - b_w]_{\times}\delta\theta_t - \delta b_w - n_w$
公式 $(0)$ 带入公式 $(7)$ 可得：
$δθt′=−[wt+wt+12−bwt]×δθt−δbw−nw0+nw12(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+1=δθt+δθt′δt(9)\tag{9} \delta\theta_{t+1} = \delta\theta_t + \delta\theta_t^{'}\delta{t}$
$δθt+1=δθt+(−[wt+wt+12−bwt]×δθt−δbw−nw0+nw12)δt=(I−[wt+wt+12−bwt]×δt)δθt−δbwδt−nw0+nw12δ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}$