专栏首页科学计算VSLAM:IMU预积分公式推导

VSLAM:IMU预积分公式推导

VSLAM:IMU预积分公式推导

一、IMU预积分

 传统的递推算法是根据上一时刻的IMU状态量,利用当前时刻测量得到的加速度与角速度,进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中,各个状态量都是估计值,并且会不断调整,每次调整都会重新进行积分,传递IMU测量值。预积分的目的是将相对测量量与据对位姿解耦合,避免优化时重复进行积分。四元数的表示方法有两种:一种是Hamilton(右手系)表示,另一种是JPL(左手系)表示。读者对公式推导时一定注意。

 常用性质:

 1. 四元数的连续积分:

q_{b_{k+1}}^{w}=q_{b_{k}}^{w} \otimes \int_{t \in[k, k+1]} \dot{q}_{t} d t

 2. 四元数的左乘与右乘:

q_{a} \otimes q_{b}=\mathcal{R}\left(q_{b}\right) q_{a}=\left[\begin{array}{cccc} s_{b} & z_{b} & -y_{b} & x_{b} \\ -z_{b} & s_{b} & x_{b} & y_{b} \\ y_{b} & -x_{b} & s_{b} & z_{b} \\ -x_{b} & -y_{b} & -z_{b} & s_{b} \end{array}\right]\left[\begin{array}{c} x_{a} \\ y_{a} \\ z_{a} \\ s_{a} \end{array}\right] \\
=\mathcal{L}\left(q_{a}\right) q_{b}=\left[\begin{array}{cccc} s_{a} & -z_{a} & y_{a} & x_{a} \\ z_{a} & s_{a} & -x_{a} & y_{a} \\ -y_{a} & x_{a} & s_{a} & z_{a} \\ -x_{a} & -y_{a} & -z_{a} & s_{a} \end{array}\right]\left[\begin{array}{c} x_{b} \\ y_{b} \\ z_{b} \\ s_{b} \end{array}\right]

 我们将四元数表示为:

q=\left[\begin{array}{llll} x & y & z & s \end{array}\right]=\left[\begin{array}{ll} \omega & s \end{array}\right]

,则左右乘可以表示为:

\mathcal{R}(q)=\Omega(\omega)+s I_{4 \times 4}=\left[\begin{array}{cc} -\omega^{\wedge} & \omega \\ -\omega^{T} & 0 \end{array}\right]+s I_{4 \times 4}
\mathcal{L}(q)=\Psi(\omega)+s I_{4 \times 4}=\left[\begin{array}{cc} \omega^{\wedge} & \omega \\ -\omega^{T} & 0 \end{array}\right]+s I_{4 \times 4}

 3. 四元数导数如下:

\dot{q}_{t}=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left(q_{t+\delta t}-q_{t}\right)
=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left(q_{t} \otimes q_{t+\delta t}^{t}-q_{t} \otimes\left[\begin{array}{l} 0 \\ 1 \end{array}\right]\right)
=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left(q_{t} \otimes\left[\begin{array}{r} \widehat{\boldsymbol{k}} \sin \frac{\theta}{2} \\ \cos \frac{\theta}{2} \end{array}\right]-q_{t} \otimes\left[\begin{array}{l} 0 \\ 1 \end{array}\right]\right)
\approx \lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left(q_{t} \otimes\left[\begin{array}{c} \widehat{\boldsymbol{k}} \frac{\theta}{2} \\ 1 \end{array}\right]-q_{t} \otimes\left[\begin{array}{l} 0 \\ 1 \end{array}\right]\right)
=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left[\mathcal{R}\left(\left[\begin{array}{c} \widehat{\boldsymbol{k}} \frac{\theta}{2} \\ 1 \end{array}\right]\right)-\mathcal{R}\left(\left[\begin{array}{l} 0 \\ 1 \end{array}\right]\right)\right] q_{t}
=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left[\begin{array}{cc} -\frac{\theta^{\wedge}}{2} & \frac{\theta}{2} \\ -\frac{\theta^{T}}{2} & 0 \end{array}\right] q_{t}

 有角速度:

\omega=\lim _{\delta t \rightarrow 0} \frac{\theta}{\delta t}

