Simulation of Tightly Coupled

INS/GPS Navigator

Ade Mulyana, Takayuki Hoshizaki

December, 2001

Purdue University

December,2001 1
Model and Parameters to Drive Simulation
Trajectory Input
Aircraft
Turbulence Input
Model

Time Input Aircraft Motion

Satellite Constellation

Errors
Processing Mode
Antennas GPS INS
Number, Location

Errors Position, Attitude, Rates Position, Attitude, Rates

Filter

Aircraft Position & Attitude

Estimate and Uncertainty
Covariance data
Errors Transformation to Sensor
Position, Attitude, and Uncertainty
passing
Errors Synthetic Image Generation
Sensor Parameters Errors
Target Tracking
Image Acquisition Imaging
Parameters
System Multi-Image
Site Model Intersection

Target Coordinates Graphic Animation

Uncertainty, CE90

December,2001 2
 Outline

1. Overview
2. Structure of Simulation
3. Simulation Models
4. Kalman Filter
5. Initial Conditions
Error Source Specifications
6. Results
7. Conclusions

December,2001 3
 Overview
(1)UAV Dynamics
Nominal Trajectory
(2) Navigation Equation
INS Output
(3) Tightly Coupled INS/GPS
INS/GPS Output
Covariance Data
(4) Covariance data is passed to
Imagery Analysis
December,2001 4
 Structure of Simulation

Tightly Coupled
INS/GPS
INS Position
Velocity
UAV IMU Nav
Orientation
Position, Velocity, Orientation
and Covariance correction Covariance
Bias Correction -
Kalman
Filter
+

GPS Receiver

December,2001 5
 Simplified IMU Model

~
x x x
where
x = Bias + White Noise

~x : Sensor Output
x : Sensor Input
Bias : Markov Process, tc=60s
for all
ax ,ay,az Accelerometer Outputs
x
x , y , z Rate Gyro Outputs

December,2001 6
 GPS Receiver Model
Pseudorange
GPS (X x) 2 (Y y) 2 (Z z) 2 c t
Pseudorange Rate
d d dt d
c
dt GPS dt dt dt
where
X, Y, Z : Satellite Position
x , y, z : Platform Position
c t : Pseudorange equvalent
Clock Bias (Random Walk)
dt
c : Pseudorange rate equivalent
dt
Clock Drift (Random Walk)
: Normally Distributed Random Number
d
: Normally Distributed Random Number
dt
December,2001 7
 Kalman Filter: Error Dynamics
d
x F x Gv
dt

x [ , , , Orientation Angle Errors

v N , vE , vD , Velocity Errors
, , h, Position Errors
Bx , By , Bz , Gyro Biases
Bax , Bay , Baz , Accelerometer Biases
dt
ct, c ] Clock Bias and Drift
dt

17 States Kalman Filter

December,2001 8
 Kalman Filter: Output Equation

GPS X x (Y y) 2 ( Z z) 2 ct
2

d d dt d
c
dt GPS dt dt dt
INS X x INS (Y y INS ) 2 ( Z z INS ) 2 ct INS
2

d d dt
c
dt INS dt INS dt INS

Measurement: Z d Random Noise: d

dt dt
Output Equation:
ZGPS ZINS H x

where H
x
December,2001 9
 Initial Error Condition
Initial Errors
[0 , 0 , 0 ] so that
[E10 , E 2 0 , E30 ] [0.001,0.001,0.002] rad
[v N , v E , v D ]0 [0.1,0.1,0.1] m/s
[, ]0 [1.57e 7,1.57e 7] rad
h 0 1 m
[ Bx , By , Bz ]0 [ B , B , B ] rad / s
[ Bax , Bay , Baz ]0 [ Ba , Ba , Ba ] g
[ b, d ]0 [ S b , Sd ] [ m, ( m / s)]
Initial Covariance Values
P0 (, , ) [02 , 02 , 02 )] rad 2
P0 (v N , v E , v D ) [v 2N 0 , v 2E 0 , v 2D 0 ] ( m / s) 2
i
2
P0 ( Bi ) ( rad / s) 2
ai
2
P0 ( Bai ) g2
P0 ( b, d ) [Sb , Sd ] [ m 2 , ( m / s) 2 ]

