This paper introduces an inertial navigation and positioning paperRINS-W, paper was published inIROS2019. In this paper, the author proposedUse only one IMUA method for conducting long time inertial navigation. The method mainly includes two parts:

  • The detector uses a cyclic neural network to detect the motion of IMU, such as zero velocity or zero lateral slip.
  • useInvariant Extended Kalman FilterCombine the detector’s output (as a pseudo-measurement) for positioning.

The test results on the open data set show that the final positioning error is 20m after driving more than 21km (as shown in the figure below).

Links to papers: arxiv.org/pdf/1903.02…

1. Inertial Navigation System & Sensor Model

The IMU direction is represented by Rn∈SO(3)\mathbf{R}_n \in SO(3)Rn∈SO(3), which represents the rotation transformation from the carrier coordinates to the world coordinates. The world coordinate system at the rate of VNW ∈ R3 \ mathbf {n} _ {n} ^ {\ mathrm {w}} \ \ mathbb in {R} ^ {3} VNW ∈ R3, In the world coordinate system PNW ∈R3\mathbf{p}_{n}^{\mathrm{w}} \in \mathbb{R}^{3} PNW ∈R3 can be written as:


R n + 1 = R n exp ⁡ S O ( 3 ) ( Omega. n d t ) v n + 1 w = v n W + ( R n a n + g ) d t p n + 1 w = p n w + v n w d t \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}

Sampling time, when the DTDTDT (R0, v0w p0w) (\ mathbf {R}, _0 \ mathbf {n} _ {0} ^ {\ mathrm {w}}, \ mathbf {p} _ {0} ^ {\ mathrm {w}}) (R0, v0w p0w) is the initial state. In this paper, the influence of earth rotation and Coriolis force is not considered.

Now let’s review the IMU model. The IMU acceleration and angular velocity model can be written as:


Omega. n I M U = Omega. n + b n Omega. + w n Omega. a n I M U = a n + b n a + w n a \begin{aligned} &\boldsymbol{\omega}_{n}^{\mathrm{IMU}}=\boldsymbol{\omega}_{n}+\mathbf{b}_{n}^{\boldsymbol{\omega}}+\mathbf{w}_{n}^{\bo ldsymbol{\omega}} \\ &\mathbf{a}_{n}^{\mathrm{IMU}}=\mathbf{a}_{n}+\mathbf{b}_{n}^{\mathbf{a}}+\mathbf{w}_{n}^{\mathbf{a}} \end{aligned}

Among them, the bn omega, bna \ mathbf {b} _ {n} ^ {\ boldsymbol {\ omega}}, \ mathbf {b} _ {n} ^ {\ mathbf {a}} bn omega, bna as, angular velocity error and acceleration error wn omega, Wna \ mathbf {w} _ {n} ^ {\ boldsymbol {\ omega}}, \ mathbf {w} _ {n} ^ {\ mathbf {a}} wn omega, wna to gaussian noise. The equation of angular velocity deviation and acceleration deviation can be written as:


b n + 1 Omega. = b n Omega. + w n b Omega. b n + 1 a = b n a + w n b a \begin{aligned} \mathbf{b}_{n+1}^{\boldsymbol{\omega}}=\mathbf{b}_{n}^{\boldsymbol{\omega}}+\mathbf{w}_{n}^{\mathbf{b}_{\boldsymbol{\ome ga}}} \\ \mathbf{b}_{n+1}^{\mathbf{a}}=\mathbf{b}_{n}^{\mathbf{a}}+\mathbf{w}_{n}^{\mathbf{b}_{\mathbf{a}}} \end{aligned}

Among them, WNB omega, wnba \ mathbf {w} _ {n} ^ {\ mathbf {b} _ {\ boldsymbol {\ omega}}}, \ mathbf {w} _ {n} ^ {\ mathbf {b} _ {\ mathbf {a}}} WNB omega, wnba for random walk noise.

In inertial navigation, accurate estimation of deviation is very important, even a small error will lead to a large deviation of position estimation.

2. Specific Motion Profiles For Wheeled Systemd

In this section, the authors introduce several common motor characteristics that tend to provide useful complementary information.

Firstly, there are four specific motion situations, which are encoded as vector forms:


z n = ( z n V E L . z n A N G . z n L A T . z n U P ) ∈ { 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}

Among them:

  • Zero velocitywhen
    z n V E L = 1 z_n^{\mathrm{VEL}}=1     , it has
    { v n = 0 R n a n + g = 0 \left\{\begin{array}{c}\mathbf{v}_{n}=\mathbf{0} \\ \mathbf{R}_{n} \mathbf{a}_{n}+\mathbf{g}=\mathbf{0}\end{array}\right.     , often used when zero speed is detectedZUPTThe algorithm is updated.
  • Zero Angular Velocity, ωn=0\ BoldSymbol {\omega}_{n}=\mathbf{0}ω =0 when znANG=1z_n^{\mathrm{ANG}}=1znANG=1
  • Zero lateral Velocity vnLAT≃0v_{n}^{\ Mathrm {LAT}} \simeq 0vnLAT≃0, when znLAT=1z_n^{\mathrm{LAT}}=1 The transformation equation of carrier coordinate velocity and world coordinate velocity is: 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}} {array} \ \ end right] vnB = RnTvnW = ⎣ ⎢ ⎡ vnFORvnLATvnUP ⎦ ⎥ ⎤.
  • Zero vertical Velocity, when znUP=1z_n^{\mathrm{UP}}=1znUP=1, vnUP≃0v_{n}^{\ Mathrm {UP}} \simeq 0vnUP≃0.

