Skip to main content
. 2013 Apr 8;13(4):4527–4552. doi: 10.3390/s130404527
Algorithm 1 Odometry algorithm based on rotating range sensor.
 INPUT:
  • -

    2 successive radar scans

  • -

    the last estimated robot velocities: (V, ω)

 M1 ← CFAR detections from scan 1
 M2 ← CFAR detections from scan 2
 Prediction of detections from image 1 onto image 2:
  • -

    Mˆ1 ← arg min{(g(Mˆ1, V, ω) − f(M1, V, ω))2}

 AssumptionAsso ← Data association phase between M2 and Mˆ1
 for k: AssumptionAsso do
   Vˆv(k) ← Extended Kalman Filter update k (V, ω)
 end for
 Ṽinlier ← RANSAC filtering of velocities assumptions Vˆv
 Ṽv ← CI Fusion (Vˆinlier)
 OUTPUT: new estimated robot velocities Vˆυ = [V, ω]T