详解惯性导航论文 RINS-W: Robust Inertial Navigation System on Wheels

在这里插入图片描述
本文介绍一篇惯性导航定位论文 RINS-W,论文发表于 IROS2019。在本论文中作者提出了仅使用一个IMU进行长时间惯性导航的方法。方法主要包括两个部分:

  • 检测器使用循环神经网络来检测IMU的运动状况,如零速或零横向滑移;
  • 使用Invariant Extended Kalman Filter结合检测器的输出(作为伪测量)来进行定位。

在公开数据集上的测试结果显示,在行驶超过21km之后,最终定位误差为20m(如下图所示)。
在这里插入图片描述

论文链接:https://arxiv.org/pdf/1903.02210.pdf

github 链接:https://github.com/mbrossar/RINS-W


1. Inertial Navigation System & Sensor Model

首先回顾下惯性导航方程,IMU方向用 Rn∈SO(3)\mathbf{R}_n \in SO(3)RnSO(3) 表示,表示从载体坐标到世界坐标的旋转变换;世界坐标系中速度为 vnw∈R3\mathbf{v}_{n}^{\mathrm{w}} \in \mathbb{R}^{3}vnwR3,世界坐标系中位置为 pnw∈R3\mathbf{p}_{n}^{\mathrm{w}} \in \mathbb{R}^{3}pnwR3,则运动方程可以写为:
Rn+1=Rnexp⁡SO(3)(ωndt)vn+1w=vnW+(Rnan+g)dtpn+1w=pnw+vnwdt\begin{aligned} \mathbf{R}_{n+1} &=\mathbf{R}_{n} \exp _{S O(3)}\left(\boldsymbol{\omega}_{n} d t\right) \\ \mathbf{v}_{n+1}^{\mathrm{w}} &=\mathbf{v}_{n}^{\mathrm{W}}+\left(\mathbf{R}_{n} \mathbf{a}_{n}+\mathbf{g}\right) d t \\ \mathbf{p}_{n+1}^{\mathrm{w}} &=\mathbf{p}_{n}^{\mathrm{w}}+\mathbf{v}_{n}^{\mathrm{w}} d t \end{aligned} Rn+1vn+1wpn+1w=RnexpSO(3)(ωndt)=vnW+(Rnan+g)dt=pnw+vnwdt

其中 dtdtdt 时采样时间,(R0,v0w,p0w)(\mathbf{R}_0,\mathbf{v}_{0}^{\mathrm{w}},\mathbf{p}_{0}^{\mathrm{w}})(R0,v0w,p0w) 是初始状态。在本论文中,没有考虑地球自转和科氏力的影响。

下面回顾下IMU模型,IMU加速度和角速度模型可以写为:
ωnIMU=ωn+bnω+wnωanIMU=an+bna+wna\begin{aligned} &\boldsymbol{\omega}_{n}^{\mathrm{IMU}}=\boldsymbol{\omega}_{n}+\mathbf{b}_{n}^{\boldsymbol{\omega}}+\mathbf{w}_{n}^{\boldsymbol{\omega}} \\ &\mathbf{a}_{n}^{\mathrm{IMU}}=\mathbf{a}_{n}+\mathbf{b}_{n}^{\mathbf{a}}+\mathbf{w}_{n}^{\mathbf{a}} \end{aligned} ωnIMU=ωn+bnω+wnωanIMU=an+bna+wna

其中,bnω,bna\mathbf{b}_{n}^{\boldsymbol{\omega}},\mathbf{b}_{n}^{\mathbf{a}}bnω,bna为角速度偏差和加速度偏差,wnω,wna\mathbf{w}_{n}^{\boldsymbol{\omega}},\mathbf{w}_{n}^{\mathbf{a}}wnωwna 为高斯噪声。角速度偏差和加速度偏差方程可以写为:
bn+1ω=bnω+wnbωbn+1a=bna+wnba\begin{aligned} \mathbf{b}_{n+1}^{\boldsymbol{\omega}}=\mathbf{b}_{n}^{\boldsymbol{\omega}}+\mathbf{w}_{n}^{\mathbf{b}_{\boldsymbol{\omega}}} \\ \mathbf{b}_{n+1}^{\mathbf{a}}=\mathbf{b}_{n}^{\mathbf{a}}+\mathbf{w}_{n}^{\mathbf{b}_{\mathbf{a}}} \end{aligned} bn+1ω=bnω+wnbωbn+1a=bna+wnba

