Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis...

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


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


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


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


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


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


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


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


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)


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


RMS Error Results


RMS Error Results


RMS Error Results


Increased PhotoG Noise (5x)


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


Flight Test Results


Flight Test Results


Flight Test Results


Failure Modes

• PhotoG dropout • TM dropout • TM parameter dropout


PhotoG Dropout: Loss of 2 out of 3 Cameras


TM Dropout


TM Dropout


TM Parameter Dropout: Roll Rate Gyro


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


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?


Backup Slides


• 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)






Flight Test Results

Date post:	17-Jul-2020
Category:	Documents
Upload:	others
View:	3 times
Download:	0 times

Estimating Store 6DOF Trajectories Using Sensor Fusion ... · 6-DOF Telemetry Analysis...

Documents