Drone Positioning - HKUSTqianzh/MSBD6000F/notes/3-1... · Correcting N via Particle Filters ......

Drone Positioning

Qian Zhang

A brief review of GPS literature (300 km to 15 cm)

• Basic satellite trilateration: 300km error • GPS Localization (phones): 1-3m error • Differential GPS (airplanes): 15cm error

Some Background on GPS

Basic GPS Localization

Range(R) (tr t s)*c

However, 3D location needs 3 equations … hence, use 3 satellites

Basic GPS Localization

Range(R) (tr t s)*c

However, 3D location needs 3 equations … hence, use 3 satellites

Basic GPS Localization

Range(R) (tr t s)*c

⋯⋮ ⋱ ⋮⋯

𝑋𝑌𝑍

=

𝑅1𝑅2𝑅3

Satellite

Geometry

Matrix

However, 3D location needs 3 equations … hence, use 3 satellites

300 Km of error due to unsynchronized clocks

Basic GPS Localization

Range(R) (tr t s)*c clk *c

⋯⋮ ⋱ ⋮⋯

𝑋𝑌𝑍𝛿𝑐𝑙𝑘

=

𝑅1𝑅2𝑅3𝑅4

𝑐𝑐𝑐𝑐

Satellite

Geometry

Matrix

1-3m error

New unknown … use 4th satellite and estimate both location and

Basic GPS Localization

1-3m error acceptable to smartphones, car GPS

Differential GPS using phase of the signals

Drone orientation warrants cm level accuracy

Agenda

01

02

Localization for Drones

Indoor Follow-Me Drone

Tremendous excitement with drones

Despite the excitement …

Drones still not ready for public places

Several Instances of Crashes and Failure …

Why do drones crash?

Drone Stabilization

Inertial Measurement Unit (IMU) crucial for tracking 3D orientation stabilization, control

Accelerometer Gyroscope Magnetometer IMU

IMU can fail

Inertial Measurement Unit (IMU) crucial for tracking 3D orientation stabilization, control

Accelerometer Gyroscope Magnetometer IMU

However, rotor vibration and magnetic interference derails the sensors … causing correlated failures

45% of drone crashes due to IMU failures [DJI Drone Survey 2015]

We ask: can we design a Plan-B for IMU failure?

Drones already use GPS for localization … recent drones using multiple GPS for reliability

Can we leverage multiple GPS receivers to compute 3D orientation (i.e., a backup for IMU sensor failure)

SafetyNet:

Basic Idea: Use multiple GPS to compute 3D orientation

Tracking Drone Orientation with Multiple GPS Receivers

A failsafe mechanism for IMU failures

Use multiple GPS to compute 3D orientation

Use multiple GPS to compute 3D orientation

Use multiple GPS to compute 3D orientation

Use multiple GPS to compute 3D orientation

Use multiple GPS to compute 3D orientation

25

Translate Relative Positions into 3D Orientation

To match IMU accuracy (~2 deg.)

Relative vector estimates have to be ~2cm

Use multiple GPS to compute 3D orientation

To match IMU, we require <2cm error for each 3D vector

Basic GPS 3m absolute position error

Opportunity: We do not care about absolute location …

we only need relative

Differential GPS relative 15cm error

We bring additional spatio-temporal information fusion techniques … for these small sized drones

Parkinson et al, GPS 96, Cohen et al, Journal of Satellite Communication 96 Hedgecock et al, Mobisys 13, Sensys 14

Temporal Differentials

Kalman Filter

Spatial Differentials

Carrier Phases

Particle Filter

Des

ign

of

Differential GPS (15 cm)

Co

ntrib

utio

ns

Backgro

un

d

SafetyNet 2∘(2 𝑐𝑚)

• Basic satellite trilateration: 300km error • GPS Localization (phones): 1-3m error • Differential GPS (airplanes): 15cm error • For drone orientation (< 2∘ ): ~2cm error

Some background on GPS

Differential GPS (using carrier phase)

GPS Carrier Phase

PhaseNTrueRange