December,2001 10
 Error Source Specifications

INS

Accelerometers Notation LN-100G LN-200IMU Units

Bias () Ba 25 50 g
White Noise (sqrt(PSD)) N a 5 50 g / sqrt (Hz )

Rate Gyros
Bias () B 0.003 0.35 deg/hr
White Noise (sqrt(PSD)) N 0.0015 0.07 deg/sqrt(h r)
(deg/hr/sqrt(Hz))
(good) (worse)

2 levels of INS are used for Simulation

December,2001 11
 Error Source Specifications

GPS
GPS Receiver Notation Receiver 1 Receiver 2 Units
Pseudorange () r 6.6 33.3 m
Pseudorange Rate () rr 0.05 0.5 m/s
ClockBias White Noise(PSD) Sb 0.009 0.009 m2
ClockDrift White Noise(PSD) Sd 0.0355 0.0355 ( m / s) 2
(good) (worse)

2 levels of GPS Receivers are used for

Simulation

December,2001 12
Satellite Geometry during the Simulation

December,2001 13
 Local Frame: x, y, z
Zecef
Nominal Trajectory
v x 61 (m / s)
(200 ft / s) x
h 0 6096 (m)
(20000 ft )
y

Yecef
z
Xecef x=Zecef
y=-Yecef
z=Xecef-6378137m
December,2001 14
 Result 1:Comparisons between INS/GPS
and Unaided INS;(Good INS,Good GPS)
Local Frame Position Errors: (true) (estimated)
Local Frame Position Errors
1000

500

dx (m) dx (m)
0

-500
0 50 100 150 200 250 300 350 400

1000

INS/GPS Error
500
dy (m)

Unaided INS Error

dy (m) 0

-500
0 50 100 150 200 250 300 350 400

30

20
dz (m)

dz (m) 10

-10
0 50 100 150 200 250 300 350 400
time (s)

0 400 (sec)
INS/GPS works very well

December,2001 15
 Result 1:Comparisons between INS/GPS
and Unaided INS;(Good INS,Good GPS)
Local Frame Velocity Errors: (true) (estimated)
Local Frame Velocity Errors
4

dvx (m/s)
2

dv x (m / s) 0

-2
0 50 100 150 200 250 300 350 400

4
dvy (m/s)

2 INS/GPS Error
Unaided INS Error

dv y (m / s) 0

-2
0 50 100 150 200 250 300 350 400

0.3

0.2
dvz (m/s)

0.1
dv z (m / s) 0

-0.1
0 50 100 150 200 250 300 350 400
time (s)

0 400 (sec)
INS/GPS works very well

December,2001 16
 Result 1:Comparisons between INS/GPS
and Unaided INS;(Good INS,Good GPS)
Local Frame Euler Angle Errors: (true) (estimated)
-3
x 10 Euler Angle Errors
1

dE1 (rad)
0

droll (rad) -1

-2
0 50 100 150 200 250 300 350 400
-4
x 10
5 INS/GPS Error
Unaided INS Error
0
dE2 (rad)

-5

dpitch (rad) -10

-15
0 50 100 150 200 250 300 350 400
-3
x 10
0
dE3 (rad)

-1

dyaw (rad) -2

-3
0 50 100 150 200 250 300 350 400
time (s)

0 400 (sec)
Roll and Pitch errors are quickly corrected
Yaw error correction takes time
Effect on Geo Positioning?
December,2001 17
 Result 2:Ensembles
(Good INS,Good GPS)
Local Frame Position Errors: (true) (estimated)
Local Frame Position Errors
0

-0.5

dx (m) dx (m)
-1

-1.5

-2
0 50 100 150 200 250 300 350 400

0
dy (m)

dy (m) -2

-4
0 50 100 150 200 250 300 350 400

1
dz (m)

dz (m) -1

-2
0 50 100 150 200 250 300 350 400
time (s)

