introduction to integrated navigation systems - muhammad ushaq

17-Sep-15 Integrated Navigation Systems (Muhammad Ushaq) 1

Introduction to Inertial Navigation System

&Integration with External Navigation Aids

Dr Muhammad Ushaq([email protected])

Institute of Space Technology

Islamabad, Pakistan

mailto:[email protected]

Navigation

17-Sep-15Integrated Navigation Systems (Muhammad Ushaq) 2

• The estimation of the state (position, velocity, and attitude) of

moving body in real time, with respect to some known reference

• A navigation system may be completely self-contained aboard the

navigating body e.g. Inertial Navigation System

Or

• It may require an external infrastructure as well as user

components, such as radio navigation systems (GPS, GLONASS,

Galileo, Beiduo, etc)


The science of NAVIGATION has played an important role for mankind

throughout the history.

Individuals, groups and nations who could reliably travel to and from

distant places have been successful militarily and politically.

Significance of Navigation


Pilotage

Recognizing (visually) landmarks to know

where you are and how you are oriented.

Dead reckoning

Estimating the position of a vehicle based

on its previous position and its course and

speed over a known interval of time.

Celestial navigation

Estimating the angles between local

vertical/horizon and known celestial objects

(e.g., the sun, moon, planets, stars) to

estimate attitude & Position.

Types of navigation

http://en.wikipedia.org/wiki/File:Marine_sextant.svg

http://en.wikipedia.org/wiki/File:Marine_sextant.svg

//upload.wikimedia.org/wikipedia/commons/8/8b/Dead-reckining.svg

//upload.wikimedia.org/wikipedia/commons/8/8b/Dead-reckining.svg


Radio navigation

Relies on radio-frequency sources

with known locations. Global

navigation satellite systems (GNSS)

use beacons on satellites for that

purpose.

Inertial navigation

INS relies on knowing initial position,

velocity, and attitude and thereafter

measuring attitude rates and

accelerations. It is the only form of

navigation that does not rely on

external sources/references.

Integrated Navigation

Combination of 2 or more Nav Systems

Master

FilterSINS

GPS

CNS

KF-1For

SINS/GPS

KF-2For

SINS/CNS

Doppler

Radar

KF-3For

SINS/Doppler

Observation For KF-3

Time

Update

Measurement

Update

1

1ˆ , f fx P

1

2ˆ , f fx P

1

3ˆ , f fx P

1 1ˆ , x P

2 2ˆ , x P

3 3ˆ , x P

ˆ , f fx P

Co

rrection

to S

INS

Corrected Solution for Position, Velocity and Attitude

+

-

+

-

+

-



Types of navigation

Reference Frames


Explicit definition of a certain number of reference frames is the

fundamental to the process of navigation.

Each reference frame is an orthogonal right-handed set of axes.

An important part of inertial navigation system synthesis consists of

finding the relationship between different frames.

The choice of the appropriate coordinate frame depends on the mission

requirements, ease of implementation, computer storage and speed, and

navigation algorithm complexity.

• True inertial frame

• Earth-centered inertial Frame

• Earth-centered earth-fixed

• Local Level Frames

o North pointing frame

o Wander azimuth frame

• Body Frame

• Sensor Frame


Reference Frames

Greenwich meridian

Inertial reference meridian

Local meridian

Equatorial plane

Reference Frames


System Location

Frame Transformation


/

90 90( )

e o o

iei i i e e e g g g

e

tX Y Z X Y Z X Y Z ENU

about Z axis about Z axis about X axis

g Z axis axis axisg g g g g g g g g b b b

g g

X Y Z X Y Z X Y Z X Y Zabout about X about Y

g g g w w wgzx y z x y z

e to g g to be to w

Directional Cosine

Matrix (DCM)

Euler Angles Quaternion

11 12 13

21 22 23

31 32 33

b

a

C C C

C C C C

C C C

Three ordered

right-handed

rotations ( , , )

Single Rotation

about a defined

vector

four-parameter

representation

0 1 2 3Q q q i q j q k

Earth Frame to Nav Frame Transformation.avi

Navigation Frame to Body Frame Transformation.avi