NTrueRangePhase )(

Possible to measure precisely (2-5 mm error)

However, difficult to estimate N

GPS Receiver

GPS Satellite

atmRx

Tx DelayClkClkNTrueRangePhase )(

Real systems more difficult:

However, since we only care about relative distances … Possible to eliminate some errors by subtracting phase equations

across receivers, satellites, time, etc.

For example …

GPS Receiver

GPS Satellite

GPS Carrier Phase

(I) Single Differential Across Receivers – Cancelling Satellite Errors

Range = 𝑅1𝑠

Range = 𝑅2𝑠

1 Relative Position (Baseline)

𝜙1𝑠 = 𝑅1

𝑠 + 𝜆𝑁1𝑠 + 𝐶𝑙𝑘𝑠 + 𝐶𝑙𝑘1 + 𝐷𝑒𝑙𝑎𝑡𝑚

𝜙2𝑠 = 𝑅2

𝑠 + 𝜆𝑁2𝑠 + 𝐶𝑙𝑘𝑠 + 𝐶𝑙𝑘2 + 𝐷𝑒𝑙𝑎𝑡𝑚

Δ𝜙12𝑠 = Δ𝑅12

𝑠 + 𝜆𝑁12𝑠 + ΔClk12

S

2 Clock Errors can be eliminated further

Δ𝜙12𝑠 = 𝑅12𝑙𝑠 + 𝜆𝑁12

𝑠 + ΔClk12

Differential GPS community has exploited such techniques to achieve orientation estimation on big planes, ships

For a small drone we need very high accuracy

Need to handle dynamic errors

(II) Satellite Double Differential: Cancel Receiver Errors

S K

R12 = Relative vector (BaseLine) between two GPSs

However, integer ambiguity remains

𝑹𝟏𝟐

1

2

Can be translated into Orientation

Δ𝜙12𝑠 = 𝑅12𝑙𝑠 + 𝜆𝑁12

𝑠 + ΔClk12

Δ𝜙12𝑘 = 𝑅12𝑙𝑘 + 𝜆𝑁12

𝑘 + ΔClk12

Δ𝜙12𝑠 − Δ𝜙12

𝑘 = 𝑅12. ( 𝑙𝑠 − 𝑙𝑘 ) + 𝜆(𝑁12𝑠 − 𝑁12

𝑘 )

Δ𝜙12𝑠𝑘 = 𝑅12(𝑙𝑠 − 𝑙𝑘 ) + 𝜆(𝑁12

𝑠𝑘)

(III) Time Double Differentials

No Integer Ambiguity

Δ𝜙12𝑠 𝑡12 = 𝑓(𝑅12 𝑡1 − 𝑅12 𝑡2 , 𝐶𝑙𝑘12(𝑡12))

Change in orientation

S Δ𝜙12𝑠 (𝑡1) = Δ𝑅12

𝑠 𝑡1 + 𝜆𝑁12𝑠 + ΔClk12(𝑡1)

Δ𝜙12𝑠 (𝑡2) = Δ𝑅12

𝑠 𝑡2 + 𝜆𝑁12𝑠 + ΔClk12(𝑡2)

Change in Orientation, but unpolluted by Integer Ambiguity

Δ

Two Observations on Orientation

Satellite Double Differentials Time Double Differential

Drone Absolute Orientation, but polluted by Integer Ambiguity

Analogous to compass and gyroscopes, but highly noisy

We take a filtering approach to handle the dynamic errors

Bayesian Filtering Approach

q q q

Transition

Measurement

Orientation Orientation Orientation

Temporal Differentials

Spatial Differentials

q q q

Trans.

Meas.

Δ𝜙12𝑠 Δ𝑡 = 𝑓(Δ𝑞, 𝐶𝑙𝑘12)

Δ𝑞

𝜙𝑖𝑗𝑠𝑘 = 𝑓(𝑞, 𝑁𝑖𝑗

𝑠𝑘)

Integer Ambiguity ?

Temporal Differentials

Spatial Differentials

Δ𝑞

Bayesian Filtering Approach

Integer Ambiguity Resolution depends on Flight Aggressiveness

Two regimes of aggressiveness

Low Dynamic Flights

Highly Aggressive flights

Kalman Filter

Particle Filter

Orientation

[𝑞2] Orientation

[𝑞1] Orientation

[𝑞3]

𝜙𝑖𝑗𝑠𝑘 = 𝑓(𝑞, 𝑁𝑖𝑗

𝑠𝑘)

Δq

Orientation Estimation - Low Dynamic flights

Static Drone

q2 = 𝑞1 + Δ𝑞

𝑁𝑖𝑗𝑠𝑘 = ⌊𝑓 , 𝜙𝑖𝑗

𝑠𝑘 ⌋

𝑞2 = 𝑓 𝜙𝑖𝑗𝑠𝑘 , 𝑁𝑖𝑗

𝑠𝑘

Trans.

Meas.

Kalman Filter

𝑞2

𝑞1 known

Pitch, Roll, Yaw [𝑞2]

Pitch, Roll, Yaw [𝑞1]

Pitch, Roll, Yaw [𝑞3]

𝜙𝑖𝑗𝑠𝑘 = 𝑓(𝑞, 𝑁𝑖𝑗

𝑠𝑘)

Δq

Highly Aggressive Flights: Cycle Slips

q2 = 𝑞1 + Δ𝑞

𝑞2 = 𝑓 𝜙𝑖𝑗𝑠𝑘 , 𝑁𝑖𝑗

𝑠𝑘

Trans.

Kalman Filter

Cycle Slips

Kalman filter derailed

Meas.

𝑞1 known

𝑁𝑖𝑗𝑠𝑘 = ⌊𝑓 , 𝜙𝑖𝑗

𝑠𝑘 ⌋ 𝑞2

Static Drone

Divergence due to cycle slips

Cycle Slips trigger divergence Several Cycle Slips during flight

Handling Aggressive Flights and Cycle Slips

• Kalman Filters are optimal under Gaussian error

– When estimates of N incorrect, error no longer Gaussian

We need 2 solutions: 1. Identify when estimates of N are poor

2. Correct estimates in real time to prevent divergence

Main Question

𝑁𝑖𝑗𝑠𝑘 = 𝑓(𝑞, 𝜙𝑖𝑗

𝑠𝑘)

cos (2𝜋𝑁𝑖𝑗𝑠𝑘) = cos (2𝜋𝑓 𝑞, 𝜙𝑖𝑗

𝑠𝑘 )

𝑐𝑜𝑠𝑚𝑒𝑡𝑟𝑖𝑐(𝑞) = Σ*ij,sk+cos (2𝜋𝑓 𝑞, 𝜙𝑖𝑗𝑠𝑘 )

Correct Orientation 𝑐𝑜𝑠𝑚𝑒𝑡𝑟𝑖𝑐 ≈ 1

𝜙𝑖𝑗𝑠𝑘 = 𝑓(𝑞, 𝑁12

𝑠𝑘)

Measurement Model

1. Identifying Poor Estimates of N

2. Correcting N via Particle Filters

)( ijNCos ~ ~ 1 High Conf.

q

f (thisN ij )

< 1 Low Conf.

v v

)max(arg1

ijCosNq

)(2maxarg2

ijCosNq

. .

.

v

Keep doing Kalman

v v v v

Trigger Particle Filter

v v v

~ ~ 1

v v

~ ~ 1 ~ ~ 1

Back to Kalman

Simple Particle Converges slowly

Δ𝑞

Measurement (q) Initial

Particles

𝑡𝑖𝑚𝑒0 𝑡𝑖𝑚𝑒1

Simple Particle Filter

Weigh and Propagate

Combine Trans. And Meas. like Kalman Filter

Initial Particles

𝑡𝑖𝑚𝑒0 𝑡𝑖𝑚𝑒1 𝑡𝑖𝑚𝑒2

Adjusted Particle Filter

Provides near optimal combining for correct particles

Accelerates convergence with few particles

Propagate the Adjusted State

Δ𝑞

Measurement (q)

Adjusted Particle Tracks Well

Adjusted Particle Tracks Well

GPS

GoPro

IMU

Pi

Evaluation Platform

Performance

Vision Ground Truth

GPS vs IMU

Sharp Angles

(Truth)

Tilt

Ori

enta

tio

n (

de

gree

) SafetyNet can track aggressive turns

Sharp Angles

SafetyNet can track aggressive turns

Comparison with IMU

Only 30% worse than IMU

Comparison with IMU

4x better than IMU

Conclusion

Despite Excitement, poor reliability is the show stopper for disruptive drone applications

We propose an orthogonal approach for drone state estimation using carrier phase GPS

We developed a Particle filter do systematically combine carrier phase data for orientation

Real Extensive flight tests achieve an accuracy comparable to IMU

Agenda

01

02

Localization for Drones

Indoor Follow-Me Drone

Indoor Follow-Me Drone

Wenguang Mao, Zaiwei Zhang, Lili Qiu, Jian He, Yuchen Cui, and Sangki Yun

The University of Texas at Austin

MobiSys 2017

Awkward postures

Burnout

Heavy Equipment

Goals • Maintain a constant following distance

• Maintain a favorable following orientation

Undesired Desired

Undesired Desired

Challenges

User

Drone Control ?

Indoor environments

Tracking ?

Existing Approaches

• GPS

– Only available for outdoor

• Computer Vision

– Color-filter based: not accurate or robust

– Image-descriptor based: computationally heavy

• Proportional Integral Derivative (PID) Controller

– Simplicity

– No system knowledge: may affect control performance

Robust Acoustic-Based Indoor Tracking (RABBIT)

User

Drone

Mobile

Mic

Audio for tracking

RABBIT MPC

RF for comm.

WiFi

DroneTrack

FMCW

FFT 𝑓1

Time

Frequency

Transmitted chirp

Received chirp (direct path)

𝑓1

𝑡𝑑 cos (2𝜋𝑓1) Mixing

Scaling

𝒕𝒅

? ?

? ?

Multipath Impact

cos (2𝜋𝑓1)

Received chirp (reflected path)

𝑓2

Time

Frequency

Transmitted chirp

Received chirp (direct path)

𝑓1

𝑡𝑑 Mixing FFT

Scaling

+cos (2𝜋𝑓2)

What if there is a reflected path close to the direct one?

MUSIC

• Super-resolution parameter estimation algorithm

• Widely used for estimating AoA

𝑥 𝑛 = cos (2𝜋𝑓𝑖𝑛)

𝐾

𝑖=1

𝑓1

𝑓2 𝑓3

𝑓1 𝑓2

MUSIC

RABBIT

cos (2𝜋𝑓1)

Received chirp (reflected path)

𝑓2

Time

Frequency

Transmitted chirp

Received chirp (direct path)

𝑓1

𝑡𝑑 Mixing

Scaling

𝒕𝒅

+cos (2𝜋𝑓2)

The same model as that used for MUSIC

Significantly enhance the capability of resolving multipath

RABBIT

Problem Solution

Multipath interference • Apply MUSIC to FMCW • Compensate speaker distortion

Propeller noise • Remove false peaks in the spectrum • Apply Kalman filter

Computation efficiency • Sub-sampling based approach

RABBIT

Problem Solution

Multipath interference • Apply MUSIC to FMCW • Compensate speaker distortion

Propeller noise • Remove false peaks in the spectrum • Apply Kalman filter

Computation efficiency • Sub-sampling based approach

Compensate Speaker Distortion Peak in simulation

Filter

Transmitted chirps

Speaker Mic

Acoustic channel Pro-

cessing

Peak after compensation Peak in real systems

RABBIT

Problem Solution

Multipath interference • Apply MUSIC to FMCW • Compensate speaker distortion

Propeller noise • Remove false peaks in the spectrum • Apply Kalman filter

Computation efficiency • Sub-sampling based approach

Remove Noise Impact

• Remove false peaks

– Temporal continuity

– Properties of MUSIC peaks

• Apply Kalman filter to remove noise in distance estimation

– Combine distance and velocity measurement

– Leverage the redundancy

Peak for the direct path

False peak

RABBIT

Problem Solution

Multipath interference • Apply MUSIC to FMCW • Compensate speaker distortion

Propeller noise • Remove false peaks in the spectrum • Apply Kalman filter

Computation efficiency • Sub-sampling based approach

Improve Efficiency

• Eigenvalue decomposition on the autocorrelation matrix

• 1400 effective samples 700 × 700 autocorrelation matrix

Matrix Size

20 × 20 30 × 30 40 × 40 60 × 60 90 × 90 120 × 120

Time (ms)

1.6 2.9 4.5 11 22 39

MUSIC Running Time on Samsung Galaxy S7

Improve Efficiency

𝒙𝟏 Mixed signals: 𝒙𝟐 𝒙𝒌 …… …… …… 𝒙𝒍𝒌+𝟏 𝒙𝒍𝒌+𝟐 𝒙(𝒍+𝟏)𝒌 …… 𝒙𝒌+𝟏 𝒙𝒌+𝟐 𝒙𝟐𝒌

Sub Sampling: ……

……

……

𝑹𝟐

𝑹𝟏

𝑹𝒌

𝑹𝒔𝒖𝒃

Combing

Model Predictive Control (MPC)

User

Drone

Mobile

Mic

Audio for tracking

RABBIT MPC

RF for comm.

WiFi

DroneTrack

MPC

Maintain desired distance 𝒅𝟎

• Measure the current distance 𝑑𝑐 using RABBIT

• Predict the user movement 𝑑𝑢

• Model the relation 𝒎 between the drone input and the displacement, and determine the next input u, s.t. , 𝑚 𝑢 = 𝑑𝑐 + 𝑑𝑢 − 𝑑0

User

Drone

𝒅𝒄

𝒅𝒖 𝒅𝟎

Implementation

• A.R.Drone 2.0

– Speakers

– Audio amplifier

– Audio player

– Battery

• Samsung Galaxy S7

– Android NDK (tracking)

– Android SDK (others)

Tracking Performance

0.7cm

Robust to multipath and propeller noise

Tracking Performance

0.7 – 0.8 cm

Robust to environment noise

Computation Cost

Tracking delay Overall delay

9 ms 13 ms

Low computation cost

Overall Performance

Compare various schemes

• DroneTrack with mobile in hand

• DroneTrack with mobile in bag

• FMCW with mobile in hand

Overall Performance

Videos taken by DroneTrack are smooth

Conclusion

• A new acoustic-based tracking system

• A holistic design of an indoor follow-me drone

• A prototype system and extensive evaluation

• Future work: combining with vision-based approaches

End of This Chapter