1 of 28

Precise Relative Positioning in Tandem Drifting using Drift Dynamics�

Adyasha Mohanty, Rémy Zawislak, Sriramya Bhamidipati, and Grace Gao

ION GNSS+ 2021

2 of 28

Autonomous Single Car Drifting at Stanford

Algorithms for drifting provide insights into how autonomous cars handle excursions past stable limits

[1] Goh & Gerdes, Journal of Dynamic Systems, Measurements and Control, 2019

1

3 of 28

Single car drifting^[1]

Localization with GNSS carrier phase (RTK^[2])

Absolute positioning at cm-level
Solve integer ambiguities via base station corrections

[1] Goh & Gerdes, Journal of Dynamic Systems, Measurements and Control, 2019

Autonomous tandem drifting

Noisy, fast dynamics lead to high trajectory tracking errors
Hard to guarantee cm-level accuracy due to large search space of ambiguities

[2] R. Langley, GPS World, 1998

How to Achieve Tandem Drifting?

2

4 of 28

3

Method	Description	Limitations
RTK^[3][4]	Transmit base station corrections to rover	Requires reference stations
PPP^[5]	Uses precise orbit and clock data with measurements from sparse network of base stations	Long convergence time
Other methods^[6][7][8]	Use constrained version of LAMBDA within optimization/Kalman filtering	Assume fixed relative position Do not account for time varying (adaptive) constraints

[3] Bisnath & Gao,GPS World, 2009

[4] Takasu & Yasuda, International Symposium on GPS/GNSS, 2008

[5] Zumberge, Journal of Geophysical Research: Solid Earth, 1997

[6] Teunissen, Artificial Satellites, 2006

[7] Henkel & Zhu, IEEE SSP, 2011

[8] Broshears & Bevly, ION GNSS+, 2013

Existing Carrier Phase-Based Methods

5 of 28

GNSS offers high measurement frequency

Integrate carrier phase-based positioning with rapid drift dynamics

Mathematical models exist for drift dynamics

Leverage drift dynamics to design reference trajectory
Track reference trajectory with m-level accuracy^[1]using controllers

Noisy tracking provides coarse idea of cars’ positions

Rely only on local computations, no base stations

4

[1] Goh & Gerdes, Journal of Dynamic Systems, Measurements and Control, 2019

Opportunities using Carrier-Phase and Drift Dynamics

6 of 28

Proposed Approach: Use Drift Dynamics to Aid Integer Fixing�

5

Form a priori estimate of relative position using noisy tracking of reference trajectories

Constrain search space of carrier phase integer ambiguities using prior estimate

Adjust search space adaptively using

Errors in tracking reference trajectory
Noise in relative position after integer fixing

Achieve fast integer fixing even with perturbations and noisy tracking

7 of 28

Key Contributions

Design precise relative positioning framework for tandem drifting cars

Aid GNSS carrier phase with a priori constraints from drift dynamics

Formulate adaptive constraint bound

Utilize a priori constraints and relative position estimate from previous time epoch
Constrain search space adaptively to enable integer fixing

Optimize float solution with barrier function

Solve for float solution (thereafter fixed relative position) in a tractable manner

6

8 of 28

Outline

Background

Model of Drift Dynamics
Reference Trajectory from Drift Dynamics

Precise Relative Positioning Framework

Bound Computation
Barrier Formulation
Adaptive Constraint Bounding

Experimental Validation

Errors: Simulated Drifting Trajectories
Constraint Bounds: Varying Added Noise in Code/Carrier Measurements

Conclusion

7

9 of 28

Outline

Background

Model of Drift Dynamics
Reference Trajectory from Drift Dynamics

Precise Relative Positioning Framework

Bound Computation
Barrier Formulation
Adaptive Constraint Bounding

Experimental Validation

Errors: Simulated Drifting Trajectories
Constraint Bounds: Varying Added Noise in Code/Carrier Measurements

Conclusion

8

10 of 28

Model of Drift Dynamics

9

Front tire

Rear tire

Path tangent

World East

Use vehicle parameters, tire models and forces to form equations of motion^[1]

Rear longitudinal force

Rear lateral force

Front lateral force

Moment of inertia

Vehicle mass

[1] Goh & Gerdes, Journal of Dynamic Systems, Measurements and Control, 2019

11 of 28

Reference Trajectory from Drift Dynamics

10

Phase Portrait

Controller Gains

Reference trajectory

Desired longitudinal forces

[9] Bobier & Gerdes, Stanford University, 2012

Propagation Model, Tire Forces

We then utilize a powerful tool called as phase portrait to understand these EOM and specifically stable/unstable regimes within drifting.

we plot the diff equations of Uy and r to generate the pp as shown to the right. This portrait reveals the presence of 3 equilibrium points, i.e. where the state derivates go to 0. 1 eq is stable, 1 is unstable but we are only concerned with the saddle point equilbirum where one of the roots has a positive and the other has a negative real part.

Using these equ values, we can formulate control laws to track both lateral and long dynamics. Then we can compute the desired long forces, utilize the propagation model and tire forces to simulate an entire drifting trajectory. We call this the ref trajectory that the cars should follow upto a meter level accuracy.

12 of 28

Outline

Background

Model of Drift Dynamics
Reference Trajectory from Drift Dynamics

Precise Relative Positioning Framework

Bound Computation
Barrier Formulation
Adaptive Constraint Bounding

Experimental Validation

Errors: Simulated Drifting Trajectories
Constraint Bounds: Varying Added Noise in Code/Carrier Measurements

