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