0 400 (sec)
Position error is less than 3m LN-100G:10mCEP
Error value is not 0 mean locally
December,2001 18
 Result 2:Ensembles
(Good INS,Good GPS)
Local Frame Velocity Errors: (true) (estimated)
Local Frame Velocity Errors
0.05

dvx (m/s)
0

dv x (m / s) -0.05

-0.1
0 50 100 150 200 250 300 350 400

0.05
dvy (m/s)

dv y (m / s) -0.05

-0.1
0 50 100 150 200 250 300 350 400

0.1
dvz (m/s)

0.05

dv z (m / s) 0
-0.05
-0.1
0 50 100 150 200 250 300 350 400
time (s)
0 400 (sec)
Velocity error is less than 0.05m/s
LN-100G:0.015m/s(rms)
December,2001 19
 Result 2:Ensembles
(Good INS,Good GPS)
Local Frame Euler Angle Errors: (true) (estimated)
-3
x 10 Euler Angle Errors
1

dE1 (rad)
0

droll (rad) -1

-2
0 50 100 150 200 250 300 350 400
-3
x 10
1
dE2 (rad)

dpitch (rad) -1

-2
0 50 100 150 200 250 300 350 400
-3
x 10
1

0
dyaw (rad)
dE3 (rad)

-1

-2

-3

0 0 50 100 150

400 (sec)
200
time (s)
250 300 350 400

Angle error is about 0.003 deg for roll and pitch,

0.06 deg for yaw,
LN-100G:0.002deg (rms) for all pitch, roll and yaw
December,2001 20
 Result 3: Comparisons between 4patterns
blue red (:) black (-.) green (--)
INS good worse worse good
GPS good good worse worse
Local Frame Position Errors: (true) (estimated)
Local Frame Position Errors
10

5
dx (m)
dx (m) 0

-5
0 50 gIgG 100 150 200 250 300 350 400
wIgG
10 wIwG
gIwG
5
dy (m)

dy (m) 0

-5
0 50 100 150 200 250 300 350 400

0
dz (m)

dz (m) -5

0 -10
0 50
400 (sec)
100 150 200
time (s)
250 300 350 400

200~300s covariance and nominal trajectory data

are passed to imagery analysis
GPS performance directly affects position errors
December,2001 21
 Result 3: Comparisons between 4 patterns
blue red (:) black (-.) green (--)
INS good worse worse good
GPS good good worse worse
Local Frame Velocity Errors: (true) (estimated)
Local Frame Velocity Errors
0.3

0.2

dv x (m / s) dvx (m/s)
0.1

0
gIgG
wIgG
-0.1
0 50 100 150 200 250 300 wIwG
350 400
gIwG
0.2

0.1
dvy (m/s)

dv y (m / s) 0

-0.1

-0.2
0 50 100 150 200 250 300 350 400

0.3

0.2
dvz (m/s)

dv z (m / s) 0.1

-0.1
0 50 100 150 200 250 300 350 400

0 400 (sec)
time (s)

GPS performance directly affects velocity errors

December,2001 22
 Result 3: Comparisons between 4patterns
blue red (:) black (-.) green (--)
INS good worse worse good
GPS good good worse worse
Local Frame Euler Angle Errors: (true) (estimated)
-3
x 10 Euler Angle Errors
1

dE1 (rad)
0

droll (rad) -1
gIgG
wIgG
-2
0 50 100 150 200 250 300 wIwG
350 400
x 10
-4 gIwG
5

0
dE2 (rad)

dpitch (rad) -10

-5

-15
0 50 100 150 200 250 300 350 400
-3
x 10
0

dyaw (rad)
dE3 (rad)

-1

-2

-3
0 50 100 150 200 250 300 350 400
time (s)
0 400 (sec)
INS accuracy helps orientation accuracy
December,2001 23
 Conclusions

We have successfully built a realistic

integrated INS/GPS which will be used
to study the effects of navigation
accuracy on target positioning
accuracy.
The INS/GPS is good at correcting roll
and pitch angles, but not yaw angle.
Improving GPS accuracy improves
aircraft position accuracy. Improving
INS accuracy improves aircraft attitude
accuracy. Both aircraft position and
attitude are needed to locate the target.
December,2001 24
 Future Work