其中,wnbω,wnba\mathbf{w}_{n}^{\mathbf{b}_{\boldsymbol{\omega}}},\mathbf{w}_{n}^{\mathbf{b}_{\mathbf{a}}}wnbω,wnba 为随机游走噪声。

在惯性导航中,偏差的准确估计是至关重要的,即使很小的误差,也会导致很大的位置估计偏差。


2. Specific Motion Profiles For Wheeled Systemd

在这一节,作者介绍了几种常见的运动特性,它们往往会提供有用的互补信息。

首先是四种特定的运动情况,将它们编码为向量形式为:
zn=(znVEL,znANG,znLAT,znUP)∈{0,1}4\mathbf{z}_{n}=\left(z_{n}^{\mathrm{VEL}}, z_{n}^{\mathrm{ANG}}, z_{n}^{\mathrm{LAT}}, \quad z_{n}^{\mathrm{UP}}\right) \in\{0,1\}^{4} zn=(znVEL,znANG,znLAT,znUP){0,1}4

其中:

  • Zero velocity,当 znVEL=1z_n^{\mathrm{VEL}}=1znVEL=1 时,则有{vn=0Rnan+g=0\left\{\begin{array}{c}\mathbf{v}_{n}=\mathbf{0} \\ \mathbf{R}_{n} \mathbf{a}_{n}+\mathbf{g}=\mathbf{0}\end{array}\right.{vn=0Rnan+g=0,当检测到零速时,常会使用 ZUPT 算法进行更新。
  • Zero angular velocity,当 znANG=1z_n^{\mathrm{ANG}}=1znANG=1 时,则有 ωn=0\boldsymbol{\omega}_{n}=\mathbf{0}ωn=0
  • Zero lateral velocity,当 znLAT=1z_n^{\mathrm{LAT}}=1znLAT=1 时,则有 vnLAT≃0v_{n}^{\mathrm{LAT}} \simeq 0vnLAT0,载体坐标速度和世界坐标速度转换方程为:vnB=RnTvnW=[vnFORvnLATvnUP]\mathbf{v}_{n}^{\mathrm{B}}=\mathbf{R}_{n}^{T} \mathbf{v}_{n}^{\mathrm{W}}=\left[\begin{array}{c}v_{n}^{\mathrm{FOR}} \\ v_{n}^{\mathrm{LAT}} \\ v_{n}^{\mathrm{UP}}\end{array}\right]vnB=RnTvnW=vnFORvnLATvnUP
  • Zero vertical velocity,当 znUP=1z_n^{\mathrm{UP}}=1znUP=1 时,则有 vnUP≃0v_{n}^{\mathrm{UP}} \simeq 0vnUP0

其中,后两种运动情况经常用在轮速机器人或汽车运动中。以上四种情形中,零速约束(零速度和零角速度)用来修正IMU偏差和姿态俯仰角;零横向速度和垂直速度用来长期估计汽车位置(后面的实验会进行说明)


3. Proposed RINS-W Algorithm

本文提出的方法如下图所示,由两部分组成:

  • Detector 由循环神经网络组成,根据IMU测量值来输出二元向量 zn\mathbf{z}_nzn
  • IEKF 是一种新的卡尔曼滤波器,输入为IMU测量值和检测器输出(作为伪测量),对状态量进行估计;
    在这里插入图片描述

3.1 Specific Motion Profile Detector

Detector 会决定在每一个时刻 nnn 二元向量 zn\mathbf{z}_nzn中有几个元素是有效的,即有几种运动形式会发生,结构如下图所示。检测器的核心模块为LSTM,输入为IMU测量值,计算方程为:
u^n+1,hn+1=LSTM⁡({ωiIMU,aiIMU}i=0n)=LSTM⁡(ωnIMU,anIMU,hn)\begin{aligned} \hat{\mathbf{u}}_{n+1}, \mathbf{h}_{n+1} &=\operatorname{LSTM}\left(\left\{\boldsymbol{\omega}_{i}^{\mathrm{IMU}}, \mathbf{a}_{i}^{\mathrm{IMU}}\right\}_{i=0}^{n}\right) \\ &=\operatorname{LSTM}\left(\boldsymbol{\omega}_{n}^{\mathrm{IMU}}, \mathbf{a}_{n}^{\mathrm{IMU}}, \mathbf{h}_{n}\right) \end{aligned} u^n+1,hn+1=LSTM({ωiIMU,aiIMU}i=0n)=LSTM(ωnIMU,anIMU,hn)

其中,u^n+1∈R4\hat{\mathbf{u}}_{n+1} \in \mathbb{R^4}u^n+1R4 包含每一个运动情形的概率值,hn\mathbf{h}_nhn 是神经网络隐藏状态,概率值最后经过阈值运算来得到二元向量 z^n=Threshold(u^n+1)\hat{\mathbf{z}}_n=\mathrm{Threshold} \left(\hat{\mathbf{u}}_{n+1}\right)z^n=Threshold(u^n+1)
在这里插入图片描述


3.2 The Invariant Extended Kalman Filter(重点)

在本文中,作者选择使用IEKF而不是传统的EKF来作为进行状态估计,如下图所示,二元变量 z^n\hat{\mathbf{z}}_nz^n 一方面会用于状态传播,另一方面会用于状态更新。

在这里插入图片描述
(1)首先定义IMU状态,IMU状态量为:xn=(Rn,vnw,pnw,bnω,bna)\mathbf{x}_{n}=\left(\mathbf{R}_{n}, \mathbf{v}_{n}^{\mathrm{w}}, \mathbf{p}_{n}^{\mathrm{w}}, \mathbf{b}_{n}^{\boldsymbol{\omega}}, \mathbf{b}_{n}^{\mathbf{a}}\right)xn=(Rn,vnw,pnw,bnω,bna)线性状态误差为:en=[ξnenb]∼N(0,Pn)\mathbf{e}_{n}=\left[\begin{array}{c}\boldsymbol{\xi}_{n} \\ \mathbf{e}_{n}^{\mathbf{b}}\end{array}\right] \sim \mathcal{N}\left(\mathbf{0}, \mathbf{P}_{n}\right)en=[ξnenb]N(0,Pn),状态更新方程为:
χn=exp⁡SE2(3)(ξn)χ^nbn=b^n+enb,\begin{aligned} &\boldsymbol{\chi}_{n}=\exp _{S E_{2}(3)}\left(\boldsymbol{\xi}_{n}\right) \hat{\boldsymbol{\chi}}_{n} \\ &\mathbf{b}_{n}=\hat{\mathbf{b}}_{n}+\mathbf{e}_{n}^{\mathbf{b}}, \end{aligned} χn=expSE2(3)(ξn)χ^nbn=b^n+enb,

其中,χn∈SE2(3)\boldsymbol{\chi}_{n} \in SE_2(3)χnSE2(3),表示为汽车状态 (Rn,vnw,pnw)(\mathbf{R}_{n}, \mathbf{v}_{n}^{\mathrm{w}}, \mathbf{p}_{n}^{\mathrm{w}})(Rn,vnw,pnw) 在李群上的形式,误差状态协方差矩阵为 Pn∈R15×15\mathbf{P}_{n} \in \mathbb{R}^{15 \times 15}PnR15×15。偏差 bn=(bnω,bna)∈R6\mathbf{b}_n=({ \mathbf{b}_{n}^{\boldsymbol{\omega}}, \mathbf{b}_{n}^{\mathbf{a}})} \in \mathbb{R}^6bn=(bnω,bna)R6,偏差估计量为 b^n=(b^nω,b^na)∈R6\hat{\mathbf{b}}_{n}=\left(\hat{\mathbf{b}}_{n}^{\omega}, \hat{\mathbf{b}}_{n}^{\mathbf{a}}\right) \in \mathbb{R}^{6}b^n=(b^nω,b^na)R6

(2)预测部分。当没有上述四种之一的运动情况被检测到时,使用第1节介绍的运动方程来计算新的状态量和协方差,协方差计算方程为:
Pn+1=FnPnFnT+GnQnGnT\mathbf{P}_{n+1}=\mathbf{F}_{n} \mathbf{P}_{n} \mathbf{F}_{n}^{T}+\mathbf{G}_{n} \mathbf{Q}_{n} \mathbf{G}_{n}^{T} Pn+1=FnPnFnT+GnQnGnT

雅可比矩阵 Fn,Gn\mathbf{F}_n,\mathbf{G}_nFn,Gn 将会在第5节进行介绍。Qn\mathbf{Q}_nQn 表示为噪声协方差矩阵,噪声为 wn=(wnω,wna,wnbω,wnba)∼N(0,Qn)\mathbf{w}_{n}=\left(\mathbf{w}_{n}^{\boldsymbol{\omega}}, \mathbf{w}_{n}^{\mathbf{a}}, \mathbf{w}_{n}^{\mathbf{b} \omega}, \mathbf{w}_{n}^{\mathbf{b}^{\mathrm{a}}}\right) \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_{n}\right)wn=(wnω,wna,wnbω,wnba)N(0,Qn)

如果由特定的运动情况被检测到,将会按照下面的方程进行状态量修改,零速时(速度归0,位置不变,这里的0表示速度为0,原论文中作者未明确表示)
z^n+1vEL=1⇒{vn+1w=vnw=0pn+1w=pnw\hat{z}_{n+1}^{\mathrm{vEL}}=1 \Rightarrow\left\{\begin{array}{l} \mathbf{v}_{n+1}^{\mathrm{w}}=\mathbf{v}_{n}^{\mathrm{w}} =0 \\ \mathbf{p}_{n+1}^{\mathrm{w}}=\mathbf{p}_{n}^{\mathrm{w}} \end{array}\right. z^n+1vEL=1{vn+1w=vnw=0pn+1w=pnw

零角速度时(姿态不变)
z^n+1ANG=1⇒Rn+1=Rn\hat{z}_{n+1}^{\mathrm{ANG}}=1 \Rightarrow \mathbf{R}_{n+1}=\mathbf{R}_{n} z^n+1ANG=1Rn+1=Rn

同时,状态估计量和协方差矩阵也要响应进行修改。

(3)更新。每一个运动情形将会产生下列的伪测量
yn+1VEL=[Rn+1Tvn+1Wbn+1a−Rn+1Tg]=[0anIMU]yn+1ANG=bn+1ω=ωnIMUyn+1LAT=vn+1LAT=0yn+1UP=vn+1UP=0\begin{aligned} &\mathbf{y}_{n+1}^{\mathrm{VEL}}=\left[\begin{array}{c} \mathbf{R}_{n+1}^{T} \mathbf{v}_{n+1}^{\mathrm{W}} \\ \mathbf{b}_{n+1}^{\mathbf{a}}-\mathbf{R}_{n+1}^{T} \mathbf{g} \end{array}\right]=\left[\begin{array}{c} \mathbf{0} \\ \mathbf{a}_{n}^{\mathrm{IMU}} \end{array}\right] \\ &\mathbf{y}_{n+1}^{\mathrm{ANG}}=\mathbf{b}_{n+1}^{\boldsymbol{\omega}}=\boldsymbol{\omega}_{n}^{\mathrm{IMU}} \\ &\mathbf{y}_{n+1}^{\mathrm{LAT}}=v_{n+1}^{\mathrm{LAT}}=0 \\ &\mathbf{y}_{n+1}^{\mathrm{UP}}=v_{n+1}^{\mathrm{UP}}=0 \end{aligned} yn+1VEL=[Rn+1Tvn+1Wbn+1aRn+1Tg]=[0anIMU]yn+1ANG=bn+1ω=ωnIMUyn+1LAT=vn+1LAT=0yn+1UP=vn+1UP=0

更新方程为:
K=Pn+1Hn+1T/(Hn+1Pn+1Hn+1T+Nn+1)e+=K(yn+1−y^n+1)=[ξ+eb+]χ^n+1+=exp⁡(ξ+)χ^n+1,bn+1+=bn+1+eb+Pn+1+=(I−KHn+1)Pn+1\begin{aligned} \mathbf{K} &=\mathbf{P}_{n+1} \mathbf{H}_{n+1}^{T} /\left(\mathbf{H}_{n+1} \mathbf{P}_{n+1} \mathbf{H}_{n+1}^{T}+\mathbf{N}_{n+1}\right) \\ \mathbf{e}^{+} &=\mathbf{K}\left(\mathbf{y}_{n+1}-\hat{\mathbf{y}}_{n+1}\right)=\left[\begin{array}{c} \boldsymbol{\xi}^{+} \\ \mathbf{e}^{\mathbf{b}+} \end{array}\right] \\ \hat{\boldsymbol{\chi}}_{n+1}^{+} &=\exp \left(\boldsymbol{\xi}^{+}\right) \hat{\boldsymbol{\chi}}_{n+1}, \mathbf{b}_{n+1}^{+}=\mathbf{b}_{n+1}+\mathbf{e}^{\mathbf{b}+} \\ \mathbf{P}_{n+1}^{+} &=\left(\mathbf{I}-\mathbf{K H}_{n+1}\right) \mathbf{P}_{n+1} \end{aligned} Ke+χ^n+1+Pn+1+=Pn+1Hn+1T/(Hn+1Pn+1Hn+1T+Nn+1)=K(yn+1y^n+1)=[ξ+eb+]=exp(ξ+)χ^n+1,bn+1+=bn+1+eb+=(IKHn+1)Pn+1

其中,K\mathbf{K}K 为卡尔曼增益矩阵,Hn+1\mathbf{H}_{n+1}Hn+1 是测量雅可比矩阵(具体形式见第5节)。

(4)初始化。为了正确估计偏差和方向,在开始阶段,将会强制静止1秒钟用于估计偏差和俯仰角。


4. Results On Car Dataset

首先是数据集的介绍,使用的数据集为comples urban LiDAR Dataset,IMU如下图所示。
在这里插入图片描述


4.1 Implementation Details

下面是实现细节,detector由4个LSTMs组成,每一个LSTM由2个隐藏层(每层250个隐藏单元)组成,然后是2层多层感知机,最后是sigmoid函数。阈值设定为:0.95 (znVEL,znANG)(z_n^{\mathrm{VEL}},z_n^{\mathrm{ANG}})znVEL,znANG,0.5 (znLAT,znUP)(z_n^{\mathrm{LAT}},z_n^{\mathrm{UP}})znLAT,znUP;滤波器工作频率为100Hz,噪声协方差矩阵为:
Qn=diag⁡(σω2I,σa2I,σbω2I,σba2I)Nn=diag⁡(σVEL,v2I,σVEL,a2I,σANG2I,σLAT2,σUP2)\begin{aligned} &\mathbf{Q}_{n}=\operatorname{diag}\left(\sigma_{\omega}^{2} \mathbf{I}, \sigma_{\mathbf{a}}^{2} \mathbf{I}, \sigma_{\mathbf{b}_{\boldsymbol{\omega}}}^{2} \mathbf{I}, \sigma_{\mathbf{b}_{\mathbf{a}}}^{2} \mathbf{I}\right) \\ &\mathbf{N}_{n}=\operatorname{diag}\left(\sigma_{\mathrm{VEL}, \mathbf{v}}^{2} \mathbf{I}, \sigma_{\mathrm{VEL}, \mathbf{a}}^{2} \mathbf{I}, \sigma_{\mathrm{ANG}}^{2} \mathbf{I}, \sigma_{\mathrm{LAT}}^{2}, \sigma_{\mathrm{UP}}^{2}\right) \end{aligned} Qn=diag(σω2I,σa2I,σbω2I,σba2I)Nn=diag(σVEL,v2I,σVEL,a2I,σANG2I,σLAT2,σUP2)

其中,协方差矩阵 QQQσω=0.01rad/s,σa=0.2m/s2,σbω=0.001rad/s,σba=0.02m/s2\sigma_{\omega}=0.01 \mathrm{rad/s},\sigma_{\mathbf{a}}=0.2 \mathrm{m/s^2},\sigma_{\mathbf{b}_{\boldsymbol{\omega}}}=0.001 \mathrm{rad/s},\sigma_{\mathbf{b}_{\mathbf{a}}}=0.02\mathrm{m/s^2}σω=0.01rad/s,σa=0.2m/s2,σbω=0.001rad/s,σba=0.02m/s2;噪声矩阵 Nn\mathbf{N}_{n}NnσVEL,v=1m/s,σVEL,a=0.4m/s2,σANG=0.04rad/s,σLAT=3m/s,σUP=3m/s\sigma_{\mathrm{VEL}, \mathbf{v}}=1\mathrm{m/s}, \sigma_{\mathrm{VEL}, \mathbf{a}}=0.4\mathrm{m/s^2}, \sigma_{\mathrm{ANG}}=0.04\mathrm{rad/s}, \sigma_{\mathrm{LAT}}=3\mathrm{m/s}, \sigma_{\mathrm{UP}}=3\mathrm{m/s}σVEL,v=1m/s,σVEL,a=0.4m/s2,σANG=0.04rad/s,σLAT=3m/s,σUP=3m/s

4.2 Evaluation Metrics

这里使用了三个评价指标:

  • Mean Absolute Trajectory Error (m-ATE),平均绝对轨迹误差(估计位置和真值位置之间的误差平均值);
  • Mean Absolute Aligned Trajectory Error (aligned m-ATE),首先对齐估计轨迹和真值轨迹,然后再计算m-ATE,主要是评估轨迹的一致性;
  • Final distance error,估计轨迹和真值轨迹最终的距离误差。

4.3 Trajectory Results

下面是实验结果,作者采用了4种方法:

  • IMU直接积分方法;
  • 差分轮速编码器得到线性速度和角速度再积分;
  • RINS-W,本文提出的方法;
  • 里程计+光纤陀螺仪,里程计提供线性速度,角速度由FoG得到。
在这里插入图片描述在这里插入图片描述在这里插入图片描述

同时作者还比较了是否使用横向和垂直速度假设时的定位误差,结果如下,使用横向和垂直速度假设时效果更好
在这里插入图片描述


5. Appendix

李群 SE2(3)SE_2(3)SE2(3) 是李群 SE(3)SE(3)SE(3) 的扩展,其形式可以写成 5×55\times55×5 的矩阵,即:
χn=[Rnvnwpnw0I]∈SE2(3)\boldsymbol{\chi}_{n}=\left[\begin{array}{ccc} \mathbf{R}_{n} & \mathbf{v}_{n}^{\mathrm{w}} & \mathbf{p}_{n}^{\mathrm{w}} \\ \mathbf{0} & \mathbf{I} \end{array}\right] \in S E_{2}(3) χn=[Rn0vnwIpnw]SE2(3)

误差 ξn=(ξnR,ξnv,ξnp)∈R9\boldsymbol{\xi}_{n}=\left(\boldsymbol{\xi}_{n}^{\mathbf{R}}, \boldsymbol{\xi}_{n}^{\mathbf{v}}, \boldsymbol{\xi}_{n}^{\mathrm{p}}\right) \in \mathrm{R}^9ξn=(ξnR,ξnv,ξnp)R9 的李代数形式为:
ξn∧=[(ξnR)×ξnvξnp0]∈se2(3)\boldsymbol\xi_{n}^{\wedge}=\left[\begin{array}{ccc} \left(\xi_{n}^{\mathrm{R}}\right)_{\times} & \xi_{n}^{\mathrm{v}} & \xi_{n}^{\mathrm{p}} \\ 0 & \end{array}\right] \in \mathfrak{s e}_{2}(3) ξn=[(ξnR)×0ξnvξnp]se2(3)

李代数 ξn∧\xi_{n}^{\wedge}ξn 的指数映射形式为:
exp⁡SE2(3)(ξn)=I+ξn∧+a(ξn∧)2+b(ξn∧)3\exp _{S E_{2}(3)}\left(\boldsymbol{\xi}_{n}\right)=\mathbf{I}+\boldsymbol{\xi}_{n}^{\wedge}+a\left(\boldsymbol{\xi}_{n}^{\wedge}\right)^{2}+b\left(\boldsymbol{\xi}_{n}^{\wedge}\right)^{3}expSE2(3)(ξn)=I+ξn+a(ξn)2+b(ξn)3

其中,a=1−cos⁡(∥ξnR∥)∥ξnR∥2a=\frac{1-\cos \left(\left\|\boldsymbol{\xi}_{n}^{\mathbf{R}}\right\|\right)}{\left\|\boldsymbol{\xi}_{n}^{\mathrm{R}}\right\|^2}a=ξnR21cos(ξnR)b=∥ξnR∥−sin⁡(∥ξnR∥)∥ξnR∥3b=\frac{\left\|\boldsymbol{\xi}_{n}^{\mathbf{R}}\right\|-\sin \left(\left\|\boldsymbol{\xi}_{n}^{\mathbf{R}}\right\|\right)}{\left\|\boldsymbol{\xi}_{n}^{\mathbf{R}}\right\|^{3}}b=ξnR3ξnRsin(ξnR)

这里使用的是Right IEKF,雅可比矩阵为:
Fn=I+[000−Rn0(g)×00−(vnW)×Rn−Rn0I0−(pnW)×Rn00000000000]dt,Gn=[Rn000(vnw)×RnRn00(pnw)×Rn00000I0000I]dt\mathbf{F}_{n}=\mathbf{I}+\left[\begin{array}{ccccc}\mathbf{0} & \mathbf{0} & \mathbf{0} & -\mathbf{R}_{n} & \mathbf{0} \\ (\mathbf{g})_{\times} & \mathbf{0} & \mathbf{0} & -\left(\mathbf{v}_{n}^{\mathrm{W}}\right)_{\times} \mathbf{R}_{n} & -\mathbf{R}_{n} \\ \mathbf{0} & \mathbf{I} & \mathbf{0} & -\left(\mathbf{p}_{n}^{\mathrm{W}}\right)_{\times} \mathbf{R}_{n} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0}\end{array}\right] d t,\\ \mathbf{G}_{n}=\left[\begin{array}{cccc}\mathbf{R}_{n} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \left(\mathbf{v}_{n}^{\mathbf{w}}\right)_{\times} \mathbf{R}_{n} & \mathbf{R}_{n} & \mathbf{0} & \mathbf{0} \\ \left(\mathbf{p}_{n}^{\mathbf{w}}\right)_{\times} \mathbf{R}_{n} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I}\end{array}\right] d tFn=I+0(g)×00000I0000000Rn(vnW)×Rn(pnW)×Rn000Rn000dt,Gn=Rn(vnw)×Rn(pnw)×Rn000Rn000000I00000Idt

z^nVEL=1\hat{z}_n^{\mathrm{VEL}}=1z^nVEL=1 时,矩阵 Fn,Gn\mathbf{F}_n,\mathbf{G}_nFn,Gn 的第4行-第9行为0;当 z^nANG=1\hat{z}_n^{\mathrm{ANG}}=1z^nANG=1 时,矩阵 Fn,Gn\mathbf{F}_n,\mathbf{G}_nFn,Gn 的前3行为0。

测量雅可比矩阵为:
HnvEL=[0RnT000RnT(g)×000−I]HnANG=[000−I0]\begin{aligned} \mathbf{H}_{n}^{\mathrm{vEL}} &=\left[\begin{array}{ccccc} \mathbf{0} & & \mathbf{R}_{n}^{T} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{R}_{n}^{T}(\mathbf{g})_{\times} & \mathbf{0} & \mathbf{0} & \mathbf{0} & -\mathbf{I} \end{array}\right] \\ \mathbf{H}_{n}^{\mathrm{ANG}} &=\left[\begin{array}{ccccc} \mathbf{0} & \mathbf{0} & \mathbf{0} & -\mathbf{I} & \mathbf{0} \end{array}\right] \end{aligned} HnvELHnANG=[0RnT(g)×0RnT0000I0]=[000I0]

这里,矩阵 HnVEL\mathbf{H}_n^{\mathrm{VEL}}HnVEL 的形式值得商榷,在上面第一种作者可能忘了讨论对旋转矩阵的求导。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/439569.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【HDU - 5988】Coding Contest(网络流费用流,改模板)

题干: A coding contest will be held in this university, in a huge playground. The whole playground would be divided into N blocks, and there would be M directed paths linking these blocks. The i-th path goes from the uiui-th block to the vivi-t…

一步步编写操作系统 61 任务状态段 TSS

I/O位图是位于TSS中的,它可以存在也可以不存在,它只是用来设置对某些特定端口的访问,没有它的话便默认为禁止访问所有端口。正是由于它可有可用,所以TSS的段界限TSS limit(即实际大小-1)并不固定。当TSS中不…

重读经典:《Deep Residual Learning for Image Recognition》

ResNet论文逐段精读【论文精读】这是李沐博士论文精读的第二篇论文,这次精读的论文是ResNet。ResNet 是 CVPR2016 的最佳论文,目前谷歌学术显示其被引用数已经达到了90000。 ResNet论文链接为:https://arxiv.org/abs/1512.03385。 1.第一遍 …

【CodeForces - 1131F 】Asya And Kittens(并查集,思维)

题干: Asya loves animals very much. Recently, she purchased nn kittens, enumerated them from 11 and nn and then put them into the cage. The cage consists of one row of nncells, enumerated with integers from 11 to nn from left to right. Adjacent…

关于Xldown和Xlup的用法(Excel VBA)

Xldown和xlup是一对组合,用于寻找某个区间中的非空单元格。 首先我们在单元格的前7行填入如下数据: A1单元格: A2单元格:2 A3单元格:3 A4单元格:4 A5单元格: A6单元格:6 A7单元格&am…

详解道路标记数据集 CeyMo: See More on Roads -- A Novel Benchmark Dataset for Road Marking Detection

本文介绍一个新的道路标记检测数据集,论文收录于 WACV2022。Ceymo数据集总共包含2887张图片,标注了11类共4706个道路标记实例,图片分辨率为 192010801920\times108019201080。其中,对于每一个道路标记实例,作者采用了三…

VBA类之一(初识类)

第一章 开头篇 ——认识类 Visual Basic是基于对象的编程(注:本文所有的代码和讨论将都以VB为基础模型,不过我会尽量使用一些大家在VBA中常见的例子来做说明的。),所以我们常见的一些东西其实都与类有关。不…

【HDU - 5009】Paint Pearls(dp,链表优化dp)

题干: Lee has a string of n pearls. In the beginning, all the pearls have no color. He plans to color the pearls to make it more fascinating. He drew his ideal pattern of the string on a paper and asks for your help. In each operation, he sele…

动手学无人驾驶(7):车道线检测

最近在研究视觉语义地图,需要进行车道线检测,发现这篇车道线检测论文效果蛮好的 (Ultra Fast Structure-aware Deep Lane Detection)。论文作者在知乎上已经介绍过了:https://zhuanlan.zhihu.com/p/157530787&#xff…

Coursera自动驾驶课程第16讲:LIDAR Sensing

在第15讲《Coursera自动驾驶课程第15讲:GNSS and INS Sensing for Pose Estimation》 我们学习了自动驾驶定位中常用的两种传感器:IMU(惯性测量单元) 和GNSS(全球导航卫星系统)。 本讲我们将学习自动驾驶汽…

DB、ETL、DW、OLAP、DM、BI关系结构图

在此大概用口水话简单叙述一下他们几个概念: (1)DB/Database/数据库——这里一般指的就是OLTP数据库,在线事物数据库,用来支持生产的,比如超市的买卖系统。DB保留的是数据信息的最新状态,只有一…

Tarjan 算法 常用模板

可以求每个点属于第几个强连通分量&#xff1a;https://blog.csdn.net/dellaserss/article/details/8267192 int Tarjan(int u){int v;dfn[u]low[u]Index;stack[Top]u;Instack[u]1;for(int i0;i<G[u].size();i){vG[u][i];if(!dfn[v]){Tarjan(v);low[u]min(low[u],low[v]);}…

【HDU - 5012】Dice(模拟,bfs)

题干&#xff1a; There are 2 special dices on the table. On each face of the dice, a distinct number was written. Consider a 1.a 2,a 3,a 4,a 5,a 6 to be numbers written on top face, bottom face, left face, right face, front face and back face of dice A. S…

重读经典:《Generative Adversarial Nets》

GAN论文逐段精读【论文精读】这是李沐博士论文精读的第五篇论文&#xff0c;这次精读的论文是 GAN。目前谷歌学术显示其被引用数已经达到了37000。GAN 应该是机器学习过去五年上头条次数最多的工作&#xff0c;例如抖音里面生成人物卡通头像&#xff0c;人脸互换以及自动驾驶中…

一步步编写操作系统 62 函数调用约定

由于我们要将c语言和汇编语言结合编程啦&#xff0c;所以一定会存在汇编代码和c代码相互调用的问题&#xff0c;有些事情还是要提前交待给大家的&#xff0c;本节就是要给大家说下函数调用规约中的那些事儿。 函数调用约定是什么&#xff1f; 调用约定&#xff0c;calling co…

重读经典:《An Image is Worth 16x16 Words: Transformers for Image Recognition at Scale》

ViT论文逐段精读【论文精读】这次李沐博士邀请了亚马逊计算机视觉专家朱毅博士来精读 Vision Transformer&#xff08;ViT&#xff09;&#xff0c;强烈推荐大家去看本次的论文精读视频。朱毅博士讲解的很详细&#xff0c;几乎是逐词逐句地讲解&#xff0c;在讲解时把 ViT 相关…

【Gym - 101612C】【2017-2018NEERC】Consonant Fencity(状压枚举,预处理)

题干&#xff1a; 把26个字母分成19个辅音字母和7个元音字母&#xff0c;让你通过 将某些字母改为大写状态&#xff0c;使得字符串中连续的两个大小写状态不同的辅音字母组成的字母对数量最多&#xff0c;输出该状态下的字符串。注意输出的字符串中同一字母必须形态统一&#…

浅谈Mysql 表设计规范

本文首先探讨下数据库设计的三大范式&#xff0c;因为范式只是给出了数据库设计的原则&#xff0c;并没有告诉我们实际操作中应该怎样操作&#xff0c;应该注意什么&#xff0c;所以我们还会谈下实际工作中需要注意的具体操作问题。 三大范式 首先放出三大范式内容&#xff0c…

从零开始学视觉Transformer(1):Hello Vision Transformer

Vision Transformer打卡营分享一门很棒的 ViT 课程&#xff0c;课程详细介绍可以看这篇文章&#xff1a; 《Vision Transformer打卡营来啦&#xff01;朱欤博士带你从零玩转ViT爆款模型&#xff01;》