Bayesian Train Localization with Particle Filter, Loosely Coupled GNSS, IMU, and a Track Map

Algorithm 1

Algorithm of the map-based train localization with GNSS, IMU, and Rao-Blackwellized particle filter.

Algorithm: Train Localization (RBPF)

Input: GNSS and IMU sensor data

Output: topological coord. () and train speed

() load map

() initialize odometry Kalman filters with zero vector

() initialize all particles by first GNSS position (30)

() loop

()    if new measurement(s) available then

()    time step: ,

()    for all   particles do

()      predict odometry KF (19)

()      update KF with speed (8)/acceleration (11)

()      if train is moving then

()       sample displacement from odometry (28)

()       compute map transition (21)

()       get geometry from map (train frame) (22)

()       compute likelihoods (9)/(10)/(14)

()       multiply particle weight by likelihoods (29)

()      else (train is stopped)

()       observe and filter gyroscope bias

()      end if

()   end for

()   normalize weights (27)

()   compute most likely output estimate (31)–(39)

()   if resampling necessary by   then

()     perform resampling

()   end if

()  end if

() end loop