vikas-ddp

Integration of Inertial Navigation System

and Global Positioning System

Using Kalman Filtering

By,

Vikas Kumar N., 99D01010

Under the guidance of

Prof. K. Sudhakar

OverviewInertial Navigation System (INS)

Global Positioning System (GPS)

Kalman Filtering

Simulation work

Hardware Implementation

Strap Down INSBody fixed gyroscopes &

accelerometers

p

q

r

ax

ay

az

Euler parameter integration

Euler angle computation

DCM

Gravitation &

Centrifugal correction

U, V, Wintegration

VN, VE, VD

calculation

, ,

Latitude,LongitudeAltitude

integration

H

Flight Dynam

ics & C

ontrol

INS module

N DEp Vq U VV V HWr

FDC States

ZYX

aaaV

rqp

zyx

T

,,

,,

Euler angle integration

1 sin cos0 cos sin

tan tan

s0 sin cosec sec

pqr

Euler parameters - Quaternions

0

1 1

2

33

0

2

001

2 00

p q rp r qq r

ee

pr q

e

e pe

e

ee

123

22

21

20 eeee


11 3 0 2[ )]n 2(si e e e e

2 2 2 21 0 1 2 3

2 3 0 121 3 0 2

[2( )]1

sc4(

n)

ios ge e e e

e e e ee e e e

2 2 2 21 0 1 2 3

1 2 0 321 3 0 2

[2( )]1

sc4(

n)

ios ge e e e

e e e ee e e e

Direction Cosine MatrixFrom local earth or navigation

frame to body frame.

coscoscossinsinsincossinsincossincoscossincoscossinsinsinsincoscossinsin

sinsincoscoscos

Navigation frame

sin

cos

sin0

cosDCM

mrqp

rqp

Acceleration CorrectionsCentrifugalGravitational

sin

cos sin

cos cos

x

y

z

g

g

U

V

W

a

a

a

Vr Wq

Ur Wp

Uq Vp g

WVU

DCMVVV

ZYX

T

D

E

N

NavigationFrame Body

Frame

Latitude integration

Longitude integration

Height integration

earth

N

RV

cosearth

E

RV

DVH

p

q

r

ax

ay

az



DCM

Gravitation &


U, V, Wintegration

VN, VE, VD

calculation

, ,


integration

H

Flight Dynam

ics & C

ontrol

INS module

N DEp Vq U VV V HWr

INS Errors• Alignment – roll, pitch, yaw errors• Gyroscope

– Bias or drift – constant gyro output– Scale factor error

• Accelerometer– Bias (changes randomly after each turn-on)– Scale factor error

• Nonorthogonality of gyro and accelerometer• Random measurement noise• Navigation errors – position & velocity

GPSSpace segment

– 24 satellitesControl segment

– Ground controlUser segment

– Receiver– Civil & military

Gives position in terms of latitude, longitude and altitude

GPS (contd.)L1 primary signal at 1575.42 MHz

– Clear Acquisition Code (C/A) – civilian use

• Unencrypted signal– Precise Code (P) – military use

• Encrypted, higher bandwidth, more precise

L2 secondary signal at 1227.6 MHzC/A can be degraded intentionally –

Selective Availability

GPS errorsEphemeris error

– Incorrect satellite location transmitted by GPS

– Affects ranging accuracySatellite Clock errorsMultipath reflection errorsAtmospheric delays

– Ionospheric– Tropospheric

Random measurement noise – Dilution of Precision (DOP)

GPS module

Flight Dynam

ics

X

Y

Z


conversion

H

Conversion using WGS-84Distance corresponding to a

degree change in latitude is given by

Distance corresponding to a degree change in longitude is given by

cos

sincos180 2222

2

h

ba

aF olon

h

ba

baF olat23

2222

22

sincos180

Where a and b are the equatorial and polar radius of the earth respectively.

Latitude at current location

Longitude at current location

2 1Lat

XF

2 1Lon

YF

Accelerometer Modelling Ideally

Actually

C is the scale factor in mV/g

D is the 0 g offset in V

16-bit ADC modellingFinally

0( ) ( )outputAcc C c a D d

0outputAcc C a

500065536

output c

c

Acc Da

C

Gyroscope ModellingIdeally

Actually

C is the scale factor in mV//s

D is the constant drift in /s16-bit ADC modellingFinally

0( ) ( )outputGyro C c p D d

0outputGyro C p

500065536

outputc

c

Gyrop D

C

Errors due to temperature effects ignored

Errors due to non-orthogonality of gyros and accelerometers ignored

GPS : Random error introduced with standard deviation of 20m.

Error Implementation

p

q

r

ax

ay

az



DCM

Gravitation &


U, V, Wintegration

VN, VE, VD

calculation

, ,

X,Y, HeightIntegration

H

H

Sensor modelling

Flight Dynam

ics

Kalman Filter

Corrected position

AimTo reduce the effect of errors in the

sensors so that INS can give out correct values of the position of the aircraft

To use the GPS output as a tool to estimate the error in the INS and to correct the error as much as possible by using the method of Kalman filters

Kalman FilterStochastic estimatorEstimate a state x with a

measurement z

1 1wx uv

xz x

kk k k

k k k

A BH

wk and vk represent process and measurement white noise with covariances Q and R.

If is the a priori estimate of the process at time tk, then the error covariance matrix is defined as

xk

kkk xxe ˆ

)( Tkkk eeEP

Compute Kalman Gain :

1)( kTkkk

Tkkk RHPHHPK

Update estimate with measurement kz

)ˆ(ˆˆ kkkkkk xHzKxx

Compute error covariancefor update estimate

kkkk PHKIP )(

Project Ahead:

kTkkkk

kkkk

QAPAP

BuxAx

1

1 ˆˆ

Prior estimate:kk Px ,ˆ

Feedback aided INS

Feedforward aided INS

9 State Filter ModelPosition, Velocity, Attitude and Gravity

perturbation equations

C

v

r r

g

(I E

v

C

r

v

g

)n n nb b

n n n

n

n n

n

n

n

Position Dynamics Equation

earth

N

RV

cosearth

E

RV

Dh V

N E D

rvN E D

N E D

V V V

FV V V

h h hV V V

r r vn n nrr rvF F

rr

h

Fh

h h hh

Velocity Dynamics Equation

ˆˆ ˆv C f (2 ) vn b n nb

2

0earth

earth

RR h

v r v (f ) C fn n n n n n bvr vv bF F

Attitude Dynamics Equation

n r v (( ) ) Cn n n n ber ev b ibF F

0E ( ) 0

0

D En n

D N

E N

State Space Model

x x uF G 0

(f )(( ) )

rr rvn

vr vv

er ev

F F

F F FF F

0 0C 00 C

nb

nb

G

fu

b

bib

Discrete Kalman Filter1x x wk k k k

exp( ) Ik F t F t

[u( )u( ) ] ( ) ( )TE t t Q t t

2 2 2 2 2 2ax ay az x y zQ diag

[w w ]T T Tk k k k kQ E GQG t

z r rn nk INS GPS

3 3 3 3 3 30 0kH I

2 2 2[v v ]Tk k k hR E diag

Correction Algorithm

ˆr r rn n n

ˆv v vn n n

ˆC (I E )Cn n nb b

Hardware Implementation

Overview

Description of target hardware

System flow

DSP simulator

Future Work

Target HardwareRequirements

– Compact– Light weight– Single voltage supply operated– Speedy & accurate calculations

• INS data acquisition & computations within 10ms• GPS data acquisition and Kalman filter

computation within 10ms (preferred)Two blocks

– GPS & INS Data Acquisition Card (GIDAC)– Navigation Processor Card (NPC)

GIDACGPS data acquisition thru Field

Programmable Gate Array (FPGA) based chip– Only 1 chip required– Faster than micro-controller

Accelerometer & Gyroscopes– Acquisition using a 2nd order

Butterworth filter – Digitizing using 16-bit, 6 channel ADC

INS acquisitionSensor signals are signal

conditioned– Filtered for noise– Scaled to 0 - 5V

6 channel, 16 bit, parallel output ADC (ADS8364) chosen– Simultaneous sampling is done– Digitized signals sent to NPC

GPS acquisition – FPGA chipReads data from GPS receiver in SIRF

(proprietary) sentence formatOne chip & faster processing due to

presence of internal DPRAMStores & calculates the positions given

by the GPS in SIRF format extremely fastInternal DPRAM stores the final

calculated position NPC every 1 sAsynchronous communication

maintained to save processor time

NPCDSP TMS320VC33Supporting Hardware

– Parallel port for connectivity– PAL chip 22V10– DPRAM 7130– JTAG connector to download emulator– Other required peripherals

DSP TMS320VC33Floating point DSP

– Max. Instruction cycle time 13ns or 150MHz– 75MIPS, 150MFlops– Parallel multiplication & ALU operations in a

single cycle– 2 timers

50 pin connector interface with external circuitry

34K words dual access SRAMInexpensive

Supporting HardwareDPRAM for parallel data transfer &

storing outputControl signals to select peripheral

chips given by Programmable Array Logic chip – Provides output for control of

dataflow, hold & read signals to ADC

Accelerometer

ax, ay, az

Gyroscopesp, q, r

2nd OrderL P F,

Gain & Scale to 0 – 5 V

16 bitSimultaneou

ssampling

ADC

16

FPGAGPSReceiv

er

8

DSPTMS320vc3

3

DPRAM Navigation Output to Control Electronics

NPC GIDAC

IMU

System FlowInitialization of DSP & peripherals

– 2 interrupts /INT0 & /INT1 defined– Timer0 & Timer1 configured– Software reset given to ADC by

program – operate in CYCLE mode– Timer0 provides clock to ADC at 5MHz– Timer1 interrupts every 10ms or 100Hz– Start_INS = 0, GPS_available = 0

• 6 channels of ADC – A0, A1, B0, B1, C0, C1 grouped 2 at a time– EOC signal in 3 pulses– Variable count = 2, used

to count these signals – /HOLDx signals are made

low whenever Timer1 overflow occurs 6 channels of ADC are sampled

• Main program waits in IDLE mode

Interrupt Service RoutinesEOC of ADC (/INT1) occurs

– Data from 6 channels stored– Start_INS = 1– Return to main program

Reading of GPS by FPGA (/INT0) occurs– Data from internal DPRAM read by

NPC– GPS_available = 1– Return to main program

DSP simulatorCode Composer Studio 3x4x

– Simulates actual DSP, target hardware not required

– C & assembly source codes can be interfaced and debugged

Inputs from C program read every 10ms from a file on PC (actual sensors not used)– Voltages from sensors in hexadecimal

format– GPS data in hexadecimal format as would be

given by SIRF sentence – position along N,E,D in m GPS modelling

– Initial state at t = 0 was hardcoded in the program

Features of CCS3x4xProbe points

– File reading– File writing– Similar to sensor reading

Output in COFF format– Assembly language code to be loaded in

TMS320C3x assemblerProfile points

– Enable clock– Count cycles– Time taken by program on a real DSP can be

computed

Computation No. of Cycles Time taken(26ns/cycle)

INS 44940 1.17msGPS & KF 2443314 6.33msADC sampling -- 1msDPRAM writing -- 1ms

•Tested for 50s running of the program

•Memory consumed 17K words – no external RAM required

•Accuracy upto 3-4 decimal places as matched with the MATLAB output

Future WorkEmulator to run assembly code on target

hardware requiredResults & timing to be checked on target

hardware for consistencySensor board can be designed

– Sensor modelling in program to be modified according to sensors being used

– Calibration to be done to calculate scale factors & biases of sensors.

Initial state at t = 0 currently hardcoded or read from a file better method to be devised

Output currently written on DPRAM a simultaneous display interface to be created

More number of states can be implemented on Kalman filter for higher accuracy of output

THANK YOU !

Documents

vikas-ddp