,则导数可表示为:

\dot{q}_{t}=\frac{1}{2}\left[\begin{array}{cc} -\omega^{\wedge} & \omega \\ -\omega^{T} & 0 \end{array}\right] q_{t}=\frac{1}{2} \Omega(\omega) q_{t}=\frac{1}{2} \mathcal{R}\left(\left[\begin{array}{l} \omega \\ 0 \end{array}\right]\right) q_{t}=\frac{1}{2} q_{t} \otimes\left[\begin{array}{l} \omega \\ 0 \end{array}\right]

1.1 当前时刻的位置,速度,旋转变量的连续表达式

我们将图像帧记作

k

k+1

,body坐标系下记作

b_k

b_{k+1}

,我们将位置,速度和旋转在时间

t_k

t_{k+1}

内进行积分,其世界坐标系下的公式可以写为:

p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}+\iint_{t \in[k, k+1]}\left[R_{t}^{w}\left(\hat{a}_{t}-b_{a_{t}}\right)-g^{w}\right] d t^{2}
v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\int_{t \in[k, k+1]}\left[R_{t}^{w}\left(\hat{a}_{t}-b_{a_{t}}\right)-g^{w}\right] d t
q_{b_{k+1}}^{w}=q_{b_{k}}^{w} \otimes \int_{t \in[k, k+1]} \frac{1}{2} \Omega\left(\widehat{\omega}_{t}-b_{\omega_{t}}\right) q_{t}^{b_{k}} d t

1.2 当前时刻的位置,速度,旋转变量的离散表达式

 我们以中值积分给出离散表示:

p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2} \bar{a}_{l} \delta t^{2}
v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\bar{a}_{l} \delta t
q_{b_{k+1}}^w=q_{b_k}^w \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\bar{\hat w_l}\delta t\end{bmatrix}
\bar{a}_{l}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}}\right)-g^{w}+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i}}\right)-g^{w}\right]
\overline{\widehat{\omega}_{l}}=\frac{1}{2}\left(\widehat{\omega}_{i}+\widehat{\omega}_{i+1}\right)-b_{\omega_{i}}

1.3 两帧之间的位置,速度,旋转增量的连续表达式

 基本思想就是将参考坐标系从

w

转到第

k

帧的body坐标系下,相当于两边同时乘

R_w^{b_k}

,我们直接用论文中的公式来表示:

R_{w}^{b_{k}} p_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}-\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)+\alpha_{b_{k+1}}^{b_{k}}
R_{w}^{b_{k}} v_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(v_{b_{k}}^{w}-g^{w} \Delta t_{k}\right)+\beta_{b_{k+1}}^{b_{k}}
q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w}=\gamma_{b_{k+1}}^{b_{k}}

 其中:

\alpha_{b_{k+1}}^{b_{k}}=\iint_{t \in[k, k+1]}\left[R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)\right] d t^{2}
\beta_{b_{k+1}}^{b_{k}}=\int_{t \in[k, k+1]}\left[R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)\right] d t
\gamma_{b_{k+1}}^{b_{k}}=\int_{t \in[k, k+1]} \frac{1}{2} \Omega\left(\widehat{\omega}_{t}-b_{\omega_{t}}\right) \gamma_{t}^{b_{k}} d t

 上述可以理解为

b_{k+1}

b_k

的相对运动量,其中

b_k

状态的改变并不会对其产生影响,可以将其作为非线性优化变量,避免重复计算。实际到这里,只要求解出积分,我们就完成了预积分的计算,我们的目标也就是在此。实际当中随机游走也是发生改变的,所以我们将上述变量再次进行一阶近似,我们再次使用论文中的公式进行表示:

\alpha_{b_{k+1}}^{b_{k}} \approx \hat{\alpha}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\alpha} \delta b_{a}+J_{b_{\omega}}^{\alpha} \delta b_{\omega}
\beta_{b_{k+1}}^{b_{k}} \approx \hat{\beta}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\beta} \delta b_{a}+J_{b_{\omega}}^{\beta} \delta b_{\omega}
\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} J_{b_{\omega}}^{\gamma} \delta b_{\omega} \end{array}\right]

 至此,IMU的预积分表达式我们就已经得到了。

