estimating store 6dof trajectories using sensor fusion ... · 6-dof telemetry analysis...
TRANSCRIPT
![Page 1: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/1.jpg)
ITEA Test Instrumentation Workshop 1: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Estimating Store 6DOF Trajectories Using Sensor Fusion between Photogrammetry
and 6DOF Telemetry
by Ed Forsman and David Schug Atlantic Test Range
NAWCAD
Name: Ed Forsman David Schug Code: 5.2.4.4 5.2.4.4 E-mail: [email protected] [email protected]
Approved for public release – distribution is unlimited NAVAIR Public Release 11-602
![Page 2: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/2.jpg)
ITEA Test Instrumentation Workshop 2: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Motivation for Sensor Fusion
• Both PhotoG and 6DOF TM produce (x, y, z, φ, θ, ψ) store trajectory / time history
• Each has weaknesses, failure modes – Camera failures, vapors – TM dropout, calibration bias
• Sensor fusion benefits – Increased accuracy – Better fault tolerance – Faster turnaround for photoG
(potential)
Euler angles:
φ ↔ roll, θ ↔ pitch, ψ ↔ yaw
![Page 3: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/3.jpg)
ITEA Test Instrumentation Workshop 3: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Store Separation Photogrammetry (PhotoG)
Process Steps • Acquire and track targets (mi)
• Camera orientation: find camera position & orientation (6DOF) that minimizes difference between measured (mi) and calculated (fi) 2-D target locations
• Store 6DOF determination: find store 6DOF that minimizes difference between measured and predicted 2-D target locations in all cameras simultaneously
2
{ , , , , , } 1
:{ , , , , , | , , } { , }
arg min , , , , ,
i i i i i i
N
i ix y z i
f x y z x y z u v
D x y z f m
![Page 4: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/4.jpg)
ITEA Test Instrumentation Workshop 4: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
6-DOF Telemetry Analysis
• Instrument senses motion in the store’s (body) reference frame
• Solution process 1) Integrate body-referenced rotation rates to
get aircraft-referenced angles and rates 2) Rotate accelerations from body coordinates
to aircraft coordinates 3) Subtract aircraft accelerations 4) Integrate accelerations twice to get velocities
then positions 5) Perform lever arm corrections for position,
velocity and acceleration
6-DOF Package • 3 accelerometers • 3 rate gyroscopes • Battery & transmitter • Fits in fuse well
![Page 5: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/5.jpg)
ITEA Test Instrumentation Workshop 5: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
PhotoG vs. 6DOF TM
Multi-Camera PhotoG 6DOF TM Package
P
R
O
S
•Accurate positions & angles
•Redundant (many cameras)
•Reliable (statistical QA)
•Fault tolerant
•Accurate accelerations and rates
•Fast (<1 hour turnaround)
•Straightforward analysis
•Solution can continue until store
impact
C
O
N
S
•SLOW (3-24 hour turnaround)
•Complex
•Weather dependant (e.g. vapors)
•Poor derivatives
•Assumes entire aircraft is a rigid
body
•Solution stops when store goes
out of view
•Accuracy dependant on calibrations
& initial conditions (drift)
•TM noise, dropouts in any of 6
parameters ends analysis
•Fault intolerant
Sensor Fusion Goal: Keep the PROS but lose the CONS
![Page 6: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/6.jpg)
ITEA Test Instrumentation Workshop 6: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Sensor Fusion Solver: Unscented Kalman Filter
• State transition model describes how state evolves from one time to the next
• Observation models predict measurements given a state – PhotoG model predicts target
locations in imagery – 6DOF TM model predicts
accelerations and rates in TM package
• Uncertainty models for process, measurement and state estimation errors
2
1 1 1
1
1 1
1 1
1
1
2k k k k
k k k
k k k
k k k
k k
k k
p p p p
p p p
p p
State Transition Model:
p represents 3-D position
represents 3-D rotation
ω represents 3-D body rotation rate
τ is the time step
![Page 7: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/7.jpg)
ITEA Test Instrumentation Workshop 7: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Sensor Fusion Solution Mechanism: Unscented Kalman Filter (UKF)
Wait for next
measurement
Data
Type
?
Predict image
(u,v)’s Predict accels
& rates
Predict state
at next time
,k kx P 0 0,x P
Correct state
by comparing predicted
vs. actual measurements
,k kx P
k=k+1 PhotoG 6DOF TM
τ = Δt
![Page 8: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/8.jpg)
ITEA Test Instrumentation Workshop 8: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Experimental Tests
• We test our filters and models using both synthetic data and real-world flight test data – Synthetic tests allow comparison to objective truth, control
of inputs and errors – Real-world testing exercises solver in intended
environment, w/ unknown • 5 Solvers tested
– Sensor Fusion of PhotoG and 6DOF TM using UKF – PhotoG only
• Non-linear least squares (NLLSQ) and UKF – 6DOF TM only
• Predictor-corrector ODE solver and UKF • Failure modes
– PhotoG dropout – TM dropout – TM parameter loss
![Page 9: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/9.jpg)
ITEA Test Instrumentation Workshop 9: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Synthetic Data Generation
• Use a damped periodic function to generate a synthetic 6-DOF time history – Continuous in 0th and 1st derivative, discontinuous in
2nd and up – 6 different values for amplitude (α), for decay (β), and
for frequency (γ), one for each 6-DOF parameter – x, y, z and φ have low frequency, fast decays; θ and ψ
have higher frequency, slower decay
• Generate PhotoG and 6DOF TM measurements based on each data point in synthetic time history
• Add Gaussian noise to measurements • Use same measurements in each solver and compare
to truth trajectory
Position: f(t)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1
Velocity: f'(t)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1
Acceleration: f''(t)
![Page 10: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/10.jpg)
ITEA Test Instrumentation Workshop 10: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Tests Using Synthetic Data
Root Mean Square (RMS) Performance Metric
• Add Gaussian white noise to measurements
• Run solver • Subtract truth from solution • Repeat 100 times to estimate RMS
error
2
1
1RMS (observed truth)
N
i
iN
![Page 11: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/11.jpg)
ITEA Test Instrumentation Workshop 11: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
RMS Error Results
![Page 12: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/12.jpg)
ITEA Test Instrumentation Workshop 12: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
RMS Error Results
![Page 13: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/13.jpg)
ITEA Test Instrumentation Workshop 13: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
RMS Error Results
![Page 14: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/14.jpg)
ITEA Test Instrumentation Workshop 14: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Increased PhotoG Noise (5x)
![Page 15: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/15.jpg)
ITEA Test Instrumentation Workshop 15: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Flight Test Data
• 500 lbs bomb ejected from F-18E wing – Tracking data from 3 cameras – 6DOF TM data
• Failure modes – PhotoG dropout – TM dropout – TM parameter loss
![Page 16: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/16.jpg)
ITEA Test Instrumentation Workshop 16: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Flight Test Results
![Page 17: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/17.jpg)
ITEA Test Instrumentation Workshop 17: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Flight Test Results
![Page 18: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/18.jpg)
ITEA Test Instrumentation Workshop 18: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Flight Test Results
![Page 19: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/19.jpg)
ITEA Test Instrumentation Workshop 19: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Failure Modes
• PhotoG dropout • TM dropout • TM parameter dropout
![Page 20: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/20.jpg)
ITEA Test Instrumentation Workshop 20: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
PhotoG Dropout: Loss of 2 out of 3 Cameras
![Page 21: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/21.jpg)
ITEA Test Instrumentation Workshop 21: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
TM Dropout
![Page 22: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/22.jpg)
ITEA Test Instrumentation Workshop 22: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
TM Dropout
![Page 23: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/23.jpg)
ITEA Test Instrumentation Workshop 23: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
TM Parameter Dropout: Roll Rate Gyro
![Page 24: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/24.jpg)
ITEA Test Instrumentation Workshop 24: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Conclusion: Sensor Fusion is…
• More accurate than either photoG or 6DOF TM alone – SF produces more accurate positions, angles and
velocities – TM mitigates noise in photoG – PhotoG provides accurate initial conditions – PhotoG corrects biases in TM
• More fault tolerant – Dropouts in one data source are mitigated by the other
source – Fewer data collection failures due to video or TM
problems – Fewer flights saves programs time and money
![Page 25: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/25.jpg)
ITEA Test Instrumentation Workshop 25: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Future Work
• Shorten turnaround time of photoG process using Sensor Fusion – Initially estimate trajectory using 6-DOF TM – Refine solution incrementally as images are tracked and analyzed – Continue photoG tracking until trajectory estimate is “good enough”
(UKF provides statistics quantifying solution quality) – Much less photoG data needed to get accuracy?
• Correct for time latency in 6DOF TM • Correct for accelerometer bias • Incorporate aircraft accelerations & rates into model
Questions?
![Page 26: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/26.jpg)
ITEA Test Instrumentation Workshop 26: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Backup Slides
![Page 27: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/27.jpg)
ITEA Test Instrumentation Workshop 27: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
• Rotation rate equations:
• Use unit quaternions instead of Euler angles to avoid gimbal lock
• Initial value problem using ordinary differential equation (ODE) solver
6-DOF Telemetry Analysis
sin
cos cos sin
( sin cos ) / cos
1
2q q
ω ↔ body-referenced rotation rate
q ↔ (q0 , iq1, jq2, kq3)
![Page 28: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/28.jpg)
ITEA Test Instrumentation Workshop 28: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Increased PhotoG Noise (5x)
![Page 29: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/29.jpg)
ITEA Test Instrumentation Workshop 29: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Increased PhotoG Noise (5x)
![Page 30: Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis •Instrument senses motion in the store’s (body) reference frame •Solution process 1) Integrate](https://reader033.vdocument.in/reader033/viewer/2022050122/5f527bb142705b0f1913c0f0/html5/thumbnails/30.jpg)
ITEA Test Instrumentation Workshop 30: 5/11/2011 GET CONNECTED to LEARN, SHARE, AND ADVANCE.
Flight Test Results