Among them, the latter two motion situations are often used in wheel speed robot or automobile movement. In the above four cases, the zero-velocity constraint (zero velocity and zero angular velocity) is used to correct IMU deviation and attitude pitch Angle. Zero lateral velocity and vertical velocity are used for long-term estimation of vehicle position (explained in a later experiment).

3. Proposed RINS-W Algorithm

The method proposed in this paper is shown in the figure below and consists of two parts:

  • DetectorIt is composed of recurrent neural network and outputs binary vector according to IMU measurement value
    z n \mathbf{z}_n     ;
  • IEKFIs a new kalman filter, the input is IMU measurement value and detector output (as pseudo-measurement), to estimate the state quantity;

3.1 Specific Motion Profile Detector

Detector will determine how many elements are valid in the NNN binary vector Zn \mathbf{z}_nzn at each moment, that is, how many forms of motion will occur, as shown in the structure below. The core module of the detector is LSTM, and the input is IMU measurement value. The calculation equation is as follows:


u ^ n + 1 . h n + 1 = LSTM ⁡ ( { Omega. i I M U . a i I M U } i = 0 n ) = LSTM ⁡ ( Omega. n I M U . a n I M U . h n ) \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}

Where u^n+1∈R4\hat{\mathbf{u}}_{n+1} \in \mathbb{R^4}u^n+1∈R4 contains the probability value of each motion, hn\mathbf{h}_nhn is the hidden state of the neural network, 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

In this paper, the author chooses to use IEKF instead of traditional EKF for state estimation. As shown in the figure below, the binary variable Z ^n\hat{\mathbf{z}}_nz^n will be used for state propagation on the one hand and state update on the other.

(1) First define IMU state, IMU state quantity is: Xn = (Rn), VNW, PNW, bn omega, bna) \ mathbf {x} _ {n} = \ left (\ mathbf {R} _ {n}, \ mathbf {n} _ {n} ^ {\ mathrm {w}}, \ mathbf {p} _ {n} ^ {\ mathrm {w}}, ^ \ mathbf {b} _ {n} {\ boldsymbol {\ omega}}, \ mathbf {b} _ {n} ^ {\ mathbf {a}} \ right) xn = (Rn), VNW, PNW, bn omega, bna), linear state error is: En = [factor nenb] ~ N (0, Pn) \ mathbf {e} _ {N} = \ left [\ begin {array} {c} \ boldsymbol {\ xi} _ {N} \ \ ^ \ mathbf {e} _ {n} {\ mathbf {b}} {array} \ \ end right] \ sim \ mathcal {n} \ left (\ mathbf {0}, \ mathbf {P} _ {n} \ right) en = [factor nenb] ~ n (0, Pn), The state update equation is:


χ n = exp ⁡ S E 2 ( 3 ) ( Is deduced n ) χ ^ n b n = b ^ n + e n b . \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}

Among them, the χ n ∈ SE2 (3) \ boldsymbol {\ chi} _ {n} \ SE_2 in (3) the χ n ∈ SE2 (3), Said for car status (Rn), VNW, PNW) (\ mathbf {R} _ {n}, \ mathbf {n} _ {n} ^ {\ mathrm {w}}, \ mathbf {p} _ {n} ^ {\ mathrm {w}}) (Rn), VNW, PNW) on lie groups form, The error state covariance matrix is Pn∈R15×15\mathbf{P}_{n} \in \mathbb{R}^{15 \times 15}Pn∈R15×15. Deviation of bn = (bn omega, bna) ∈ R6 \ mathbf {b} _n = ({\ mathbf {b} _ {n} ^ {\ boldsymbol {\ omega}}, ^ \ mathbf {b} _ {n} {\ mathbf {a}})} \ \ mathbb in bn = {R} ^ 6 (bn omega, bna) ∈ R6, Deviation estimator for b ^ n = (b ^ n omega, b ^ na) ∈ R6 \ hat {\ mathbf {b}} _ {n} = \ left (\ hat {\ mathbf {b}} _ {n} ^ {\ omega}. \ hat {\ mathbf {b}} _ {n} ^ {\ mathbf {a}} \ right) \ in \ mathbb {R} ^ ^ n = {6} b (b ^ n omega, b ^ na) ∈ R6.

(2) Prediction. When no one of the above four motion conditions is detected, the motion equation introduced in Section 1 is used to calculate the new state quantity and covariance, and the covariance calculation equation is:


P n + 1 = F n P n F n T + G n Q n G n T \mathbf{P}_{n+1}=\mathbf{F}_{n} \mathbf{P}_{n} \mathbf{F}_{n}^{T}+\mathbf{G}_{n} \mathbf{Q}_{n} \mathbf{G}_{n}^{T}

The Jacobian matrix Fn,Gn\mathbf{F}_n,\mathbf{G}_nFn,Gn will be introduced in section 5. Qn\mathbf{Q}_nQn is denoted as noise covariance matrix, Noise is the wn = (wn, omega wna, WNB omega, 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, omega wna, WNB omega, wnba) ~ n (0, Qn).

If it is detected by a specific motion condition, the state quantity will be modified according to the following equation. At zero velocity (velocity returns to 0, position remains unchanged, 0 here means velocity is 0, which was not explicitly stated by the author in the original paper) :


z ^ n + 1 v E L = 1 ⇒ { v n + 1 w = v n w = 0 p n + 1 w = p n w \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.

At zero angular velocity (attitude unchanged) :


z ^ n + 1 A N G = 1 ⇒ R n + 1 = R n \hat{z}_{n+1}^{\mathrm{ANG}}=1 \Rightarrow \mathbf{R}_{n+1}=\mathbf{R}_{n}

Meanwhile, the state estimator and covariance matrix should be modified in response.

(3) Update. Each motion situation will produce the following pseudo-measurements:


y n + 1 V E L = [ R n + 1 T v n + 1 W b n + 1 a – R n + 1 T g ] = [ 0 a n I M U ] y n + 1 A N G = b n + 1 Omega. = Omega. n I M U y n + 1 L A T = v n + 1 L A T = 0 y n + 1 U P = v n + 1 U P = 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}

The updated equation is:


K = P n + 1 H n + 1 T / ( H n + 1 P n + 1 H n + 1 T + N n + 1 ) e + = K ( y n + 1 – y ^ n + 1 ) = [ Is deduced + e b + ] χ ^ n + 1 + = exp ⁡ ( Is deduced + ) χ ^ n + 1 . b n + 1 + = b n + 1 + e b + P n + 1 + = ( I – K H n + 1 ) P n + 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}

Where K\mathbf{K}K is the Kalman gain matrix, Hn+1\mathbf{H}_{n+1}Hn+1 is the measurement Jacobian matrix (see Section 5 for the specific form).

(4) Initialization. In order to estimate the correct deviation and direction, a 1 second stop will be forced at the beginning to estimate the deviation and pitch Angle.

4. Results On Car Dataset

The first is the introduction of Dataset. The Dataset used is the comples urban LiDAR Dataset, as shown in the figure below.

4.1 Implementation Details

Below are the implementation details. The Detector consists of 4 LSTMs, each LSTM consists of 2 hidden layers (250 hidden units per layer), followed by 2 layers of multi-layer perceptrons, and finally the SigmoID function. The threshold is set as: 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); The operating frequency of the filter is 100Hz, and the noise covariance matrix is:


Q n = diag ⁡ ( sigma Omega. 2 I . sigma a 2 I . sigma b Omega. 2 I . sigma b a 2 I ) N n = diag ⁡ ( sigma V E L . v 2 I . sigma V E L . a 2 I . sigma A N G 2 I . sigma L A T 2 . sigma U P 2 ) \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}

Among them, Covariance matrix: QQQ sigma omega = 0.01 rad/s, sigma a = 0.2 m/s2, sigma b omega = 0.001 rad/s, sigma ba = 0.02 m/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} sigma omega = 0.01 rad/s, sigma a = 0.2 m/s2, sigma b omega = 0.001 rad/s, sigma ba = 0.02 m/s2. Noise matrix Nn \ mathbf {N} _ {N} sigma VEL in Nn, v = 1 m/s, sigma VEL, a = 0.4 m/s2, sigma ANG = 0.04 rad/s, sigma LAT = 3 m/s, sigma UP = 3 m/s \ sigma_ {\ mathrm {VEL}. = 1 \ mathrm \ mathbf {n}} {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} sigma VEL, v = 1 m/s, sigma VEL, a = 0.4 m/s2, sigma ANG = 0.04 rad/s, sigma LAT = 3 m/s, sigma UP = 3 m/s.

4.2 Evaluation Metrics

Three evaluation indicators are used here:

  • Mean Absolute Trajectory Error (M-ATE) is the average Error between the estimated position and the true position.
  • Mean Absolute Aligned Trajectory Error (Aligned m-ate). Align the estimated Trajectory and truth Trajectory first, and then calculate the M-ATE to evaluate the Trajectory consistency.
  • Final distance error, the Final distance error of the estimated trajectory and truth trajectory.

4.3 the Trajectory of the Results

Here are the results. The authors used four methods:

  • IMU direct integration method;
  • Differential wheel speed encoder obtains linear velocity and angular velocity and then integrates;
  • Rins-w, the method proposed in this paper;
  • Odometer + Fiber optic gyroscope, odometer provides linear speed, angular speed is obtained by FoG.

At the same time, the author also compared the positioning error of whether to use transverse and vertical velocity assumptions, and the results are as follows: the effect is better when using transverse and vertical velocity assumptions.