论文：https://arxiv.org/abs/1904.06064
代码：https://github.com/mbrossar/ai-imu-dr
a. 本文主要提出了一种方法，能够仅仅使用IMU的航位推算得到较为精准的轨迹，如下图所示：
b. 该方法只适用于像汽车一样只能向前跑的系统，因为这样的系统在进行kalman滤波器的时候可以加入动力模型的辅助信息，在这里，辅助信息主要是指系统在侧向 y l a t y^{lat} ylat和与地面垂直的方向 y u p y^{up} yup上是没有移动的，文中称之为伪测量；
c. 论文的重要改进点在于，(b)中辅助信息的均值可以认为是0，但是协方差是否一直都是一个值呢？显然，在系统进行转弯的时候，水平方向的协方差要比直线方向的协方差是要大很多的，同理，在爬坡的时候，垂直地面方向的协方差也应该比直线的时候协方差是要大的；鉴于一般的滤波器都是人为设计协方差矩阵，同时设计好的协方差不能修改，因此文章希望使用CNN（并非RNN，作者说RNN不是很好）对观测的协方差进行动态的更新，进一步的，作者使用CNN也同时预测出来了上述的伪测量的值(这里没有给出伪量测的值)。
一般情况下，IMU的模型建立为真值+零偏+高斯白噪声，如下：
(1)
w
n
I
M
U
=
w
n
+
b
n
w
+
w
n
w
a
n
I
M
U
=
a
n
+
b
n
a
+
w
n
a
\begin{aligned} w_{n}^{IMU} &= w_{n}+b_{n}^{w}+w_{n}^{w} \\ a_{n}^{IMU} &=a_{n}+b_{n}^{a}+w_{n}^{a} \end{aligned} \tag{1}
wnIMUanIMU=wn+bnw+wnw=an+bna+wna(1)
其中零偏一般是一个随机游走的“准常量”（quasi-constant）（一般来说与温度有关），因此对零偏进行建模，其模型如下：
(2)
b
n
+
1
w
=
b
n
w
+
w
n
b
w
b
n
+
1
a
=
b
n
a
+
w
n
b
a
\begin{aligned} b_{n+1}^{w} &= b_{n}^{w}+w_{n}^{b_w} \\ b_{n+1}^{a} &= b_{n}^{a}+w_{n}^{b_a} \end{aligned} \tag{2}
bn+1wbn+1a=bnw+wnbw=bna+wnba(2)
其中
w
n
w
,
w
n
a
,
w
n
b
w
,
w
n
b
a
w_{n}^{w}, w_{n}^{a}, w_{n}^{b_w}, w_{n}^{b_a}
wnw,wna,wnbw,wnba为高斯分布的白噪声。
IMU的运动模型可以定义为如下形式：
(3)
R
n
+
1
I
M
U
=
R
n
I
M
U
exp
(
(
ω
n
d
t
)
×
)
v
n
+
1
I
M
U
=
v
n
I
M
U
+
(
R
n
I
M
U
a
n
+
g
)
d
t
p
n
+
1
I
M
U
=
p
n
I
M
U
+
v
n
I
M
U
d
t
\begin{aligned} \mathbf{R}_{n+1}^{\mathrm{IMU}} &=\mathbf{R}_{n}^{\mathrm{IMU}} \exp \left(\left(\boldsymbol{\omega}_{n} d t\right)_{ \times}\right) \\ \mathbf{v}_{n+1}^{\mathrm{IMU}} &=\mathbf{v}_{n}^{\mathrm{IMU}}+\left(\mathbf{R}_{n}^{\mathrm{IMU}} \mathbf{a}_{n}+\mathbf{g}\right) d t \\ \mathbf{p}_{n+1}^{\mathrm{IMU}} &=\mathbf{p}_{n}^{\mathrm{IMU}}+\mathbf{v}_{n}^{\mathrm{IMU}}dt \end{aligned} \tag{3}
Rn+1IMUvn+1IMUpn+1IMU=RnIMUexp((ωndt)×)=vnIMU+(RnIMUan+g)dt=pnIMU+vnIMUdt(3)
其中的所有变量都是真值形式，没有考虑到偏差等等的因素，如果把带有偏差的值运用到上面动态模型中，则误差会十分大，甚至比直接使用轮子的积分来的更差。
状态变量选择为：
x
n
:
=
(
R
n
I
M
U
,
v
n
I
M
U
,
p
n
I
M
U
,
b
n
ω
,
b
n
a
,
R
n
c
,
p
n
c
)
\mathbf{x}_{n} :=\left(\mathbf{R}_{n}^{\mathrm{IMU}}, \mathbf{v}_{n}^{\mathrm{IMU}}, \mathbf{p}_{n}^{\mathrm{IMU}}, \mathbf{b}_{n}^{\omega}, \mathbf{b}_{n}^{\mathrm{a}}, \mathbf{R}_{n}^{\mathrm{c}}, \mathbf{p}_{n}^{\mathrm{c}}\right)
xn:=(RnIMU,vnIMU,pnIMU,bnω,bna,Rnc,pnc)
其中系统的输入为IMU的测量值，
R
n
I
M
U
，
v
n
I
M
U
，
p
n
I
M
U
\mathbf{R}_{n}^{\mathrm{IMU}}，\mathbf{v}_{n}^{\mathrm{IMU}}，\mathbf{p}_{n}^{\mathrm{IMU}}
RnIMU，vnIMU，pnIMU是载体在第n步（或者理解为t时刻）的姿态，速度和位置，其中的参考系都是世界坐标系，坐标系的信息如下图:
可以看到每个变量都是从载体系到世界坐标系的转换，不要被变量上面的IMU所迷惑了； b n ω , b n a \mathbf{b}_{n}^{\omega}, \mathbf{b}_{n}^{\mathrm{a}} bnω,bna是IMU的零偏， R n c , p n c \mathbf{R}_{n}^{\mathrm{c}}, \mathbf{p}_{n}^{\mathrm{c}} Rnc,pnc是IMU到载体系的外参矩阵，这里也做作为一个更新量进行了估计。
同正常的kalman滤波步骤一样，我们首先建立预测方程（文章中把预测阶段称为传播propagate）和更新方程：
(5)
x
n
+
1
=
f
(
x
n
,
u
n
)
+
w
n
y
n
=
h
(
x
n
)
+
n
n
\begin{aligned} \mathbf{x}_{n+1} &=f\left(\mathbf{x}_{n}, \mathbf{u}_{n}\right)+\mathbf{w}_{n} \\ \mathbf{y}_{n} &=h\left(\mathbf{x}_{n}\right)+\mathbf{n}_{n} \tag{5} \end{aligned}
xn+1yn=f(xn,un)+wn=h(xn)+nn(5)
其中正如上面所说， y n y_n yn是辅助变量，理想情况下，我们的设置值为[0, 0]，即 h ( x n ) ≈ 0 h\left(\mathbf{x}_{n}\right)\approx0 h(xn)≈0
这部分更类似于观测方程的建立，主要是把状态估计中的速度（载体在世界坐标系下的速度，即
W
V
n
^WV_n
WVn）转移到载体系下
v
n
c
=
[
v
n
f
o
r
v
n
l
a
t
v
n
u
p
]
=
(
R
n
c
)
T
(
R
n
I
M
U
)
T
v
n
I
M
U
+
(
ω
n
)
×
p
n
c
\mathbf{v}_{n}^{\mathrm{c}}=\left[ \begin{array}{c}{v_{n}^{\mathrm{for}}} \\ {v_{n}^{\mathrm{lat}}} \\ {v_{n}^{\mathrm{up}}}\end{array}\right]=(\mathbf{R}_{n}^{\mathrm{c}})^T (\mathbf{R}_{n}^{\mathrm{IMU}})^T \mathbf{v}_{n}^{\mathrm{IMU}}+\left(\boldsymbol{\omega}_{n}\right) \times \mathbf{p}_{n}^{\mathrm{c}}
vnc=⎣⎡vnforvnlatvnup⎦⎤=(Rnc)T(RnIMU)TvnIMU+(ωn)×pnc
对于汽车等只能向前的物体，上述的
v
n
l
a
t
,
v
n
u
p
v_n^{lat}, v_n^{up}
vnlat,vnup的值应该近似于0，因此伪量测为
y
n
=
[
y
n
l
a
t
y
n
u
p
]
=
[
h
l
a
t
(
x
n
)
+
n
n
l
a
t
h
u
p
(
x
n
)
+
n
n
u
p
]
=
[
v
n
l
a
t
v
n
u
p
]
+
n
n
y_n=\left[\begin{array}{c}{y_n^{lat}} \\ {y_n^{up}}\end{array}\right]=\left[\begin{array}{c}{h^{lat}(x_n)+n_n^{lat}} \\ {h^{up}(x_n)+n_n^{up}}\end{array} \right]=\left[\begin{array}{c}{v_n^{lat}} \\ {v_n^{up}}\end{array} \right] + n_n
yn=[ynlatynup]=[hlat(xn)+nnlathup(xn)+nnup]=[vnlatvnup]+nn
其中
n
n
n_n
nn为噪声，这里算作一个松散的约束，这样的效果往往比强硬的把伪测量看做0要好的多。
整体结构如下图：
整体而言，系统使用IEKF进行状态的估计，而使用神经网络根据IMU的测量值产生出伪量测及其协方差，整个算法中预测阶段的系统噪声的协方差 Q Q Q是固定的，因为都是高斯白噪声。笔者还是比较看好这样的slam系统的，因为个人认为神经网络不是很适合slam这种几何学的问题，而作为辅助的话还是很看好的。
从系统结构图上可以看到，系统使用的是IMU的量测值，其为基于时间的一组序列，作者选择其中N组数据进行卷积，之后产生对应的测量协方差：
N
n
+
1
=
C
N
N
(
{
ω
i
I
M
U
,
a
i
I
M
U
}
i
=
n
−
N
n
)
\mathbf{N}_{n+1}=\mathrm{CNN}\left(\left\{\omega_{i}^{\mathrm{IMU}}, \mathbf{a}_{i}^{\mathrm{IMU}}\right\}_{i=n-N}^{n}\right)
Nn+1=CNN({ωiIMU,aiIMU}i=n−Nn)
作者说道设计中的三个动机如下：
网络最终的输出为一个2维向量
z
n
=
[
z
n
l
a
t
,
z
n
u
p
]
T
∈
R
2
z_n=[z_n^{lat}, z_n^{up}]^T \in R^2
zn=[znlat,znup]T∈R2，随后作者引入一个超参数
β
\beta
β用于控制协方差的扩张上下限，最终的协方差值取做：
N
n
+
1
=
diag
(
σ
lat
2
1
0
β
tanh
(
z
n
lat
)
,
σ
up
2
1
0
β
tanh
(
z
n
up
)
)
\mathbf{N}_{n+1}=\operatorname{diag}\left(\sigma_{\text { lat }}^{2} 10^{\beta \tanh \left(z_{n}^{\text { lat }}\right)}, \sigma_{\text { up }}^{2} 10^{\beta \tanh \left(z_{n}^{\text { up }}\right)}\right)
Nn+1=diag(σ lat 210βtanh(zn lat ),σ up 210βtanh(zn up ))
因此协方差的动态变化范围为
1
0
−
β
～
1
0
β
10^{-\beta}～10^{\beta}
10−β～10β。
网络采用1维卷积的方式，共2层，第一层卷积核的大小为5，输出的深度为32，膨胀系数为1，第二层卷积核大小为5，输出深度为32，膨胀系数为3，最后还有一个全连接层将特征输出综合输出为2，取得滑动窗口的大小为N=15，最终一共产生的参数量为6210（这个参数量笔者计算的稍微不太对，按照stride=2来计算的话最终的结果是6304）。
在实现的一些初始参数这里就不过多解释了，需要注意的是，用于不同的机器人的时候，需要对这些参数稍加修改，具体还是参照论文中的部分。
网络的训练步骤就是：
具体的一些超参数这里就不赘述了。
论文最终对比了IMLS（以激光雷达数据为基础），双目ORB-SLAM2和纯IMU进行积分的方法，结果如下：
结论还是很强势的，毕竟只是使用IMU的数据进行航位推算，最终能达到比ORB-SLAM2的效果还要还一点也确实很不错了。
除此之外，还记得作者设计网络时候的三个动机吗？作者也把其中第二个动机进行了验证，结果如下图：
恩～果然在转弯的时候，方差出现了变化。
笔者最后把github上的代码下载下来运行了一下，几个测试数据集的效果还是不错的，是一个足以期待的方向。