GPS
Use of carrier phase observations
Use of dual frequencies
Differential carrier phase GPS
INS
Estimate Scale Factor and Nonlinearity as
well as Bias:
Sx M xy M xz a x B x D x

a M yx Sy M yz a y B y D y

M zx M zy Sz a z Bz D z

December,2001 25
 References
(INS)
[1] Titterton, D. H. and Weston, J. L. (1997). Strapdown Inertial
Navigation Technology. Peter Peregrinus Ltd.
[2] Rogers, R. M. (2000). Applied Mathematics In Integrated
Navigation Systems. AIAA Education Series.
[3] Chatfield, A. B. (1997). Fundamentals of High Accuracy
Inertial Navigation. Volume 174, Progress in Astronautics and
Aeronautics. AIAA.
[4] Britting, K. R. (1971). Inertial Navigation Systems Analysis.
Wiley Interscience.
(Kalman Filter)
[5] Brown, R. G. and Hwang, P. Y. C. (1985). Introduction to
Random Signals and Applied Kalman Filtering. John Wiley &
Sons.
[6] Gelb, A. (1974). Applied Optimal Estimation. M.I.T. Press.

December,2001 26
 References (Cont.)
(Navigation Sensors)
[7] B. Stieler and H. Winter (1982). Gyroscopic Instruments and
Their Application to Flight Testing. AGARDograph No.160
Vol.15.
[8] Lawrence, A. (1992). Modern Inertial Technology. Springer-
Verlag.
[9] IEEE Standard Specification Format Guide and Test Procedure
for Single-Axis Laser Gyros. IEEE Std. 647-1995.
(GPS)
[10] Kaplan. E. D. (1996). Understanding GPS Principles and
Applications. Artech House.
(Others)
[11] Military Standard for Flying Qualities of Piloted Aircraft 1797A.
[12] Department of Defense World Geodetic System 1984, Its
Definition and Relationships with Local Geodetic Systems,
National Imagery And Mapping Agency Technical Report

December,2001 27
 Kalman Filter:Output Equation
h1 h1 h1
x 1 x e
y z
y
1 1 0 e
h k h k h k
1 k4 z e
x y z b
k
k x
1 1 h1 h1 h1
1 e
y e
x y z

k GPS
k INS 0 z e
h k h k h k

k4 d
1 ~
x
x y z ~y
~
z

H1
k : Number of Visible Satellites
x e , y e , z e : Platform ECEF Coordinate s
h i Xi x

x (X i x ) 2 (Yi y) 2 ( Zi z) 2

December,2001 28
 Kalman Filter:Output Equation

x x x
h
x e y y y
0 0
y e 0 0
z 0 0 h 0 0 0 0
e 43 43 z z z 43 43
b
x e h
1 0

x
y e 0 0 0
z 0 0
e NED 0
d 0 T ECEF 0 0 0 0

43 0 0

0 0 0
43 43 43 0 1

H2
x [, , , v N , v E , v D , , , h ,
Bx , By , Bz , Bax , Bay , Baz , b, d ]

December,2001 29
 Simplified IMU Error Model
~
a a a
where 0
Sx M xy M xz a x B x D x

a M yx S y M yz a y B y D y

M zx M zy Sz a z B z D z
~
a : Sensor Output

a : Sensor Input
Bi : Bias (MarkovProc ess)
B Bias STD for ensembles
25 g for Accelerome ters
0.003 deg/ hr for Gyros
Di : White Noise
PSD D (5 g ) 2 for Accelerome ters
(0.0015 deg/ hr ) 2 for Gyros
December,2001 30
 Clock Error Model

b d D b
d Dd
b ct : Clock Bias
d ct : Clock Drift
D b White Noise , PSD 0.009 m 2
D d White Noise , PSD 0.0355 (m / s) 2
Updating & Propagation in the Kalman Filter
b k b k
K k ( ZGPS Z INS )
d k d k
b k 1 1 T b k
0 1
d k 1 d k
December,2001 31