1 Ph. Bonnifait

Road Invariant Extended Kalman Filter for an Enhanced estimation of GPS Errors using Lane

Markings

Zui Tao and Philippe Bonnifait

Professor at the Université de Technologie de Compiègne

Heudiasyc UMR 7253 CNRS, France

IROS 2015, Hamburg

2 Ph. Bonnifait

Autonomous parking valet

3 Ph. Bonnifait

Context and Objectives

Positioning systems with standard automotive sensors

Map-aided with a Lane Departure Warning System camera

Lane markings accurately mapped

4 Ph. Bonnifait

GPS errors are not white

100 50 0 50 100 0.5

0

0.5

1autocorrelation of x error

t (s)

100 50 0 50 100 0.5

0

0.5

1autocorrelation of y error

t (s)

Pseudorange measurements are affected by atmosphere biases Satellite positions used in real-time ephemeris are inaccurate Position fixes are computed by a Kalman filter that has its own dynamic

0 100 200 300 400 500 600 2.5

2

1.5

1

0.5

0

0.5

1

1.5

Err

or

(m)

Time (s)

x error

0 100 200 300 400 500 600 2

1

0

1

2

3

4

5

Err

or

(m)

Time (s)

y error

5 Ph. Bonnifait

GPS error models

Shaping models Research approaches

Autoregressive process:

I. Miller, M. Campbell and D. Huttenlocher (2011)

II. K. Jo, K. Chu and M. Sunwoo (2013)

Random bias

J. Laneurit, R. Chapuis and F. Chausse (2006)

6 Ph. Bonnifait

𝑥𝐺𝑃𝑆 = 𝑥 + 𝜀𝑥1 + 𝜀𝑥2

𝑦𝐺𝑃𝑆 = 𝑦 + 𝜀𝑦1 + 𝜀𝑦2

Designing enhanced GPS error models using observability analysis

7 Ph. Bonnifait

Nonlinear observability analysis

Approaches Theory

Geometrical observability Rank condition after linearization and Lie derivatives (Local weak observability)

Algebraic observability Algebraic equation linking the state vector to the measured output Y and input U and a finite number of their time derivatives

8 Ph. Bonnifait

Modelling and observability analysis in the road frame

𝑥 = 𝑣 · cos𝜓𝑦 = 𝑣 · sin𝜓

𝜓 = 𝜔 − 𝜀𝜔

𝜀𝜔 = 0

𝜀 x1 = −𝜀𝑥1 𝜏1

𝜀 x2 = −𝜀𝑥2 𝜏2

𝜀 y1 = −𝜀𝑦1 𝜏1

𝜀 y2 = 0

𝐶0 = (𝑦 − 𝑦𝐴) c𝑜𝑠 𝜓

𝑥𝐺𝑃𝑆 = 𝑥 + 𝜀𝑥1 + 𝜀𝑥2

𝑦𝐺𝑃𝑆 = 𝑦 + 𝜀𝑦1 + 𝜀𝑦2

𝑌 = [𝑥𝐺𝑃𝑆, 𝑦𝐺𝑃𝑆, 𝐶0

𝑈 = [𝑣, 𝜔

𝑋 = 𝑥, 𝑦, 𝜓, 𝜀𝜔, 𝜀𝑥1, 𝜀𝑦1, 𝜀𝑥2, 𝜀𝑦2𝑇

Augmented state vector:

State evolution model:

GPS and camera observation models:

System input:

9 Ph. Bonnifait

Observability analysis in the road frame

Conclusions: • Lateral direction:

autoregressive bias and random bias are both observable • Longitudinal direction:

autoregressive bias shaping model (only) is observable

Algebraic observability:

10 Ph. Bonnifait

Road invariant extended Kalman filter

Objective: To implement an EKF in a road frame to maintain state observability

11 Ph. Bonnifait

From one road to another

- Deterministic transformation between road frames - Bijective transformation

12 Ph. Bonnifait

From ENU to road frame

GPS measurement:

Detected lane marking:

13 Ph. Bonnifait

From road frame to ENU

14 Ph. Bonnifait

Road invariant extended Kalman filter

GPS and camera observations are transformed into the road frame to update state vector

Map-matching is performed in the ENU frame. It monitors the road frame change

System output is in the ENU frame

15 Ph. Bonnifait

Experimental setup

• Experimental vehicle: Renault Fluence ZE

• CAN bus gateway (yaw rate and wheels speeds)

• GPS: U-Blox 6T receiver

• Camera: MobilEye

• Ground truth: IMU Oxford RT3000

• 3 tests carried out in urban conditions

• Speed : 10m/s

• 3 Km long

16 Ph. Bonnifait

Result

17 Ph. Bonnifait

Real-time implementation

18 Ph. Bonnifait

Result

Lateral PE (m) Longitudinal PE (m) I II III I II III

mean 1.30 0.07 0.04 1.55 -0.32 -0.19 Std. dev. 1.12 0.29 0.26 1.18 0.32 0.29 median 0.96 0.10 0.09 1.31 0.30 0.24 95th percentile 3.20 0.68 0.55 3.88 0.88 0.73 max 6.78 1.83 1.37 4.69 1.50 1.36

Positioning error statistics:

I: Stand-alone U-blox

II: ENU EKF

III: Road invariant EKF

19 Ph. Bonnifait

Result Cumulative distribution of lateral and longitudinal positioning errors:

median 95th percentile max

Lateral positioning 10% 19% 25%

Longitudinal positioning 20% 17% 9%

Improvement by road invariant EKF with respect to ENU EKF:

20 Ph. Bonnifait

Result Estimated GPS biases:

21 Ph. Bonnifait

Result Estimated standard deviation of the GPS biases:

22 Ph. Bonnifait

Conclusions and Perspectives

Close-to-market sensors for autonomous vehicle navigation

Observability analysis shaping filter design enhanced GPS error modeling

Road invariant extended Kalman filter Detailed implementation it maintains complete observability of the state (in particular of the shaping filters)

Experimental results Significant accuracy improvement

23 Ph. Bonnifait

Road Invariant Extended Kalman Filter for an Enhanced estimation of GPS Errors using Lane

Markings

Thank you for your attention!

E-mail: philippe.bonnifait@hds.utc.fr

Professor at the Université de Technologie de Compiègne

Heudiasyc UMR 7253 CNRS, France

Special thanks to Vincent Frémont, Stéphane Bonnet and Javier-Ibañez Guzman for the experiments