Earth Frame to Wander Azimu Frame Transformation.avi


Inertial Navigation Sensors

Accelerometer:

Acceleration measurement

Gyroscope:

Keep track of the orientation of sensing axes of accelerometers.


Working of Inertial Sensors

Inertial Navigation Systems (INS)


A complete three-dimensional navigation solution (Position, Vel, Attitude)

IMU (Accelerometers, Gyroscopes, Electronics)

Characteristic of INS


Most accurate (short term basis) and complete navigation solution

(Position, velocity & attitude)

Self-contained

Least susceptible to weather conditions and electromagnetic interference

Reliance on initial conditions (alignment/aiming)

Reliance on the precision of the gyro & accelerometers

Errors grow/accumulate with time

Mechanization of INS

According to sensors mounting


• Platform Inertial Navigation Systems

Sensors are mounted on a stabilized platform. Platform is isolated

from angular rotation of the host-vehicle.

Platform stabilized wrt Inertial Frame. ***

Platform stabilized wrt Local level north-pointing Frame. ***

• Strapdown Inertial Navigation

Sensors are mounted/strapped-down rigidly on the host-vehicle.

Sensors sense the same angular rotation as experienced by the

host-vehicle.

Stabalized Platform Mechanization.avi

North Pointing Mechanization.avi

Platform inertial navigation

17-Sep-15 15Integrated Navigation Systems (Muhammad Ushaq)

Comparison of Platform and Strapdown Nav


Platform Strapdown

Sensor Mounting Physical platform Vehicle body

Sensor Measurement

Torquer command calculate

Platform Physical platform computational

Attitude Determination

Read from pick up Calculate form

Use of gyrosStabilize platform

In reference frameMeasure

p p

ipf b

ib