1.4 两帧之间的位置,速度,旋转增量的离散表达式

\hat{\alpha}_{i+1}^{b_{k}}=\hat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} \bar{a}_{l}^{\prime} \delta t^{2}
\hat{\beta}_{i+1}^{b_{k}}=\hat{\beta}_{i}^{b_{k}}+\overline{\hat{a}}_{l}^{\prime} \delta t
\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \widehat{\omega_{l}}^{\prime} \delta t \end{array}\right] \\
\overline{\hat{a}}_{i}^{\prime}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}}\right)+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i}}\right)\right]
\overline{\widehat{\omega}_{\imath}}^{\prime}=\frac{1}{2}\left(\widehat{\omega}_{i}+\widehat{\omega}_{i+1}\right)-b_{\omega_{i}}

1.5 连续表达式下的位置、速度、旋转增量误差、协方差、Jacobian

 IMU在每一个时刻积分出来的变量都是有误差的,我们针对误差进行分析,分析误差的传递方程,下面为连续时间下的误差导数方程:

\left[\begin{array}{l} \delta \dot{\alpha}_{t}^{b_{k}} \\ \delta \dot{\beta}_{t}^{b_{k}} \\ \delta \dot{\theta}_{t}^{b_{k}} \\ \delta \dot{b}_{a_{t}} \\ \delta \dot{b}_{w_{t}} \end{array}\right]=\left[\begin{array}{ccccc} 0 & I & 0 & 0 & 0 \\ 0 & 0-R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)^{\wedge} & -R_{t}^{b_{k}} & 0 & 0 \\ 0 & 0 & -\left(\widehat{\omega}_{t}-b_{\omega_{t}}\right)^{\wedge} & 0 & -I \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \end{array}\right]\left[\begin{array}{c} \delta \alpha_{t}^{b_{k}} \\ \delta \beta_{t}^{b_{k}} \\ \delta \theta_{t}^{b_{k}} \\ \delta b_{a_{t}} \\ \delta b_{w_{t}} \end{array}\right]+\left[\begin{array}{llll} 0 & 0 & 0 & 0 \\ -R_{t}^{b_{k}} & 0 & 0 & 0 \\ 0 & -I & 0 & 0 \\ 0 & 0 & I & 0 \\ 0 & 0 & 0 & I \end{array}\right]\left[\begin{array}{c} n_{a} \\ n_{w} \\ n_{b_{a}} \\ n_{b_{w}} \end{array}\right]
=F_{t} \delta z_{t}^{b_{k}}+G_{t} n_{t}

 我们对

\delta \dot{\beta}_{t}^{b_{k}}

进行推导,假设true表示真实测量值,含误差,nominal表示不含噪声的理论值,则有:

\delta \dot{\beta}=\dot{\beta}_{\text {true }}-\dot{\beta}_{\text {nominal }}

 其中:

\dot{\beta}_{\text {true }}=R_{t_{\text {true }}}^{b_{k}}\left(\hat{a}_{t_{\text {true }}}-b_{a_{t} \text { true }}\right)=R_{t}^{b_{k}} \exp \left(\delta \theta^{\wedge}\right)\left(\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}\right)\\
=R_{t}^{b_{k}}\left(I+\delta \theta^{\wedge}\right)\left(\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}\right)
=R_{t}^{b_{k}}\left[\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}+\delta \theta^{\wedge}\left(\hat{a}_{t}-b_{a_{t}}\right)\right]
=R_{t}^{b_{k}}\left[\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}-\left(\hat{a}_{t}-b_{a_{t}}\right)^{\wedge} \delta \theta\right]
\dot{\beta}_{\text {nominal }}=R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)

 则:

\delta \dot{\beta}=\dot{\beta}_{\text {true }}-\dot{\beta}_{\text {nominal }}=-R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)^{\wedge} \delta \theta-R_{t}^{b_{k}} \delta b_{a_{t}}-R_{t}^{b_{k}} n_{a}

 我们再对

\delta \dot{\theta}_{t}^{b_{k}}

进行推导:

\dot{q}_{t_{t r u e}}=\frac{1}{2} q_{t_{t r u e}} \otimes\left[\begin{array}{c} \omega_{t r u e} \\ 0 \end{array}\right]=\frac{1}{2} q_{t} \otimes \delta q \otimes\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\ 0 \end{array}\right]
\dot{q}_{t_{\text {nominal }}}=\dot{q}_{t}=\frac{1}{2} q_{t} \otimes\left[\begin{array}{c} \hat{\omega}_{t}-b_{\omega_{t}} \\ 0 \end{array}\right]

 根据四元数的导数性质:

\dot{q}_{t_{t r u e}}=\left(q_{t} \dot{\otimes} \delta q\right)=\dot{q}_{t} \otimes \delta q+q_{t} \otimes \dot{\delta q}
=\frac{1}{2} q_{t} \otimes\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}} \\ 0 \end{array}\right] \otimes \delta q+q_{t} \otimes \dot{\delta q}

 我们将上面的等式进行汇总:

\frac{1}{2} q_{t} \otimes \delta q \otimes\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\ 0 \end{array}\right]=\frac{1}{2} q_{t} \otimes\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}} \\ 0 \end{array}\right] \otimes \delta q+q_{t} \otimes \dot{\delta q} \\
\frac{1}{2} \delta q \otimes\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\ 0 \end{array}\right]=\frac{1}{2}\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}} \\ 0 \end{array}\right] \otimes \delta q+\dot{\delta q} \\
2 \dot{\delta q}=\delta q \otimes\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\ 0 \end{array}\right]-\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}} \\ 0 \end{array}\right] \otimes \delta q \\
2 \dot{\delta q}=\mathcal{R}\left(\left[\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\ 0 \end{array}\right]\right) \delta q-\mathcal{L}\left(\begin{array}{c} \widehat{\omega}_{t}-b_{\omega_{t}} \\ 0 \end{array}\right) \delta q \\
2 \dot{\delta} q=\left[\begin{array}{cc} -\left(2 \widehat{\omega}_{t}-2 b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}}\right)^{\wedge} & -n_{\omega}-\delta b_{\omega_{t}} \\ \left(n_{\omega}+\delta b_{\omega_{t}}\right)^{T} & 0 \end{array}\right]\left[\begin{array}{l} \frac{\delta \theta}{2} \\ 1 \end{array}\right]

 最终:

\dot{\delta} \theta=-\left(2 \widehat{\omega}_{t}-2 b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}}\right)^{\wedge} \frac{\delta \theta}{2}-n_{\omega}-\delta b_{\omega_{t}}
\approx-\left(\widehat{\omega}_{t}-b_{\omega_{t}}\right)^{\wedge} \delta \theta-n_{\omega}-\delta b_{\omega_{t}}

 我们将导数的定义拿来:

\delta \dot{z}_{t}^{b_{k}}=\lim _{\delta t \rightarrow 0} \frac{\delta z_{t+\delta t}^{b_{k}}-\delta z_{t}^{b_{k}}}{\delta t}

 则:

\delta z_{t+\delta t}^{b_{k}}=\delta z_{t}^{b_{k}}+\delta \dot{z}_{t}^{b_{k}} \delta t=\left(\mathrm{I}+F_{t} \delta t\right) \delta z_{t}^{b_{k}}+\left(G_{t} \delta t\right) n_{t}

 对比扩展卡尔曼公式,我们发现其正好是非线性系统进行线性的表达式,据此,我们给出相似的协方差预测公式:

P_{t+\delta t}^{b_{k}}=\left(\mathrm{I}+F_{t} \delta t\right) P_{t}^{b_{k}}\left(\mathrm{I}+F_{t} \delta t\right)^{T}+\left(G_{t} \delta t\right) Q\left(G_{t} \delta t\right)^{T}

 其中,协方差初始值为0,噪声协方差矩阵可以表示为:

Q^{12 \times 12}=\left[\begin{array}{cccc} \sigma_{a}^{2} & 0 & 0 & 0 \\ 0 & \sigma_{w}^{2} & 0 & 0 \\ 0 & 0 & \sigma_{b_{a}}^{2} & 0 \\ 0 & 0 & 0 & \sigma_{b_{w}}^{2} \end{array}\right]

 类似,我们也可以获得误差的Jacobian迭代公式:

J_{t+\delta t}=\left(\mathrm{I}+F_{t} \delta t\right) J_{t}

 Jacobian的初始值为单位矩阵。

1.6 离散形式的增量分析

 实际只需根据中值积分,将连续形式表达式进行离散化即可,推导过程省略,比较简单

\left[\begin{array}{l} \delta \alpha_{k+1} \\ \delta \theta_{k+1} \\ \delta \beta_{k+1} \\ \delta b_{a_{k+1}} \\ \delta b_{w_{k+1}} \end{array}\right]=\left[\begin{array}{ccccc} I & f_{01} & \delta t & f_{03} & f_{04} \\ 0 & f_{11} & 0 & 0 & -\delta t \\ 0 & f_{21} & I & f_{23} & f_{24} \\ 0 & 0 & 0 & I & 0 \\ 0 & 0 & 0 & 0 & I \end{array}\right]\left[\begin{array}{l} \delta \alpha_{k} \\ \delta \theta_{k} \\ \delta \beta_{k} \\ \delta b_{a_{k}} \\ \delta b_{w_{k}} \end{array}\right]
+\left[\begin{array}{cccccc} v_{00} & v_{01} & v_{02} & v_{03} & 0 & 0 \\ 0 & \frac{-\delta t}{2} & 0 & \frac{-\delta t}{2} & 0 & 0 \\ -\frac{R_{k} \delta t}{2} & v_{21} & -\frac{R_{k+1} \delta t}{2} & v_{23} & 0 & 0 \\ 0 & 0 & 0 & 0 & \delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & \delta t \end{array}\right]\left[\begin{array}{c} n_{a_{k}} \\ n_{w_{k}} \\ n_{a_{k+1}} \\ n_{w_{k+1}} \\ n_{b_{a}} \\ n_{b_{w}} \end{array}\right]

 其中:

f_{01}=\frac{\delta t}{2} f_{21}=-\frac{1}{4} R_{k}\left(\hat{a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t^{2}-\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t^{2}
f_{03}=-\frac{1}{4}\left(R_{k}+R_{k+1}\right) \delta t^{2}
f_{04}=\frac{\delta t}{2} f_{24}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{3}
f_{11}=I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t
f_{21}=-\frac{1}{2} R_{k}\left(\hat{a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t-\frac{1}{2} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t
f_{23}=-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta t
f_{23}=-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta t
f_{24}=\frac{1}{2} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2}
v_{00}=-\frac{1}{4} R_{k} \delta t^{2}
v_{01}=v_{03}=\frac{\delta t}{2} v_{21}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2} \frac{\delta t}{2}
v_{02}=-\frac{1}{4} R_{k+1} \delta t^{2}
v_{21}=v_{23}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2}

 离散误差传递方程可以简写为:

\delta z_{k+1} 15 \times 1=F^{15 \times 15} \delta z_{k}^{15 \times 1}+V^{15 \times 18} Q^{18 \times 1}

 则Jacobian的迭代公式为:

J_{k+1}{ }^{15 \times 15}=F J_{k}

 Jacobian的初始值为:

J_{k}=I

。注意,我们在此计算Jacobian,仅仅是为了后端非线性优化过程对bias的计算提供帮助。

 协方差的迭代公式:

P_{k+1}{ }^{15 \times 15}=F P_{k} F^{T}+V Q V^{T}

 协方差矩阵初始值为0,噪声的协方差矩阵为:

Q^{18 \times 18}=\left[\begin{array}{cccccc} \sigma_{a}^{2} & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{w}^{2} & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{a}^{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{w}^{2} & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{b_{a}}^{2} & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{b_{w}}^{2} \end{array}\right]

本文分享自微信公众号 - 科学计算technomania(Quant_Times),作者:大亮

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2021-04-29

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • VSLAM:预积分公式推导(一)

     传统的递推算法是根据上一时刻的IMU状态量,利用当前时刻测量得到的加速度与角速度,进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中,各个状态量都...

    猫叔Rex
  • SVM公式推导笔记

    参考资料: 对偶函数-http://blog.pluskid.org/?p=702 KTT和拉格朗日乘子-http://www.cnblogs.com/zhan...

    Gxjun
  • BP算法的公式推导

    版权声明:本文为博主原创文章,遵循 CC 4.0 by-sa 版权协议,转载请附上原文出处链接和本声明。

    张凝可
  • 数值积分|泰勒(Taylor)公式求积分

    泰勒(Taylor)公式大致可以叙述为:函数在一个点的邻域内的值可以用函数在该点的值及各阶导数值组成的无穷级数表示出来。ƒ(x)在x=a处的泰勒展开式为:

    fem178
  • 高等数学-积分公式

    \[\int \cot{x}dx = \ln{|\sin{x}|}dx + c \]

    Kindear
  • 数值积分|自适应辛普森积分公式

    也就是说至少要三个积分点,两个积分子区间。所以,自适应辛普森积分公式要从S1起步,即

    fem178
  • 公积金贷款逾期预测Baseline分享

    今天老肥和大家分享的是山东省第二届数据应用创新创业大赛-日照分赛场-公积金贷款逾期预测的baseline, 这个赛题是结构化数据的分类问题,相对来说入门门槛较低...

    老肥码码码
  • 数值积分| 辛普森公式

    辛普森积分法是一种用抛物线近似函数曲线来求定积分数值解的方法。把积分区间等分成若干段,对被积函数在每一段上使用辛普森公式,根据其在每一段的两端和中点处的取值近似...

    fem178
  • 全连接网络到卷积神经网络逐步推导(组图无公式)

    摘要: 在图像分析中,卷积神经网络(Convolutional Neural Networks, CNN)在时间和内存方面优于全连接网络(Full Connec...

    朱晓霞
  • 全连接网络到卷积神经网络逐步推导(组图无公式)

    摘要: 在图像分析中,卷积神经网络(Convolutional Neural Networks, CNN)在时间和内存方面优于全连接网络(Full Connec...

    小莹莹
  • 梯度下降算法中的偏导公式推导

    前言:最近在跟着吴恩达老师(Andrew Ng)的视频课程学习机器学习,该视频是2014年拍的,虽然有点老,但理论却并不过时,是非常经典的机器学习入门教程,也正...

    阿波张
  • 储备一些微分与积分公式

    看、未来
  • Focal Loss 论文理解及公式推导

    精度最高的目标检测器往往基于 RCNN 的 two-stage 方法,对候选目标位置再采用分类器处理. 而,one-stage 目标检测器是对所有可能的目标位置...

    AIHGF
  • HDOJ(HDU) 2524 矩形A + B(推导公式、)

    Problem Description 给你一个高为n ,宽为m列的网格,计算出这个网格中有多少个矩形,下图为高为2,宽为4的网格.

    谙忆
  • 数理统计----协方差公式推导

    协方差公式推导 cov(X,Y)=∑ni=1(Xi−X¯)(Yi−Y¯)n=E[(X−E[X])(Y−E[Y])] cov(X,Y)=∑i=1n(Xi−X¯...

    葆宁
  • python各种推导式分析

    推导式comprehensions(又称解析式),是Python的一种独有特性。推导式是可以从一个数据序列构建另一个新的数据序列的结构体。 共有三种推导,在Py...

    py3study
  • 梯度下降法公式推导过程--再次补充:导数部分化简

    前面一篇就是基础性的推导过程。从反馈的情况看,总体还是讲明白了。但是在导数的部分,仍有不少的存疑。 其实在数学方面,我也是学渣。所以尽我所能,希望再次的补...

    俺踏月色而来
  • 【SLAM】苏黎世大学机器人和感知小组—IMU的预积分模型理论推导,VINS算法的IMU预积分模型理论

    论文地址: http://rpg.ifi.uzh.ch/docs/RSS15_Forster.pdf

    CNNer
  • RNN的简单的推导演算公式(BPTT)

    附上y=2x-b拟合的简单的代码. 1 import numpy as np 2 x = np.asarray([2,1,3,5,6]); 3 y = n...

    Gxjun

扫码关注云+社区

领取腾讯云代金券