Conclusion

11

13 of 28

Leader-Follower Setup

Follower

car B

12

14 of 28

Precise Relative Positioning Framework

Bound Computation

Barrier Formulation

Rounding of Integers

Reference trajectory

Relative position vector

A priori estimate, length and angle constraints

Float solution

Adaptive Constraint Bounding

Fixed solution

Updated constraints for next time epoch

13

15 of 28

Bound Computation

Add Gaussian noise to reference trajectory of leader, follower
Noisy reference trajectories: Form a priori estimate of relative position vector
Model constraints in physical space as uniform bounds on length, angle

Initial Bounds: Scale added Gaussian noise with scaling factor
Update bounds with adaptive constraint bounding

14

16 of 28

Barrier Formulation

Constrained objective

Barrier function transforms optimization to unconstrained one

Barrier function

Optimize for float ambiguities and round them to compute fixed integers

Geometry matrix

Double differenced code and carrier measurements

Pre-factor matrix with Identity

Integer ambiguities

Relative position vector

15

Hyperparameter

Penalty factor

17 of 28

Adaptive Constraint Bounding

No perturbations: Shrink bounds next time
Presence of perturbations: Expand bounds next time

A priori estimate and bounds on noise in nominal case

CP fixed position coincides with prior estimate

Reduced

bounds

CP fixed position doesn’t coincide with prior estimate

Increased

bounds

Uncertainty Bounds

A priori estimate

Carrier Phase (CP) Fixed Position

Noise in Reference Trajectory

A priori estimate and bounds on noise in degraded scenarios

16

18 of 28

Outline

Background

Model of Drift Dynamics
Reference Trajectory from Drift Dynamics

Precise Relative Positioning Framework

Bound Computation via Adaptive Constraints
Barrier Formulation
Adaptive Constraint Bounding

Experimental Validation

Errors: Simulated Drifting Trajectories
Constraint Bounds: Varying Added Noise in Code/Carrier Measurements

Conclusion

17

19 of 28

18

GPS SDR-SIM

GNSS SDR

Ephemeris,

user time

Raw GPS

measurements

Software Simulator

Relative

position

Parameter	Value
Constellation	GPS
No. of satellites	10-12
Std. dev of added noise in code phase	1.55-2 m
Std. dev of added noise in carrier phase	1.55-2 cm

Reference trajectories

(vehicle parameters

from real-world testing^[1])

Our Algorithm

[1] Goh & Gerdes, Journal of Dynamic Systems, Measurements and Control, 2019

Experimental Setup

20 of 28

Parameter	Value
Std. dev noise in reference trajectory	0.15 m (length), 1e-3 rad (angle)
Initial scaling factor for constraint bounds	3
Distance between leader follower	1-20.5 m
Longitudinal velocity	8 m/s
Lateral velocity	-4.2 m/s
Yaw rate	1.16 rad/s

19

Experimental Parameters

Simulated Tandem Drifting using Drift Dynamics

21 of 28

Baselines and Validation Metrics

Metrics

Mean error w.r.t. ground truth

Maximum error w.r.t. ground truth

Distribution of errors (percentile)

Error in North, East directions

[10] Teunissen, Journal of Geodesy, 1995

20

Baselines

LAMBDA^[10]

Fixed constraint bounds with different factors to bound the noise from reference trajectories

Scaling factor of 10

Scaling factor of 1e-3

22 of 28

Error	Lambda	Fixed Bounds (1e-3)	Fixed Bounds (1)	Our Algorithm
Mean (cm)	1.73	6.75	8.53	6.73
Max (cm)	977.00	37.30	43.66	37.30

21

Our algorithm achieves lowest RMSE and outperforms all three baselines

Errors: Simulated Drifting Trajectories

Error Plots from Our Algorithm

Error Distribution from Our Algorithm

23 of 28

Errors: Simulated Drifting Trajectories

Our algorithm provides cm-level accuracy where 95% errors < 20 cm

22

Error Plots from Our Algorithm

Error Distribution from Our Algorithm

Error	Lambda	Fixed Bounds (1e-3)	Fixed Bounds (1)	Our Algorithm
Mean (cm)	1.73	6.75	8.53	6.73
Max (cm)	977.00	37.30	43.66	37.30

24 of 28

Constraint Bounds:�Varying Added Noise in Code/Carrier Measurements

Fixed Bounds

(Scaling factor of 10)

Unlike fixed bounds, adaptive constraint bounds enable high accuracy with noisy GPS measurements

Fixed Bounds

(Scaling factor of 1e-3)

Adaptive Bounds

23

25 of 28

Outline

Background

Model of Drift Dynamics
Reference Trajectory from Drift Dynamics

Precise Relative Positioning Framework

Bound Computation via Adaptive Constraints
Barrier Formulation
Adaptive Constraint Bounding

Experimental Validation

Errors: Simulated Drifting Trajectories
Constraint Bounds: Varying Added Noise in Code/Carrier Measurements

Conclusion

24

26 of 28

Conclusion

We designed a precise positioning framework for tandem drifting by combining GNSS carrier phase and drift dynamics

For simulated tandem drifting experiments, our algorithm maintains cm-level accuracy at different measurement noise levels while other algorithms show m-level accuracies

Adaptive constraint bounding aids integer ambiguity fixing and low mean/maximum error in relative position

25

27 of 28

Future Work

Conduct real-world experiments

26

28 of 28

Toyota Research Institute

Asta Wu for assisting with software simulator

27

Acknowledgements