bf

)( n

in

p

ip b

nb

b

ib

n

bC

Platform inertial navigation

Inertial sensors are mounted on a stable platform

Mechanically isolated from the rotation of the vehicle

Provide very accurate estimates of navigation data

Single Axis Platform 2 -Axis Platform3 -Axis Platform

Gimbal lock problem

4 -Axis Platform

Gimbal lock problem

Resolved


Uni Axis.avi

2 Axis Platform.avi

3 Axis, 3 Gimbal+aircra+background.avi

4 Gimbal Tria Axis.avi

Computation of Velocity (Platform INS)

2

{2 sin [ ] tan } {2 cos [ ]}

{2 sin [ ] tan }

( ){2 cos [ ]} [ ]

( )

p pp p p px xx x ie y ie z

E E

p pp p p px zy y ie x y

E N

ppyp p px

z z ie x

E N

v vv f L L v L v

R h R h

v vv f L L v v

R h R h

vvv f L v g

R h R h


0

0

0

0

0

0

tp p p

x x x

tp p p

y y y

tp p p

z z z

v v v dt

v v v dt

v v v dt

Position Computation/Update

( )

cos( )

h

p

y

N

p

x

E

z

vL

R h

vL

R h

v


00 0

00 0

00 0

[ ]( )

[ cos ]( )

pt t y

N

pt t

x

E

t t

z

vL L Ldt dt

R h

vdt L dt

R h

h h v dtdt

Attitude is directly read from pick-offs


Attitude Update

Strapdown inertial navigation systems (SINS)

Inertial sensors (gyros, accelerometers) are directly attached/strapped-

down to the body of host vehicle.

: low cost, small size, light weight, greater reliability.

: computing complexity, sensors needed to measure higher rates

of turn.

/

e

90 90( )

Z axis Z axis X axis

e o o

iei i i e e e g g g

tX Y Z X Y Z X Y Z ENU

about about about

g Z axis axis axisg g g g g g g g g b b b

g g

X Y Z X Y Z X Y Z X Y Zabout about X about Y


Initial estimates

Of velocity&position

Body Mounted

accelerometers

Resolution of

Specific force

measurements

Body Mounted

gyroscopes

Attitude

computation

Gravity

Updating/Correction

Coriolis

correction

Navigation

computer

Position

Position

Velocity

& Attitude

Initial estimatesOf attitude

(Provided by Initial Align/Aiming)

b

ibfnf

b

ib

n

bC

n

ie n

en

SINS Computations


( 2 )n n n n n n

ib en ieV f V g

n b b

ib n ib

n b b

ib n ib

f C f

C

g

y

M

V

R h

( )

g

x

N

V

R h Cos

g

zh V

SINS Computations (velocity, Position)


( ) ( )

t t

t

t t t dt

( ) ( )

t t

t

t t t dt

( ) ( )

t t

z

t

h t t h t V dt

3 2

3 1

2 1

0

0

0

b

nC

bb bnn nb

dCC

dt

1 21

22

( )g

Ctan

C

1 13

33

( )g

Ctan

C 1

23( )

gSin C

( )

b

g

Cos Cos Sin Sin Sin Cos Sin Sin Sin Cos Sin Cos

Cos Sin Cos Cos Sin

Cos Cos Cos Sin Sin Sin Sin Cos Sin Cos Cos Cos

C

SINS Computations (Attitude)


0

( ) 0 ( )

0

b b

nbz nby

b b b b

n nbz nbx n

b b

nby nbx

C t C t

INS Errors

Sources of Errors

Fixed Drifts/Biases of inertial sensors.

Misalignments of sensor’s sensing axes with orthogonal axes.

Acceleration dependent drifts.

Scale Factor errors

Non linearity errors

Random errors (variations) in above mentioned errors

Computation errors

Initial Errors in Position, velocity and attitude (initial misalignments)

Sensor errors cause unboundedly growing errors in estimation of:

Position

Velocity

Attitude


Propagation of Errors

2

1

( ) ( )

gg gyg px x

x y z y x

M N M M

ie ie

VV VSin Tan Cos V h

R h R h R h R h

2

1

( ) ( )

gg gy g px x

y x z x ie y

M M N N

ie

VV VSin Tan V Sin h

R h R h R h R h

2

2( ) ( ) ( )

gg g gy gx x x

z x ie

N M N N N

px y zie

VV V VtanCos V Cos Sec tan h

R h R h R h R h R h


Propagation of Attitude Errors


2

2

(2 )( ) ( )

(2 os ) 2 2( )

( ) (

g ggyg g g g g g gxz

x y z z y x y

N N N

g ggx yg g gx

z ie z ie y

N N

g gg gx yx z

N N

ie

ie

V tan VVV f f V Sin tan V

R h R h R h

V VVC V Sin V Cos V Sec

R h R h

V VV V

R h R

2

)

p

xtan hh

2

2

2 2

2 ( )( )

2 ( ) ( ) ( )

gg gyg g g g g g g gx z

y z x x z x y z

N M M

g g gg gx y zg px x

ie x y

N N M

ie

VV Tan VV f f Sin V V V

R h R h R h

V Tan V VV V SecCos V h

R h R h R h

2 2

2 os 2 2( )

( ) ( )

ggyg g g g g g g gx

z x y y x x y ie x

N M

g g gy px x

z

M N

ie

VVV f f C V V Sin V

R h R h

V V Vh

R h R h

Propagation of Velocity Errors


2

1

( )

g

yg

y

M M

VV h

R h R h

2

1

( ) ( ) ( )

g g

g x x

x

N N N

V VV Tan Sec h

R h Cos R h R h Cos

g

zh V

0bx 0by 0bz

0bx 0by 0bz



Propagation of Position Errors

Propagation of Sensor Errors

17-Sep-15 29

0 1000 2000 3000 4000 5000 6000 7000 8000-15000

-10000

-5000

0

Attitude Error Arc Sec

z []

0 1000 2000 3000 4000 5000 6000 7000 8000-500

0

500

1000

1500

x []

0 1000 2000 3000 4000 5000 6000 7000 8000-1

0

1

2x 10

4

y []

Time [sec]

Attitude Errors in Standalone SINS

Integrated Navigation Systems (Muhammad Ushaq)

17-Sep-15 30

0 1000 2000 3000 4000 5000 6000 7000 8000-4000

-2000

0

2000

Velocity Error [m/s]

V

E

0 1000 2000 3000 4000 5000 6000 7000 8000-20

0

20

40

60

V

N

0 1000 2000 3000 4000 5000 6000 7000 80000

5

10x 10

4

V

U

Time [sec]

Velocity Errors in Standalone SINS


17-Sep-15 31

Position Errors in Standalone SINS

0 1000 2000 3000 4000 5000 6000 7000 8000-15000

-10000

-5000

0

5000

Position Error

(L

at)

(m

)

0 1000 2000 3000 4000 5000 6000 7000 8000-10

-5

0

5x 10

5

(L

on

)(m

)

0 1000 2000 3000 4000 5000 6000 7000 80000

2

4

6x 10

7

(h

) (m

)

Time [sec]



Calibration of Inertial Sensors

Remove/compensate the structural errors in the sensor outputs.

Structural errors are the differences between sensors expected output

and their measured output.

The measurements made by the sensor can be compensated in real-

time to digitally remove any errors.

Calibration provides a means of providing enhanced performance by

improving the overall accuracy of the underlying sensors.

Random errors cannot be estimated by calibration.

Statistical tools are employed to minimize the effect of random

errors on performance.


Non Inertial Navigation Systems

Radio navigation

• GNSS (GPS, GLONASS, Galileo, Beiduo)

• Psudolites (Ground-based)

• Doppler Radar (onboard)

• Other navigation systemsTACAN, Loran, Omega,VOR, DME, VOR/TACAN, and JTIDS

RelNav

Celestial navigation (Astronavigation)

Global Positioning System (GPS)


35

Space segment

Consists of 24 satellites

Six orbital planes

Altitude: 20183～20187 km

Orbital period: 12 h

L-band frequencies:

L1 : 1575.42 MHZ

L2 : 1227.60 MHZ

17-Sep-15 Integrated Navigation Systems (Muhammad Ushaq)

LIFU/Lectures/Vid/Animation 1/GPS Constellation/GPS Constellation.avi

36

Available Services for GPS Users

Standard Position Service(SPS)

Precise positioning Service(PPS)


37

Navigation Message

GPS time

Ephemeris data (positions of satellites at a given time)

Atmospheric propagation correction data

System almanac data

Any other information needed by the GPS receivers


38

Control segment

17-Sep-15

Consist ofMonitor stations, a master

control station and uplink

antennas


FunctionMonitor and control the orbits of the satellites;

Maintain the GPS system time

Upload necessary information to the satellites.

User segment

Function

Selects the optimally positioned satellites and using the navigation

signals from them measures four independent pseudoranges and

pseudorange rates.

Coverts signals to three-dimensional position and velocity of the

receiver/carrier vehicle.



Errors in GPS Signal

Source Effect (m)

Signal

arrival C/A±3

Signal

arrival P(Y)±0.3

Ionospheric

effects±5

Ephemeris

errors±2.5

Satellite

clock errors±2

Multipath

distortion±1

Tropospheric

effects±0.5

C/A ±6.7

P(Y) ±6.0


GPS Augmentation

With other GNSS

For GPS receivers capable of utilizing signals

from other satellite systems such as GLONASS,

Galileo etc, these extra systems can increase

the number of satellite signals in view

Differential GPS (DGPS)

Ground-based reference stations to broadcast

the difference between the positions indicated

by the satellite systems and the known fixed

(pre-surveyed) positions.

The digital correction signal is typically

broadcasted locally over ground-based

transmitters of shorter range.


GPS Augmentation

Satellite Based Augmentation System (SBAS)


Wide Area Augmentation System (WAAS) USA

European Geostationary Navigation Overlay Service (EGNOS) EU

Multi-functional Satellite Augmentation System (MSAS) Japan

Quasi-Zenith Satellite System (QZSS) Japan

GPS Aided Geo Augmented Navigation (GAGAN) India

System for Differential Correction and Monitoring (SDCM) Russia

Satellite Navigation Augmentation System (SNAS) China

Implementation & Coverage of SBAS


SNASBeiDou

Implementation & Coverage of SBAS

Kalman Filter

A linear quadratic state estimator.

Operates recursively on streams of noisy measurements

to produce a optimal estimate of the state.

Prediction: Prediction of the current state based on

previous state

Correction: After availability of measurement correction is

made using a weighted average, with more weight being

given to estimates with higher certainty


Kalman Filter –The Work Horse for Integrated Navigation

A key function performed by the Kalman filter is the

statistical combination of external (non-inertial)

information and INS information to track or contain drifting

parameters of the sensors in the INS.


State Vector Time Update

Error Covariance Time

Update

Take Measurement

Computation of Kalman

Gain

Measurement Update of

State Vector

Measurement Update of

State Error Covariance

Matrix

1ˆ ˆ

k k kx x

1 1

T

k k k k kP P Q

1( )T T

k k k k k k kK P H H P H R

ˆ ˆ ˆ( )k k k k k kx x K z xH

T T

k k k k k k k k kP I K H P I K H K R K

ˆˆ

ˆˆ

GI

k

GI

RRZ

VV

Discrete Kalman Filtering equations

Compute Kalman Gain :1)( k

T

kkk

T

kkk RHPHHPK

Update estimate

with measurement

)ˆ(ˆˆ kkkkkk xHzKxx

Compute error covariance

for update estimate kkkk PHKIP )(

Project Ahead:

1

1

ˆ ˆk k k

T

k k k k k

x x

P P Q

Prior estimate:

kk Px ,ˆ

kz

Kalman filter in action

ˆkx

Measurement

Output17-Sep-15 47Integrated Navigation Systems (Muhammad Ushaq)

17-Sep-15 48

Extensions/variants of Kalman filter

Extended Kalman Filter

Unscented Kalman Filter

Particle Filter

Unscented Particle Filter

Federated Kalman Filter

Adaptive Kalman Filter



Integrated Navigation Systems

Integrated Navigation Systems

To control the unboundedly growing trend in INS errors, navigation

solution from external navigation aids are fused with navigation solution

from INS using information fusion methodologies.

17-Sep-15 50

Time


Scope of Integrated Navigation

With the swift advancement in the inertial sensor

technologies and computing power, the strapdown

inertial navigation system (SINS) has replaced the

conventional gimbaled systems.

Error sources in the inertial instruments plus

unpredictable variations in the gravitational field

forces combine to cause a gradual error build up

navigation solutions.

The extended duration of the vehicle’s flight and

absence of updates from the ground sources lead

to a greater probability of errors in the navigation

solution.

Therefore, an external aiding is deemed vital

to augment the navigation system for

precision navigation.


Errors are bounded

Low data rate

No attitude information

Susceptible to jamming

(intentional and unintentional)

High data rate

Unbounded errors

Self-contained (un-susceptible to jamming)

GPS Vs INS

Radio

Navigation

GPS,

GLONASS

Galileo

etc

Both translational and rotational

information

INS


Loosely Coupled Integrated Navigation

IMU

GPS

MeasurementsGPS Kalman

Filter

Navigation

Kalman

Filter

Accelerometer Bias &

gyro drift correction

(Optional)

Position, velocity and

attitude correction

+

-

Navigation

Algorithm

INS derived position

velocity & attitude

GPS derived

position & velocity17-Sep-15 53Integrated Navigation Systems (Muhammad Ushaq)

Tightly Coupled Integrated Navigation

IMU

GPS

Measurements

Navigation

Kalman

Filter

Accel. Bias & gyro

drift correction

Position, velocity and

attitude correction

+

-

Navigation

Computations

INS derived pseudo

range & delta range

GPS derived pseudo range &

delta range


17-Sep-15 55

Mathematical Model for SINS/GPS Integ

Tg g g

x y z x y z bx by bz bx by bzx V V V h

T

rx ry rz rx ry rzW

2 2 2 2 2 2

gx gy gz ax ay azQ diag

( ) ( ) ( ) ( ) ( )x t F t x t G t w t 6 9 6 6 15 15

0 0

N S

X

F FF

3 3

3 3

9 3 9 3

0

0

0 0

n

b

n

b

C

G C

2 2 2 2 2 2 2 2 2 2 2 2 2 2 2

E N U E N U bx by bz bx by bzV V V hP

T

Vx Vy Vz hv v v v v v v

2 2 2 2 2 2

Gx Gy Gz G G GV V V hR diag

ˆ ˆ

ˆ ˆ

ˆ ˆ

ˆ ˆ

ˆ ˆ

ˆ ˆ

i G

i i G G

i G

g g

ix Gx

g g

iy Gy

g g

iz Gz

Rm h Rm h

R h Cos R h Cos

h hz

V V

V V

V V

0 0 0 1 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 1 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 Rm h 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 Rn h cos lat 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 1 0 0 0 0 0 0

H

2 2 3 3 1 1

.....2! 3! 1!

n n

k

F F FI F

n

2 2 3 3 1 1

.....2! 3! 4! !

n n

k

F F F FI G

n

3 3

3 3

3 3 3 3

0

0

0 0

n

b

n

S b

C

F C

NF


Centralized Data Fusion

Measurement

Computation

for Integrated

Navigation

Integrated

Navigation

Data Fusion

Navigation

Computation

Processing Electronics

CCD

Ephemeris Data

Celestial Nav System

Correction

GPS

Receiver

InertialSensors Electronics

IMUCorrected Position, Velocity & Attitude

Attitu

de

Co

mp

uta

tion

Attitude

Velocity

Position

Velocity

Position

Attitude

Doppler RadarVelocity

Doppler radar


Limitations of Centralized Kalman Filter

Information from all navigation sources is fused in a single filter.

High Computational Load

Lacking fault tolerance

Characteristics of Federated Kalman Filter

Distribute the system information among parallel filters.

Increase system throughput by using parallel processing

Improved System reliability.

Multi level fault detection, isolation and recovery capability.

Federated Kalman Filtering


Federated Kalman Filter

Measurements

1.A priori state and error covariance of LFs 1. State estimate after measurements

2. Updating of error covariance

Initial state estimate &

covariance

Time Update (Prediction)

1 1 1Ti i iP k k k P k k Q k

ˆ ˆ1 1 , 1,2i i ix k k k x k i

1 1 1ˆ ˆ1 , 1 Ti i i i i i iP k x k P k k x k k H R k z k

ˆ ˆ1 1m i mx k k k x k

1 1 1Tm m mP k k k P k k Q k

3. Fusion of state and error covariance2. A priori state and error covariance of MF

1 1 1 11 2f mP k P k P k P k

Measurement Update

Information Sharing1( 1)i iQ k Q

1( 1) ( 1)i i fP k P k

ˆ ˆ( 1) ( 1)

1,2,i fx k x k

i m

ˆ ˆ ˆ( ) ( ) ( ) 1,2i i i i ix k x k K z k z k i

1 1

, , , , , ,

1

ˆ ˆ ˆN

f k f k m k m k j k j k

j

x P P x P x


Federated Kalman Filtering

17-Sep-15 59

Master

FilterSINS

GPS

CNS

KF-1For

SINS/GPS

KF-2For

SINS/CNS

Doppler

Radar

KF-3For

SINS/Doppler


Time

Update

Measurement

Update

1

1ˆ , f fx P

1

2ˆ , f fx P

1

3ˆ , f fx P

1 1ˆ , x P

2 2ˆ , x P

3 3ˆ , x P

ˆ , f fx P

Co

rrection

to S

INS


+

-

+

-

+

-




Simulation Results

39.5

39.6

39.7

39.8

39.9

40

40.1

40.2

115.4

115.6

115.8

116

116.2

116.4

116.6

116.8

-1

0

1

2

3

4

5

6

x 104

Latitude ()

Trajectory

Longitude ()

Alt

itu

de (

m)

Ideal Trajectory

Estimated Trajectory

Simulated trajectory for SINS/GPS Integration


17-Sep-15 61

0 500 1000 1500 2000 2500 3000 3500-4000

-2000

0

2000

4000

Attitude Error Arc Sec

z [

]

0 500 1000 1500 2000 2500 3000 3500-400

-200

0

200

400

600

800

x [

]

0 500 1000 1500 2000 2500 3000 3500-1000

-500

0

500

1000

1500

2000

y []

Time [sec]

Attitude errors - SINS/GPS integration

Simulation Results (cont..)


17-Sep-15 62

0 500 1000 1500 2000 2500 3000 3500-0.1

-0.05

0

0.05

0.1

0.15

Velocity Error [m/s]

V

E

0 500 1000 1500 2000 2500 3000 3500-0.1

-0.05

0

0.05

0.1

0.15

V

N

0 500 1000 1500 2000 2500 3000 3500-0.1

-0.05

0

0.05

0.1

V

U

Time [sec]

Velocity errors - SINS/GPS integration



17-Sep-15 63

0 500 1000 1500 2000 2500 3000 3500-500

0

500

1000

Position Error (m)

(La

t)

0 500 1000 1500 2000 2500 3000 3500-1500

-1000

-500

0

500

1000

(Lo

n)

0 500 1000 1500 2000 2500 3000 3500-10

-5

0

5

10

(h)

Time [sec]

Position errors - SINS/GPS integration




Fault Tolerant Integrated Nav Systems

Real systems have system/measurement noise may have

unknown biases, and varying covariance due to several non-

benign situations.

This results in degradation/divergence Kalman filter.

Remedy (one kind of): Adaptively tune the system noise

covariance and measurement noise covariance according to

real system and measurement noise

| 1

T

ad vk k kR C HP H

* * T

ad k vk kQ K C K | 1 | 1

1

1ˆ ˆ

kT

vk k k k k k k k k

i k M

C z H x z H xM

Fault Tolerant Integ Nav Systems

Master

FilterSINS

GPS

CNS-1

Fault Detection&

Adaption

KF-1For

SINS/GPS

CNS-2 +

Altimeter

KF-2For

SINS/CNS-1

KF-3For

SINS/CNS-2+Alt

Doppler

Radar

KF-4For

SINS/Doppler


Time

Update

Measurement

Update

1

1ˆ , f fx P

1

2ˆ , f fx P

1

3ˆ , f fx P

1

4ˆ , f fx P

1 1ˆ , x P

2 2ˆ , x P

3 3ˆ , x P

4 4ˆ , x P

ˆ , f fx P

Co

rrection

to S

INS


+

-

+

-

+

-

+

-

Fault Detection&

Adaption

Fault Detection&

Adaption

Fault Detection&

Adaption





17-Sep-15 66

0 500 1000 1500 2000 2500 3000 3500 4000 4500-4000

-2000

0

2000

4000

Attitude Error (Arc Sec)

z [

]

Standard KF

Adaptive KF

KF under normal conditions

0 500 1000 1500 2000 2500 3000 3500 4000 4500-800

-600

-400

-200

0

200

400

x []

Standard KF

Adaptive KF


0 500 1000 1500 2000 2500 3000 3500 4000 4500-400

-200

0

200

400

600

y []

Time [sec]

Standard KF

Adaptive KF


SINS/GPS Integration using AKF


17-Sep-15 67

0 500 1000 1500 2000 2500 3000 3500 4000 4500-0.1

0

0.1

0.2

0.3

0.4

Velocity Error (m/s)V

E [

m/s

]

Standard KF

Adaptive KF


0 500 1000 1500 2000 2500 3000 3500 4000 4500-0.1

0

0.1

0.2

0.3

0.4

V

N [

m/s

]

Standard KF

Adaptive KF


0 500 1000 1500 2000 2500 3000 3500 4000 4500-0.1

0

0.1

0.2

0.3

0.4

V

U [

m/s

]

Time [sec]

Standard KF

Adaptive KF




17-Sep-1568

0 500 1000 1500 2000 2500 3000 3500 4000 4500-2000

-1000

0

1000

2000

Position Error (m)

(L

at)

[m

]

Standard KF

Adaptive KF


0 500 1000 1500 2000 2500 3000 3500 4000 4500-1000

-500

0

500

1000

1500

(L

on

g)

[m]

Standard KF

Adaptive KF


0 500 1000 1500 2000 2500 3000 3500 4000 4500-40

-30

-20

-10

0

10

20

(H

) [m

]

Time [sec]

Standard KF

Adaptive KF




introduction to integrated navigation systems - muhammad ushaq

Documents