Gps Imu Ekf Navigation

8/11/2019 Gps Imu Ekf Navigation

1/68

GPS-IMU-EKF Navigation

Doc.No.: GPSIMU-INPE-002

I ss ue: I ss ue 1

Page: 1 of 68


Xr

Zf

B

A'

B'

C

A

C'

D'

D

Zr

Yr

XfYf

O

Project: Document No.:

Autonomous Aerospace Navigation System with integratedGlobal Positioning System and Inertial Measurement Unit

CNPq Process 471381/2004-7

GPSIMU-INPE-002

National Institute of Space Research - INPE

Av. dos Astronautas 1758 Phone: +55-12-3945-6198Sao Jose dos Campos-SP 12227-010 Fax: +55-12-3945-6226Brazil E-Mail: [email protected]

Doc. No.: GPSIMU-INPE-002

Issue: Issue 1

Written: Gustavo Baldo Carvalho Date: October 26, 2007

Approved: Dr. Helio Koiti KugaINPE Coordinator Date: October 26, 2007

Dr. Stephan TheilZARM Coordinator

National Institute of Space Research - INPE Page 1 of 68


2/68



I ss ue: I ss ue 1

Page: 2 of 68

Contents

1. Introduction 6

2. System description 7

2.1. Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.2. Inertial Measurement Unity . . . . . . . . . . . . . . . . . . . . . . . . . 72.3. Global Positioning System - GPS . . . . . . . . . . . . . . . . . . . . . . 82.4. Estimation and data fusion . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3. Definitions 11

3.1. Reference Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.2. Navigation variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4. Estimator 17

5. Navigation Mechanization 20

5.1. Chosing navigation frame . . . . . . . . . . . . . . . . . . . . . . . . . . 205.2. GPS measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225.3. Platform ECI Navigation mechanization . . . . . . . . . . . . . . . . . . 255.4. Gravitational Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295.5. Navigation transformation . . . . . . . . . . . . . . . . . . . . . . . . . . 30

5.5.1. Platform X Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . 315.5.2. Receiver Antenna X Vehicle . . . . . . . . . . . . . . . . . . . . . 325.5.3. Platform X Receiver Antenna . . . . . . . . . . . . . . . . . . . . 33

6. Ground Initialization and Alignment 34

6.0.4. Alignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 376.0.5. Initialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.0.5.1. Initialization with full priory vehicle information . . . . 386.0.5.2. Auto-Initialization . . . . . . . . . . . . . . . . . . . . . 40

7. Navigation Filter 417.1. State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

7.1.1. State Derivatives with respect to time . . . . . . . . . . . . . . . 427.2. State System Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

7.2.1. Derivative of Angular rate bias time Derivatives with respect toState Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

7.2.2. Derivative of Quaternions time Derivatives with respect to StateSpace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45



3/68



I ss ue: I ss ue 1

Page: 3 of 68

7.2.3. Derivative of Specific force bias time Derivatives with respect to

State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 477.2.4. Derivative of Speed time Derivatives with respect to State Space . 487.2.5. Derivative of Clock drift speed time Derivatives with respect to

State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 507.2.6. Derivative of Position time Derivatives with respect to State Space 517.2.7. Derivative of Clock bias range time Derivatives with respect to

State Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 527.3. System Noise Covariance Matrix . . . . . . . . . . . . . . . . . . . . . . . 537.4. Measurement coupling Matrix . . . . . . . . . . . . . . . . . . . . . . . . 54

7.4.1. Derivative of Pseudorange with respect to State Space . . . . . . 55

7.4.2. Derivative of Delta-Pseudorange with respect to State Space . . . 56

8. Bibliography 58

A. Physical Constants 61

B. Mechanics 62

B.1. Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62B.1.1. Scalar Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . 62B.1.2. Vectorial Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . 63

B.2. Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66



4/68



I ss ue: I ss ue 1

Page: 4 of 68

List of Figures

1. EKF Estimator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172. Pseudorange. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223. Satellite ranging. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234. Platform navigation mechanization. . . . . . . . . . . . . . . . . . . . . . 285. Platform/Antenna installation. . . . . . . . . . . . . . . . . . . . . . . . 306. Initialization and Alignment with vehicle priory information. . . . . . . . 347. Auto-Initialization and Alignment. . . . . . . . . . . . . . . . . . . . . . 368. Vectorial representation in cartesian reference frame. . . . . . . . . . . . 639. Vectorial relative representation. . . . . . . . . . . . . . . . . . . . . . . . 64

List of Tables

1. Summary of Continuous-Discrete EKF equations. . . . . . . . . . . . . . 192. WGS-84 definitions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 613. Earth zonal harmonics. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 614. Physical constants. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 615. Mathematical constants. . . . . . . . . . . . . . . . . . . . . . . . . . . . 61



5/68



I ss ue: I ss ue 1

Page: 5 of 68

Document Change Record

Issue Date Changed Pages /Changed Chapters

Remarks Done

Issue 1 October 26,2007

all Initial Version



6/68



I ss ue: I ss ue 1

Page: 6 of 68

1. Introduction

The aim of this document is the practical design and development for a navigation sys-tem via the integration of a Global Positioning System (GPS) receiver ([10], [19], [21]and [27]) and an Inertial Measurement Unity (IMU) ([3], [13], [16], [24], [28] and [31])through an Extended Kalman Filter (EKF) ([4], [5], [6], [9], [20], [36] and [38]) forsimulation and application in a real Hardware system.It presents schematics and mathematical models involving the integration, showingstrategies for their output processing in the direction of providing a blended naviga-tion solution since system power-ON status, including initialization and alignment, upto navigation status.

It also presents explanations about the navigation mechanization equations and rela-tionship between reference frames used in the navigation solution.The EKF constitutes the navigation filter, where the system model is developed with thestate space technique ([22]) used to fuse IMU measurements of specific force and angularrate ([11]) to the GPS Pseudoranges and Doppler measurements affected by clock biasand drift ([8]).

This document was composed with the support of a Scholarship CNPq/Embraer-Brazil.



7/68



I ss ue: I ss ue 1

Page: 7 of 68

2. System description

The system here developed is a navigation system which uses data fusion among differ-ent sensors to obtain the navigation solution.This navigation system is constituted of 2 sensors, GPS receiver and IMU, which are ca-pable to sense some physical entities related to the desired navigation. However, becauseof related biases, drifts and noises the sensors add errors to their measurements, makingthat the navigation solution provided directly from the sensors would be inaccurate andinviable for some applications.Hence, in order to provide accurate navigation solution, the noisy measurements areintegrated in a state propagation/update process with a known mathematic model of

the system. This constitutes the data fusion process among the sensors and dynamicmodel through an estimator.

2.1. Vehicle

The vehicle here is considered a flying Airspacecraft modeled as rigid body moving with6 degree of freedom under the effect of Earth gravitation and resulting specific force (asdrag and throttle for example) which rotates around its center of gravity (CG).The vehicle has only 2 navigation sensors installed: The IMU and GPS receiver withone antenna. The installation is modeled as fixed, that means, the platform and receiverantenna do not present relative motion or rotation with respect to the reference framefixed to the vehicle body CG. However it does consider distances from installation pointto the vehicle CG as well as angular displacements of each sensor reference frame withrespect to the vehicle reference frame.Although inputs and measurements from external world are made by the sensors withrespect to their position and attitude in the inertial space, the navigation solution of thevehicle reference frame with respect to the inertial space is the goal of this document.

2.2. Inertial Measurement Unity

One of the most used navigation sensors is the IMU (Inertial Measurement Unit) ([11]),

also known as platform. This sensor is capable to provide measurements of the three-dimensional specific force, as well as its attitude rate, both with respect to the inertialspace measured in the platform reference frame.In each orthogonal direction at least 2 types of primary sensors are required: an ac-celerometer and a gyroscope. Hence, to provide a three-dimensional solution, 3 orthog-onal accelerometers and 3 orthogonal gyroscopes are used embedded in the IMU.Due to the fact that the physical laws are well known, and thus the related propa-gating equations, the present position, velocity and attitude can be computed through



8/68



I ss ue: I ss ue 1

Page: 8 of 68

the time integration of the current kinematic accelerations and attitude rates. Since

the IMU senses the relative movement, its observations are used to propagate previousstate estimation to new state estimation, which are however highly dependent on initialconditions. Since the IMU does not sense the states directly, but their dynamics, it isconsidered a dead reckoning sensor.While the attitude rates are sensed by the embedded gyros, the accelerometers are capa-ble only of measuring the specific force. Since the kinematic acceleration, composed ofgravitational acceleration and specific force together, a gravitational model must be usedin function of position to calculate the gravitational part, thus allowing the integration.Although these physical laws do not change with time, the primary sensors and grav-itational models are not perfect. This causes that the measurements are affected by

various errors, drifts and misalignments during the navigation. Since the solution is apropagation from previous state estimation, the solution inevitably has unbounded errorgrowth with the time leading to inaccurate navigation computation. It is also impor-tant to remember that since the gravitational models are dependent on the position,more strictly on the altitude, and that the position available comes from the navigationsolution itself, there is a positve error feedback loop that contributes to unbound thepure solution obtained from the integration of measurements turning the platform intoa unbounded drifting sensor in the time.Therefore, although the IMU has the advantage of having high sample rate and of be-ing a stand alone sensor, it has its main disadvantages associated to the gyros drift,

accelerometers bias and gravitational models imperfections meaning poor long time ac-curacy and unbounded error growth.Hence, the fast navigation solution provided can not reach the accuracy requirements forlong duration missions. However, the measurements can be used in estimation processes,where other sensors can help on correcting the states, then avoiding unbounded growthof error while keeping the track to the actual position, velocity and attitude.

2.3. Global Positioning System - GPS

The Global positioning system ([10]), frequently known as GPS, is composed by a con-stellation of 24 or more synchronized satellites in different orbits around the Earth.

A GPS receiver is a sensor capable of measuring the three-dimensional present positionand velocity with respect to Earth-Centered reference frames (ECI or ECEF) by meansof using signals sent by the GPS constellation.Due to the fact that the GPS constellation orbits are well known, and thus the GPSsatellites position and speed, the present position and speed can be computed using theincoming GPS signals.Although the GPS is not a stand alone sensor, it can be thought as one, once it pro-vides the vehicle a navigation solution, independently of where it is around the Earth.



9/68



I ss ue: I ss ue 1

Page: 9 of 68

Since the GPS receiver senses the states directly, it does not need information about

previous states in order to produce a solution, that means, it is dependent on initialconditions. Hence the GPS receiver is considered a absolute sensor with bounded errorin time. This feature gives long operation time stability, what is not provided by a deadreckoning sensor like IMU.However, unlike the IMU, it can not provide a fast navigation solution because of itslow sample rate and the fact that the vehicle must have at least 4 GPS satellite signalsavailable.Therefore, although the advantage of being a stand alone sensor and the long operationtime stability, it has also associated inaccuracies due to the path of the satellite signalsto the receiver, which suffer the influence of the atmosphere, multipath effects and clock

bias and drift.Hence, the navigation solution provided by the GPS receiver can not reach high accuracyrequirements. However, the measurements can be used in estimation processes as aidingsensor, where they help inertial sensors keeping the track to the actual position, velocityand attitude.

2.4. Estimation and data fusion

As discussed by ([32]), there are 2 methods of integration: loosely coupled and tightlycoupled. While no information from the estimation filter is taken to the aiding sensor in

the loosely coupled method, in the tightly coupled method, the aiding sensor is feed withnavigation information, what enhances its measurement data integrity and performance.However, its implementation is more difficult due the fact that the design must considermore deep details about design and operation of each sensor. This poses a problemmainly for the GPS aiding sensor, which must permit manipulation of its internal algo-rithms for tracking satellite signals.Since such algorithms dictate the performance of the GPS sensor working alone, theyconfigure market secrets for the manufacturers, causing then that the number of openGPS receivers available for integration on tightly coupled systems is very small. Thatis one of the reasons why most of the developments available in the literature followsthe loosely coupled method ([32]) besides the fact of its easier implementation when in

comparison with the tightly coupled.Having these reasons in mind, this development intends the development of the nav-igation filter implementing the tightly coupled method as a contribution using a lowcost open source hardware available in the market (GPS1005-PCI from GPS Creations:www.gpscreations.com).As already discussed, none of the sensors above can reach high accuracy and timingrequirements at the same time when used alone. Hence, to improve the accuracy, thenavigation dynamics can be modeled and propagated using IMU measurements and then



10/68



I ss ue: I ss ue 1

Page: 10 of 68

updated using GPS through an estimator that implements sensor data fusion. However

the GPS receiver is treated here purely as sensor instead of navigation system itself. Thisapproach then brings navigation information to the receiver algorithm for producing itsobservations of satellite pseudoranges instead of the position. This implements then thetightly coupled integration method between a dead reckoning sensor represented by theIMU and an absolute sensor represented by the GPS receiver.Since the propagation equations configure a non-linear system, the estimation is providedthrough an Extended KalmanFilter (EKF) ([9]), working with data coming from IMUand GPS receiver with different sampling rates in an improved update ([6]).



11/68



I ss ue: I ss ue 1

Page: 11 of 68

3. Definitions

3.1. Reference Frames

The reference frames used in this document are:

XY Zi - ECI Reference Frame - Earth-Centered-Inertial reference frame fixed tothe distant stars ([7]).

XY Ze - ECEF Reference Frame - Earth-Centered-Earth-Fixed reference framefixed to the rotating Earth ([7]).

XY Zv

- Vehicle Reference Frame - reference frame fixed to the moving vehiclewith Xpointing forward in the longitudinal axis and Zdown.

XY Zr - Receiver Antenna Reference Frame - reference frame fixed to the receiverantenna, which in turn is fixed to the vehicle without relative movement withrespect to vehicle frame.

XY Zp - Platform Reference Frame - reference frame fixed to the platform, whichin turn is fixed to the vehicle without relative movement with respect to vehicleframe.

3.2. Navigation variablesHere the variables treated in this document are defined.

Time information:

t - Real time (s) t - Measurement of biased time (s) E- Sideral Time angle for Greenwich at t (rad)

Tei - ECI to ECEF rotation matrix (rad) iie - Earth angular rate vector (rad/s)

Installation information:

Platform with respect to vehicle: LvP- Platform Lever Arm in Vehicle Frame (m)



12/68



I ss ue: I ss ue 1

Page: 12 of 68

Tpv- Platform Installation matrix (Vehicle Frame to Platform rotation matrix)

It is assumed:

d(LvP)dt

= 0

d2(LvP)dt2

= 0

wvpv = 0 d(wvpv)

dt = 0

d(Tp

v)

dt = 0

d2(Tp

v)

dt2 = 0

Vehicle with respect to platform: LpV- Vehicle Lever Arm in Platform Frame (m)

Tvp

- Platform Frame to Vehicle rotation matrix

It is assumed:

d(LpV)

dt = 0

d2(LpV)

dt2 = 0

wpvp = 0

d(wpvp)dt

= 0

d(Tv

p)

dt = 0

d2(Tv

p)

dt2 = 0

Receiver Antenna with respect to vehicle: LvR - Receiver Antenna Lever Arm in Vehicle Frame (m)

Trv

- Receiver Antenna Installation matrix (Vehicle Frame to Receiver An-

tenna rotation matrix) It is assumed:

d(LvR)dt

= 0

d2(LvR)dt2

= 0

wvrv = 0 d(wvrv)

dt = 0



13/68



I ss ue: I ss ue 1

Page: 13 of 68

d(Tr

v)

dt = 0

d2(Tr

v)

dt2 = 0

Vehicle with respect to Receiver Antenna: LrV- Vehicle Lever Arm in Receiver Antenna Frame (m)

Tvr

- Receiver Antenna Frame to Vehicle rotation matrix

It is assumed:

d(LrV)dt

= 0

d2(LrV)

dt2 = 0

wrvr = 0 d(wrvr)

dt = 0

d(Tv

r)

dt = 0

d2(Tv

r)

dt2 = 0

Receiver Antenna with respect to Platform: LpPR - Receiver Antenna Lever Arm in Platform Frame (m)

T

r

p - Receiver Antenna Installation matrix (Platform Frame to Receiver An-tenna rotation matrix)

It is assumed:

d(LpPR

)

dt = 0

d2(LpPR)dt2

= 0

wprp = 0 d(wprp)

dt = 0

d(Tr

p)

dt = 0

d2(Tr

p)

dt2 = 0

Platform with respect to Receiver Antenna: LrRP- Platform Lever Arm in Receiver Antenna Frame (m)

Tpr

- Receiver Antenna Frame to Platform rotation matrix

It is assumed:



14/68



I ss ue: I ss ue 1

Page: 14 of 68

d(LrRP)

dt = 0

d2(LrRP)dt2

= 0

wrpr = 0 d(wrpr)

dt = 0

d(Tp

r)

dt = 0

d2(Tp

r)

dt2 = 0

Navigation States:

Vehicle: riV - Vehicle position in ECI (m)

viV - Vehicle speed in ECI (m/s)

aiV- Vehicle kinematic acceleration in ECI (m/s2)

qvi- Vehicle attitude quaternions with respect to ECI

Tvi- Vehicle attitude matrix with respect to ECI

iiv - Vehicle angular rate with respect to ECI in ECI (rad/s)

viv

- Vehicle angular rate with respect to ECI in Vehicle frame(rad/s)

iiv - Vehicle angular acceleration with respect to ECI in ECI (rad/s2)

viv- Vehicle angular acceleration with respect to ECI in Vehicle frame(rad/s2)

Platform: riP- Platform position in ECI (m)

viP- Platform speed in ECI (m/s)

aiP- Platform kinematic acceleration in ECI (m/s2)

qpi - Platform attitude quaternions with respect to ECI

Tpi - Platform attitude matrix with respect to ECI

iip - Platform angular rate with respect to ECI in ECI ( rad/s)

pip - Platform angular rate with respect to ECI in Platform frame(rad/s)

iip - Platform angular acceleration with respect to ECI in ECI (rad/s2)

pip- Platform angular acceleration with respect to ECI in Platform frame(rad/s2)

Receiver Antenna:



15/68



I ss ue: I ss ue 1

Page: 15 of 68

riR - Receiver Antenna position in ECI (m)

viR - Receiver Antenna speed in ECI (m/s)

aiR - Receiver Antenna kinematic acceleration in ECI (m/s2)

qri- Receiver Antenna attitude quaternions with respect to ECI

Tri- Receiver Antenna attitude matrix with respect to ECI

iir - Receiver Antenna angular rate with respect to ECI in ECI (rad/s)

rir - Receiver Antenna angular rate with respect to ECI in Receiver Antennaframe(rad/s)

iir - Receiver Antenna angular acceleration with respect to ECI in ECI(rad/s2)

rir - Receiver Antenna angular acceleration with respect to ECI in ReceiverAntenna frame(rad/s2)

Inputs and Measurements:

Platform: Inputs:

fpP

- Measurement of biased platform specific force in Platform frame

(m/s2)

pip - Measurement of biased platform angular rate in Platform frame(rad/s)

Computed values:

fpP

- Platform real specific force in Platform frame (m/s2)

bpf- Specific force bias in Platform frame (m/s2) pip - Platform real angular rate in Platform frame (rad/s) bp - Angular rate bias in Platform frame (rad/s)

p

ip - Platform real angular acceleration in Platform frame (rad/s2

) Receiver:

Measurements:

P R - Measurement of Pseudorange of satellite with respect to receiverantenna (m)

dP R - Measurement of Delta-Pseudorange of satellite with respect toreceiver antenna (m/s)



16/68



I ss ue: I ss ue 1

Page: 16 of 68

tR - Satellite signal biased reception time (s)

tT- Satellite biased transmission time (s) DC O - Measurement of carrier DCO frequency (Hz)

Computed values:

P R - Estimation of Pseudorange of satellite with respect to receiver an-tenna (m)

dP R - Estimation of Delta-Pseudorange of satellite with respect to re-ceiver antenna (m/s)

Rbc - Range corresponding to receiver clock bias (s)

Vbd - Speed corresponding to receiver clock drift (m/s) bSV - Estimated satellite clock bias (s) tr - Estimated satellite signal relativistic delay (s) TI- Estimated satellite signal ionospheric delay (s) TT- Estimated satellite signal tropospheric delay (s) TM- Estimated satellite signal multipath delay (s) T- Estimated satellite signal total extra receiver delay (s)

ri

S

- Estimated satellite position with respect to ECI (From NAV messageand transmission time estimation) (m)

R - Estimation of receiver antenna geometric range to satellite (m) viS - Estimated satellite speed with respect to ECI (From NAV message

and transmission time estimation) (m/s)

d(T)dt

- Variation of Satellite signal total extra receiver delay



17/68



I ss ue: I ss ue 1

Page: 17 of 68

4. Estimator

Since the equations treated here are non-linear, an estimator for non-linear processmust be used. This is here implemented by means of Continuous-discrete ExtendedKalman Filter EKF (continuous differential equations in the system state propagationand discrete measurements in the update process) ([9]).The figure 1 represents a summary for the implementation of the EKF.

Estimator

Propagation

Update

States

Propagation

Covariance

Propagation

Observation

model

Kalman Gain

States

Update

Covariances

Update

Linear F

matrix

Linear H

matrix

R matrix

Q matrix

+

-

X(-)

P(-)

P(+)

X(+)

Z h

h

System

dynamicsSensors

Dynamics

Noise

Measurements

Noise

+

+

ZX

wv

UInputs

Figure 1: EKF Estimator.



18/68



I ss ue: I ss ue 1

Page: 18 of 68

Given the state initial conditions X0, the estimator propagates the states X through

the system dynamics differential equations represented by fusing noisy measurementsZ.At the same time state covariancesP are also propagated using given initial conditionsP

0based on the current knowledge of the system process associated noises Q and on

the linearized system dynamics F around the current state propagation X. It shows

how accurate are the propagated states X.By using propagated states X and observation model h, the measurements can bepredicted and then compared to real incoming measurements Z. This difference is thenused to updated states X+ using an weighting gain K. This gain is based on the

knowledge of current states covariance propagation P, on the current knowledge of

noises associated to the measurement process R and on the linearized measurementsdynamicsHaround the current propagated state X. This is known as the data fusionprocess.The data fusion is also used to result in the updated state covariance P+, which will bethen again propagated and used to fuse new measurements into the state estimation.The state covariances update P+ is calculated also based on the current knowledge ofthe noises associated to the measurement process R, on the current states covariance

propagation P and on the linearized measurements dynamics H around the current

propagated state X.



19/68



I ss ue: I ss ue 1

Page: 19 of 68

The Table 1 summarizes the Continuous-discrete Extended Kalman Filter equations

used in the estimator development.

System Model X(t) = f(X(t), U(t), t) + G w(t)Measurement Model Zk = hk(X(tk)) + k, k= 1,...,N

Assumptions w(t) N(0, Q(t)), k N(0, Rk), E[w(t), Tk ] = 0Initial Conditions X(0) N(X0, P0)State Estimate Propa-gation

X(t) = f(X(t), U(t), t)

State Covariance

Propagation

P(t) = F(X(t), U(t), t)P(t) + P(t)FT(X(t), U(t), t)

+G.Q(t) GT

State Estimate Up-date

Xk+

= Xk

+ Kk

Zk hk(Xk

)

State Covariance Up-date (Joseph form)

Pk

+ =

I Kk H

k(Xk

)

Pk

I K

k H

k(Xk

)T

+ Kk R KT

k

Gain Matrix Kk

= Pk

HTk

(Xk

)

Hk

(Xk

) Pk

HTk

(Xk

) + Rk

1

Linearization Defini-tions

Hk

(Xk

) hk(X(tk))X(tk)

X(t)=

Xk

F(X

, U(t), t) f(X(t),U(t),t)X(t)

X(t)=X

Table 1: Summary of Continuous-Discrete EKF equations.



20/68



I ss ue: I ss ue 1

Page: 20 of 68

5. Navigation Mechanization

The navigation solution is constituted by position, speed and attitude obtained throughtime integrations of kinematic acceleration and angular rate.For a integrated GPS/IMU navigation, there are 3 possible candidates for the navigatingorigin: Vehicle CG, Platform reference origin and GPS antenna reference origin.A natural thought would be to implement the equations using the Vehicle CG. However,since the states to be integrated are measured by the platform and are not referred tothe vehicle, the use of the platform reference origin as navigating point is much moresuitable avoiding the complexity of the equations to be integrated when related to thevehicle CG which can also increase round-off effect.

Related to the use of the GPS antenna reference origin, besides the complexity of theequations and the round-off effect in the integration, there is also the fact of the differ-ence in the rates of data delivered by the platform in comparison to the GPS receiverdiscussed before. All these topics converge to the sense that such use is simply outof question in terms of computational efficiency. Furthermore, if the navigation solu-tion is somehow needed for the antenna reference origin, the vehicle navigation (or evenplatform navigation) can always be easily converted to it.

5.1. Chosing navigation frame

The choice for the reference frame to which the navigation mechanization is referredto is in principle arbitrary and actually based on requeriment criteria as speed of com-putation, accuracy, operational ease or required output coordinates. Usually, the lastcriterion is chosen in navigation applications.In most of the cases for near Earth navigation, local level reference frames as the North-East-Down (NED) or East-North-Up (ENU) (see [7]) are used. However, their mech-anization is far more complex then the mechanization in Earth-Centered-Earth-Fixed(ECEF) cartesian system and not properly the best in terms of computational efficiency.As shown by [35], the navigation computation in terms of ECEF is slightly superior inaccuracy, more robust and is about 32% more efficient computationally than the locallevel mechanization, already including computation of the local level navigation solution

from the cartesian mechanization in case it is somehow needed. The higher efficiencyis due the elimination of angular velocity computations inside the propagation used tobring the ECEF solution to local level frames, once gravitational field computations havesimilar efficiency in both cases. Therefore the ECEF approach is more suitable for ap-plications requiring GPS/IMU navigation solution, which preferably use this reference.In case of space applications, a not rotating frame is more desireable then an Earth-fixed one. Therefore, an inertial reference frame like the Earth-Centered-Inertial (ECI)reference frame is used, instead of ECEF, constituting this another contribution of this



21/68



I ss ue: I ss ue 1

Page: 21 of 68

development.

Considering that the only difference between ECI and ECEF is the Earth rotation andthat this rotation is well known, the same advantages mentioned above can be given tothe use of ECI frame. Furthermore, although the navigation equations can be mecha-nized in any reference frame, the natural reference frame for systems which take use ofan IMU is the inertial reference frame. This then adds as a further advantage, once themeasurements provided by the IMU are already related to the inertial space.However, a question may arise about the use of GPS measurements in the mechanization,which are given in ECEF as standard mechanization in the literature. This question canbe answered by remembering the difference in the rates of data delivered by the IMU incomparison to the GPS receiver. While a GPS receiver typically provides data at 10Hz,

the IMU typically provides data at 100Hz, 10 times more. From that, it becomes clearthat the use of ECI to propagate the navigation states is more suitable then the use ofECEF. Furthermore, if the pseudorange approach is used, then the satellite positions canbe easily adapted to provide ECI solution (see [10]) in order to be used directly togetherwith the ECI position provided by the navigation mechanization in ECI. This eliminatesany further concern about some possible disadvantage of using GPS measurements inECI navigation mechanization. This is another contribution of this work.



22/68



I ss ue: I ss ue 1

Page: 22 of 68

5.2. GPS measurements

In absence of clock bias, three measurements are enough to determine the position in athree-dimensional space. However, in the presence of clock bias, 3 measurements onlygive a region which contains the position, without the possibility to determine the spe-cific position. Thus, a fourth measurement is needed to determine the position withinthis region.The complete range description, which takes the clock bias into account is called pseu-dorange.

Figure 2: Pseudorange.

From the signal correlation performed in the electronic, the GPS receiver is capable ofproviding two types of measurements for GPS signals: Pseudorange and Doppler.The raw measurement of the pseudorange and delta-pseudorange due doppler are

P R= (tR tT) c (1)dP R= ( DCO IFL1) c/L1 (2)

To determine the position and speed, a model for the GPS measurements is needed.



23/68



I ss ue: I ss ue 1

Page: 23 of 68

r

i

Sk- r

R

i

rR

i

YECI

r

i

Sk

ZECI

XECI

Figure 3: Satellite ranging.

The measurement models for the pseudorange and delta-pseudorange due doppler are

P R= R+ Rbc T cR= |riS riR|

T=bSV + tr TITT TM(3)

dP R= (viS viR)T (riS riR)

R+ (Vbd+ bd) d(T)

dt c

d(T)dt

=T(tk+1) T(tk)tk+1 tk

(4)

where bd represent the associated noise around the real clock drift speed Vbd such as

Vbd=Vbd+ bd (5)

and the delays are:

tr - Relativistic delay



24/68



I ss ue: I ss ue 1

Page: 24 of 68

TI- Ionospheric delay

TT - Tropospheric delay TM- Multipath effect delay

In the filtering process, the goal is to bring the model results as near as possible to theraw measurements such as: P R P Rand dP R dP R.Note: The satellite clock bias is represented by bSV which is not included in the clockbias term Rbc that represents only the receiver clock bias.

It is possible to note that the pseudorange equation contributes with the same 4 un-

knownsrX,ry,rz and Rbc for each observed satellite while the delta-pseudorange equa-tion contributes with the same 4 unknowns vX, vy, vz and Vbd. Thus, if at least 4satellites are available, the position, clock bias, speed and clock drift can be determined.Please refer to [10] for computation algorithms of the satellite position and speed as wellas the involved delays.



25/68



I ss ue: I ss ue 1

Page: 25 of 68

5.3. Platform ECI Navigation mechanization

The platform measurements are divided in two groups: Accelerometer and Gyro mea-surements.Once the output is available, it is a very imperfect representation of the real desiredactuating specific force and angular rate.The measurements in platform frame can be represented by:

fp

P =fp

P+ bpf+

p

f (6)

pip= pip+ b

p+

p

(7)

where bp and bpfrepresent biases while

p

and pf

are the associated noises around the

real physical entities fp

P and pip.

Once the platform measurements are available, the platform inertial navigation solu-tion can be computed by time integration. The navigation solution is constituted byposition riP, speed v

iP, kinematic acceleration a

iP, attitude q

pi, angular rate iip and an-

gular accelerationiip.By integrating the platform angular rate, the attitude can be obtained. In quaternionsform

qpi =q0

p

i+

t

t0

qpidt (8)

where q0pi is the quaternion form of the platform initial inertial attitude T0p

iobtained by

alignment.Note: The resulting quaternion may not be norm 1 due round-off problems. This canbe solved by a simple renormalization after propagation. The same problem occurs atupdate phase, where renormalization must also be used.

The quaternion derivative can be expressed as function of the angular rate

qpi =

1

2

q(pip) qpi

=1

2q(pip b

p) q

p

i1

2q(p

) qp

i

(9)

where q

is defined as ([7])

q() =

0 z y xz 0 x yy x 0 zx y z 0

(10)



26/68



I ss ue: I ss ue 1

Page: 26 of 68

Note that while the bias can be estimated and used in the integration, the noise can not.

However, by using its standard deviation in the filter covariances propagation process,it can better guide the quaternions estimation. Thus in the equation above, only thefirst term is actually considered in the quaternions propagation.

Fromqpithe platform attitude can be transformed to the rotation matrix form ([7])

T(q) =

q21 q22 q23+ q24 2(q1q2+ q3q4) 2(q1q3 q2q4)2(q1q2 q3q4) q21+ q22 q23+ q24 2(q2q3+ q1q4)

2(q1q3+ q2q4) 2(q2q3 q1q4) q21 q22+ q23+ q24

(11)

The angular rate ispip=

pip bp

iip= (Tp

i)T pip

(12)

Since the platform does not provide angular acceleration measurements, an approxima-tion can be done in order to obtain the angular acceleration:

pip=pip(t + t) pip(t)

tiip= (T

p

i)T

pip

(13)

By integrating the platform kinematic acceleration, the speed can be obtained

viP =v0iP+

tt0

aiP dt (14)

where v0iPis the platform initial inertial speed obtained by initialization.

Then, by integrating the obtained platform inertial speed, the position can be obtained

ri

P =r0i

P+ tt0 v

i

Pdt (15)

where r0iPis the platform initial inertial position also obtained by initialization.

Since the platform is not able to measure the kinematic acceleration, but only spe-cific force, a gravitational acceleration model (see section 5.4) applied at the platformposition must be used in order to compute the gravitational acceleration part containedin the total kinematic acceleration needed in the integration. Furthermore, since the



27/68



I ss ue: I ss ue 1

Page: 27 of 68

specific force is measured in the platform reference frame, it must be converted to ECI

by using the platform ECI attitude. Thus the equation for kinematic acceleration is

aiP =Tp

i(qp

i)T fp

P+ gi

P(riP)

=Tpi(qp

i)T (fp

P bpf pf) + giP(riP)

=Tpi(qp

i)T (fp

P bpf) + giP(riP) Tpi (q

p

i)T p

f

(16)

Also note that while the bias can be estimated and used in the integration, the noisecan not. However, by using its standard deviation in the filter covariances propagationprocess, it can better guide the speed estimation (and indirectly the position by furtherintegration). Thus in the equation above, only the first two terms are actually consid-ered in the speed propagation.

The figure 4 represents the propagation for the platform navigation computation. Itis possible to note that in the kinematic acceleration computation there is a need for apositive fedback on the position, which comes from previous integrations. For that rea-son a stand alone inertial navigation system without external adding can drift mainlyin the vertical position because of the dependency on the gravitational model on theposition. It is here that comes the importance of using the GPS receiver with boundederror to reset the states for next propagation.In the attitude integration there is also a similar effect, which in turn causes the attitude

drift with the time.There is also a further dependency of kinematic acceleration on previous integrationsof attitude, which represent also a source for positioning and speed drift with the time.As shown later in section 7.4.1, the fact of the GPS antenna is installed with a leverarm with respect to IMU, brings the possibility of also updating the attitude using thedependencies of the pseudoranges with respect to the quaternions.



28/68



I ss ue: I ss ue 1

Page: 28 of 68

fP p

+

gP

i

rPi

Gravitational

model

Ti

p

x

xT

Tp

i

fP

i aPi

r0P

iv0P

i

vP

i

Rotation

matrix

ip

p

ip

p

q

i

p

x1/2 x

q0i

p

qi

p.

Quaternion

T0i

p

Figure 4: Platform navigation mechanization.

Once the platform inertial navigation solution is known, the vehicle inertial navigationsolution can be calculated by the installation equations as explained in next sections.



29/68



I ss ue: I ss ue 1

Page: 29 of 68

5.4. Gravitational Model

The gravitational acceleration in function of the position r , due geodetic effects, can beregarded as:

g(r) = Er3 r 3

2J2EA

2E

r5

1 5 rzr

2rx

1 5 rzr

2ry

3 5 rzr

2rz

(17)

Note the model above can be used for both ECEF or ECI. The derivatives of the modelwith respect to the position are

g(ri)r i

=grx

gry

grz

(18)

g

rx=

Er3

+ 3 E r2x

r5 3

2 J2EA2E 1r5 + 152 J2EA2E r

2x

r7

+152 J2EA2E r

2z

r7 105

2 J2EA2E r

2xr

2z

r9

3E ryrxr5 + 152 J2EA2E ryrxr7 1052 J2EA2E r2zryrxr9

3E rzrxr5 + 452 J2EA2E rzrxr7 1052 J2EA2E r3zrxr9

(19)

g

ry=

3E rxryr5 + 152 J2EA2E

rxryr7 1052 J2EA2E

r2z

rxryr9

Er3

+ 3 E r2y

r5 3

2 J2EA2E 1r5 + 152 J2EA2E

r2yr7

+152 J2EA2E r

2z

r7 105

2 J2EA2E r

2yr

2z

r9

3E rzryr5 + 452 J2EA2E rzryr7 1052 J2EA2E r3zryr9

(20)

g

rz =

3E rxrzr5 + 452 J2EA2E rxrzr7 1052 J2EA2E r3zrxr9

3E ryrzr5

+ 452

J2EA

2E

ryrzr7

1052

J2EA

2E

r3zryr9

Er3

+ 3 E r2z

r5 9

2 J2EA2E 1r5 + 45 J2EA2E r

2z

r7

1052 J2EA2E r

4z

r9

(21)



30/68



I ss ue: I ss ue 1

Page: 30 of 68

5.5. Navigation transformation

Although the time integration of the kinematic acceleration and the angular rate gives anavigation solution for the platform, what is of real interest is the navigation solution forthe vehicle, on which the platform is installed. Hence after the platform navigation wasobtained, it must be translated to the vehicle navigation. Furthermore, since the GPSmeasurements are related to the antenna reference frame, there is also the need to relatethem with respect to the platform reference frame in order to enable the navigation filterto update the platform navigation.Hence, the system must be able to change among vehicle, receiver and platform naviga-tion variables. This task is accomplished through the installation relationship equations.Also, as shown later in section 7.4.1, the fact of using a lever arm for GPS antenna withrespect to IMU, brings the possibility of also updating the attitude using the dependen-cies of the pseudoranges with respect to the quaternions.Depending on the design of the user vehicle, the installation position of the platform orreceiver antenna and its angular alignment with respect to the vehicle reference framecan be diverse. In the Figure 5, the installation relationship is sketched for platform andreceiver antenna.

rP

i

Yi

Zi

Xi

Xv

PXp

Yp

i

Z

p

Yv

Zv

LP

v

V

rV

i

rR

i

Yi

Zi

Xi

Xv

RXr

Yr

i

Zr

Yv

Zv

LR

v

V

rV

i

Figure 5: Platform/Antenna installation.

Note that since the navigation is computed with respect to platform, it needs initial

conditions with respect to platform, what is although given with respect to vehicle.Thus, the given vehicle initial conditions can be translated to the platform by using theframe relationship as presented in the following section.



31/68



I ss ue: I ss ue 1

Page: 31 of 68

5.5.1. Platform X Vehicle

Since the platform navigation solution is influenced by vehicle motions, and that theplatform is not necessarily installed on vehicle CG, the following relations must be estab-lished (See Appendix B.1.2). The relationships between platform and vehicle navigationare

Vehicle to Platform:

Tpi =Tp

v Tv

i

iip=iiv

iip=iiv

riP =riV + (T

v

i)T LvP

viP =viV + (T

v

i)T (viv LvP)

aiP =aiV + (T

v

i)T (viv viv LvP+ viv LvP)

(22)

whereV andPare respectively the vehicle and platform reference origins, which coincidewith their respective CGs,LvP is the platform displacement from vehicle CG, also knownas platform lever arm, and Tp

v is the platform installation matrix with respect to the

vehicle reference frame.

It is assumed d(LvP)

dt = 0,

d2(LvP)

dt2 = 0, w

v

pv= 0,

d(wvpv)

dt = 0,

d(Tpv)

dt = 0,

d2(Tpv)

dt2 = 0.

Platform to Vehicle:

Tvp

= (Tpv

)T

Tvi =Tv

p Tp

i

LpV = Tpv LvP

iiv =iip

iiv =iip

riV =riP+ (Tpi )T LpV

viV =viP+ (T

p

i)T (pip LpV)

aiV =aiP+ (T

p

i)T (pip pip LpV + pip LpV)

(23)

It is assumed d(Lp

V)

dt = 0,

d2(LpV)

dt2 = 0, wpvp = 0,

d(wpvp)

dt = 0,

d(Tvp)

dt = 0,

d2(Tvp)

dt2 = 0.



32/68



I ss ue: I ss ue 1

Page: 32 of 68

5.5.2. Receiver Antenna X Vehicle

Since the receiver antenna measurements are influenced by vehicle motions, and thatthe antenna is not necessarily installed on vehicle CG, the following relations mustbe established (See Appendix B.1.2). The relationships between receiver antenna andvehicle navigation are

Vehicle to Receiver Antenna:

Tri =Tr

v Tv

i

iir =iiv

iir =iiv

riR=riV + (T

v

i)T LvR

viR=viV + (T

v

i)T (viv LvR)

aiR=aiV + (T

v

i)T (viv viv LvR+ viv LvR)

(24)

whereV andR are respectively the vehicle and receiver antenna reference origins, whichcoincide with their respective CGs, LvR is the antenna displacement from vehicle CG,also known as antenna lever arm, and Tr

vis the antenna installation matrix with respect

to the vehicle reference frame.

It is assumed d(LvR)

dt = 0,

d2(LvR)

dt2 = 0, w

v

rv= 0, d(w

vrv)

dt = 0,

d(Trv)

dt = 0,

d2(Trv)

dt2 = 0.

Receiver Antenna to Vehicle:

Tvr

= (Trv

)T

Tvi =Tv

r Tr

i

LrV = Trv LvR

iiv =iir

iiv =iir

riV =riR+ (Tri )T LrVviV =v

iR+ (T

r

i)T (rir LrV)

aiV =aiR+ (T

r

i)T (rir rir LrV + rir LrV)

(25)

It is assumed d(LrV)

dt = 0,

d2(LrV)

dt2 = 0, wrvr = 0,

d(wrvr)

dt = 0,

d(Tvr)

dt = 0,

d2(Tvr)

dt2 = 0.



33/68



I ss ue: I ss ue 1

Page: 33 of 68

5.5.3. Platform X Receiver Antenna

Since the receiver antenna does not coincide with platform installation on vehicle, thefollowing relations must be established (See Appendix B.1.2). The relationships betweenplatform and receiver antenna navigation are

Platform to Receiver Antenna:

Trp

=Trv (Tp

v)T

Tri =Tr

p Tp

i

Lp

PR=Tp

v (Lv

R LvP

)

iir =iip

iir =iip

riR=riP+ (T

p

i)T LpPR

viR=viP+ (T

p

i)T (pip LpPR)

aiR=aiP+ (T

p

i)T (pip pip LpPR+ pip LpPR)

(26)

where P and R are respectively the platform and receiver antenna reference origins,which coincide with their respective CGs,LpPRis the antenna displacement from platformorigin, also known as antenna lever arm with respect to platform, andTr

p

is the antenna

installation matrix with respect to the platform reference frame.

It is assumed d(Lp

PR)

dt = 0,

d2(LpPR

)

dt2 = 0, wprp = 0,

d(wprp)

dt = 0,

d(Trp)

dt = 0,

d2(Trp)

dt2 = 0.

Receiver Antenna to Platform:

Tpr=Tp

v (Tr

v)T

Tpi =Tp

r Tr

i

LrRP =Tr

v (LvP LvR)

i

ip= i

ir

iip= iir

riP =riR+ (T

r

i)T LrRP

viP =viR+ (T

r

i)T (rir LrRP)

aiP =aiR+ (T

r

i)T (rir rir LrRP+ rir LrRP)

(27)

It is assumed d(LrRP)

dt = 0,

d2(LrRP)

dt2 = 0, wrpr = 0,

d(wrpr)

dt = 0,

d(Tpr)

dt = 0,

d2(Tpr)

dt2 = 0.



34/68



I ss ue: I ss ue 1

Page: 34 of 68

6. Ground Initialization and Alignment

At the very start of operation, when the user vehicle is yet stopped on the ground, beforethe platform can provide reasonable measurements to be used in navigation computa-tions, the initial conditions for its navigation solution, that means position, speed andattitude, must be defined in order to be used in its integration equations (See Section5.3).The process by which these values are defined and processed is divided in two parts:Initialization and Alignment.Although both process are here treated separated, they are coupled in the sense that thecalculation of initial attitude (alignment) is fully dependent on the position and that the

calculation of initial position and speed (initialization) is also dependent on the attitude.Thus, the initialization and alignment can be seen as iterative processes, which can bestarted using priory given reference states.

Initialization/AlignmentLoop

AlignmentProcessing

PositionInitializationProcessing

VehicleReference State

Calculation

0,

0, h

0

r0V

e, 0 e

v

Lp

v, Tv

p

IMUReference State

Calculation

r0P

e

R0,P

0,Y

0 ap,

ip

p

0 e

p(k) = 0 e

p(k-1)Matrix toEuler 321

angles

Average

Average

Euler 321 anglesto Matrix

0 e

p

ECI to ECEF

Date,UTC

x

0 i

p

0 e

p

r0P

e

x

r0P

i

0 i

e

xT

InitalizeAlign

Pre-Alignment

Processing

r0P

e, 0 e

p

0 e

p

x

ie

iv0P

i

Figure 6: Initialization and Alignment with vehicle priory information.

However, its worth to observe that although the alignment and initialization are per-



35/68



I ss ue: I ss ue 1

Page: 35 of 68

formed for the platform and not for the vehicle, it is the vehicle initial conditions that

are available at the start of operation. Thus they must be translated to the platformbefore the iterative process starts. The translation must also be performed for the GPSantenna, in order to use GPS receiver as aiding in the process. Such transformationsare done by using the platform and antenna installation informations.The process starts by taking the platform computed priori information and doing a firstalignment with data from platform measurements. The alignment is the process, whichprovides attitude information for the platform position calculation. In the loop, theplatform initial position is calculated based on alignment computed attitude and givenvehicle initial position. With the information of the platform position and platformmeasurements, the alignment can calculate again the platform attitude, which will be

used in a new platform position calculation. After defined time period, all calculatedvalues are averaged in order to generate the platform initial position and attitude.With these results and the current time information, the platform initial position andattitude are obtained in ECI and by using the Earth angular rate cross product to theposition, the speed is easily obtained.Note that although the first priory platform attitude is calculated with basis on givenpriory information in the process above, an auto initialization procedure based onlyin the priory information of altitude and longitude and platform measurements is alsopossible. In this case, the platform measurement components are used to calculate thelatitude, roll, pitch and yaw needed in the Initialization/Alignment loop.



36/68



I ss ue: I ss ue 1

Page: 36 of 68

Initialization/AlignmentLoop

AlignmentProcessing

PositionInitializationProcessing

0, h

0L

p

v, Tv

p

IMU AutoReference State

Calculation

r0P

e

a p, ip

p

0 e

p(k) = 0 e

p(k-1)Matrix toEuler 321

angles

Average

Average

Euler 321 anglesto Matrix

0 e

p

ECI to ECEF

Date,UTC

x

0 i

p

0 e

p

r0P

e

x

r0P

i

0 i

e

xT

InitalizeAlign

Pre-AlignmentProcessing

r0P

e, 0 e

p

0 e

p

x

ie

iv0P

i

0

Figure 7: Auto-Initialization and Alignment.

However, if the priory information for altitude and longitude must be given, and thatthe latitude is also generally known, the only advantage is that the roll, pitch and yawangles must not be given. Anyway, since the platform measurements contain errors, theresult of such procedure is worse for must of the cases where low precision platform isused. For High precision platform, the auto-initialization procedure could be possible.

In both processes above, the ECI to ECEF transformation takes place only at the endin order to avoid the time handling during the loop process.Once the platform initial state conditions are obtained in ECEF, the transformation toECI is

r0iP

= (T0e

i)T r0eP (28)

T0p

i=T0

p

e T0e

i(29)



37/68



I ss ue: I ss ue 1

Page: 37 of 68

The transformation matrix from ECI to ECEF is defined as ([7])

T0e

i(0E) =

cos(0E) sin(0E) 0sin(0E) cos(0E) 0

0 0 1

(30)

where 0E is the sideral time angle at initialization which is defined with respect tocurrent Julian Date ([7]).

6.0.4. Alignment

Alignment is the process by which the platform is initial attitude is calculated. Once

the platform is aligned, the vehicle initial attitude is completely defined by using theplatform installation matrix.The method here described is called analytical Coarse alignment ([13]). This methodaligns the platform on the ground computationally by means of using the local gravityand Earth angular rate.Since the gravitational specific force acts together with the centrifuge effect of the Earthangular rate, the resulting specific force is called local apparent gravity and is definedas

geA

(r0eP

) =ge(r0eP

) + (Eeie Ee

ie r0eP), E

e

ie=

0 0 ET

(31)

where r0

e

P is the platform initial position in ECEF and

E is the Earth angular rate.

Considering the platform in absence of other external specific forces, its output will beexactly the measurement of the local apparent gravity and the Earth angular rate in theplatform reference frame. Hence, by using the apparent gravity ge

A, the Earth angular

rate Ee

ie and the cross product between both ge

A Eeie, a equation system can be

constructed, which relates these references to the platform outputs

(ap)T(pip)

T

(ap pip)T

=

(gpA

)T

(Ep

ie)T

(gpA Epie)T

=

(geA

)T T0pe

(Ee

ie)T T0p

e

(geA Eeie)T T0pe

(32)

where T0p

eis the platform initial attitude with respect to ECEF. Note the negative sign

in the accelerometer measurement. That is because the accelerometer is only capable tomeasure the specific force that results from the reaction to the apparent gravity.This method can not be used at or near the Earth poles because of the co-linearity ofboth gravity and Earth angular rate. The co-linearity would make the matrix inversionundefined, once the third line in the matrix would be a linear combination of the firsttwo.



38/68



I ss ue: I ss ue 1

Page: 38 of 68

Solving for T0p

e

T0p

e=

(g

eA

)T

(Ee

ie)T

(geA Eeie)T

1

(ap)T(pip)T

(ap pip)T

(33)

Note that, the alignment process is highly dependent on the platform initial positionr0

eP

because the gravitation specific force model.It is worth to mention that the used values are platform measurements and not the finalresult obtained from the processing of its outputs (such process discounts gravitationalspecific force from the accelerometers).Note: This alignment method is only valid for platform with gyros capable of sensing

Earth rotation rate. In the case of low cost platforms, this method can not be appliedbecause of the abscense of the rotation rate vector needed in the equation shown before.In this case the alignment is highly dependent on given initial conditions. Later whenthe vehicle stars moving, the attitude can be corrrected by the estimation filter by usingGPS measurements and the fact that the platform and antenna are somehow displacedfrom vehicle CG by their Lever arm installation.

6.0.5. Initialization

Initialization is the process by which the initial conditions for platform position riP,speedviPand attitude T0

p

i

are defined.

However, it is the vehicle initial conditions that are available and even in other referenceframe, like ECEF for example (This reference frame is more suitable for Earth positioningand operation start).Then, by means of using the installation information they can be transformed to theplatform conditions in ECEF, and later obtained in ECI with the time information.As mentioned before, there are two ways of initializing the platform: initialization withfull priory vehicle information or auto-initialization.

6.0.5.1. Initialization with full priory vehicle information

Using the priory vehicle information available at the operation start, the initial condi-tions are obtained for the platform.The vehicle initial position in ECEF can be obtained from the geodetic position definedin terms of in geodetic latitude 0G, longitude 0 and altitude h0 with respect to theEarth reference ellipsoid WGS-84 ([15]).

r0eV

=

(RN+ h0)cos0G cos0(RN+ h0)cos0G sin0

(RN(1 e2E) + h0)sin0G

(34)



39/68



I ss ue: I ss ue 1

Page: 39 of 68

where eEis the Earth ellipsoid excentricity and

RN= AE

1 (eE sin0G)2(35)

is the length of the local normal from the ellipsoid surface to its intersection with theECEF Z axis ZECEF. AE is the Earth semimajor axis.

The geodetic latitude is related to the geographic latitude 0 by

0G = atan

A2EB2E

tan(0)

(36)

For the initial attitude, the vehicle is normally assumed to be leveled. However, if the rolland pitch angles are to be considered, the complete transformation from NED (North-East-Down level reference frame) to vehicle reference frame is completely defined by therollR0, pitch P0 and yawY0 angles (c = cos, s= sin)

Tvn

(Y0, P0, R0) =

T321

(Y0, P0, R0) =

cP0 cY0 cP0 sY0 sP0cR0 sY0+ sR0 sP0 cY0 cR0 cY0+ sR0 sP0 sY0 sR0 cP0

sR0 sY0+ cR0 sP0 cY0 sR0 cY0+ cR0 sP0 sY0 cR0 cP0

(37)

The ECEF to NED transformation is function of the longitude and geodetic latitude as

T0n

e(0G, 0) =

sin0G cos0 sin0G sin0 cos0Gsin0 cos0 0cos0G cos0 cos0G sin0 sin0G

(38)

Then the vehicle attitude with respect to ECEF is completely established

T0v

e=T0

v

n T0n

e(39)

Through the installation information the ECEF reference state for platform is

r0eP

=r0eV

+ (T0v

e

)T

LvP (40)

T0p

e=Tp

v T0v

e(41)

Once the Initialization/Alignment process was accomplished, the position and attitudeare transformed to ECI as already shown. To complete the initial conditions, the speedis calculated directly from the ECI position and the Earth angular rate as

v0iP

=iie r0iP, iie= [0 0 E]T (42)were E is the Earth angular rate.



40/68



I ss ue: I ss ue 1

Page: 40 of 68

6.0.5.2. Auto-Initialization

Using fewer priory vehicle information available, that means, only longitude and altitude,the initial conditions can still be obtained by using platform measurements.Remembering that the accelerometers provide measurements for the reaction to theapparent gravitygp

A= apP, platform angles with respect to a local level reference frame

l with same heading can be obtained by using the apparent gravity components as

y =sin1

gA

px

|gpA|

, x = sin1

gA

py

|gpA| cos(y)

, z = 0 (43)

These angles will result in a transformation matrix T0

p

lfrom the local level reference

frame to platform reference frame by using the same expression defined for the roll,pitch and yaw in the last section, but making R = x, P =y, and Y =z.This matrix can be used to bring the platform angular rate measurements to the locallevel reference frame (yaw not aligned to North direction)

lip= (T0p

l)T pip (44)

Then the components of the resulting angular rate can be used to calculate the geodeticlatitude and platform yaw as

lxy =

(wly)2 + (wlx)

2, 0G= tg1

w lzwlxy

, Yp =tg1

wlywlx

(45)

Taking R = x, P= y, and Y = Yp it will result in a transformation matrix T0

p

nfrom

the local North-East-Down reference frame to platform reference frame.By using the obtained geodetic latitude, the transformation matrix T0

n

efrom ECEF to

local North-East-Down reference frame can be obtained as shown in last section.Finally, the platform attitude is

T0p

e=T0

p

n

T0

n

e(46)

Also by using the obtained geodetic latitude and the given altitude and longitude, thevehicle position in ECEF is obtained as already shown and from it the platform positionin ECEF. From here on, the platform ECI position, speed and attitude are obtainedexactly as already shown.



41/68



I ss ue: I ss ue 1

Page: 41 of 68

7. Navigation Filter

Here, the state space form of the navigation mechanization and update equations ispresented, as well as their linearization about the states. This represents the systemknowledge to be applied to the generical estimator.

7.1. State Space

Since the estimator used here requires Gaussian processes, the states to be estimatedcan not be biased, otherwise the estimator can fail.Because of that, besides the variables of the navigation solution themselves, some biases

should be estimated in order to be discounted from the corresponding variables in orderto improve estimator performance.Thus the states with known differential equation and required biases are grouped asstates in only one vector as the states of the estimator in the form:

X=

bp qp

i bpf v

iP Vbd r

iP Rbc

T(47)

where the state initial conditions are represented by X0.

Note however that not all navigation variables take place in the state space vector oncethe differential equations that describe their dynamics are not known. The navigation

states that take no part on the state can still be computed as already shown in thenavigation mechanization section.The Inputs that represent the Platform measurements are grouped in only one vectorin the form:

U=

pip fp

P

T(48)

The Measurements that represent the GPS measurements are grouped in only one vectorin the form:

Z=

P R1 ... P RN dP R1 ... dP RN

T(49)

where Nis the number of tracked satellites.

The state dynamics is represented by the time derivation for each the state groupedin the form:

f(X, U) = X=

bp

qp

ibp

f viP

Vbd riP

Rbc

T(50)

The noisesWassociated to the system process are assumed independent and are definedas Gaussian processes ([2]) with the diagonal covariance matrix Q such as:

W(t) N(0, Q(t)) (51)



42/68



I ss ue: I ss ue 1

Page: 42 of 68

The measurement model is represented in the form:

h(X) =Z=

P R1 ... P RN dP R1 ... dP RN

T(52)

The noisesVassociated to the measurement process are assumed independent. and aredefined as Gaussian processes with the diagonal covariance matrix Rsuch as:

V(t) N(0, R(t)) (53)

7.1.1. State Derivatives with respect to time

Angular rate bias:

bp

w = 0 (54)

Quaternions:

qpi =

1

2

q(pip bp) qpi (55)

Specific force bias:

bpf= 0 (56)

Speed:

viP =Tp

i(qp

i)T (fp

P bpf) + g(riP) (57)

Clock drift Speed:

Vbd= 0 (58)

Position:

riP =viP (59)

Clock bias Range:

Rbc= Vbd (60)



43/68



I ss ue: I ss ue 1

Page: 43 of 68

7.2. State System Matrix

The State system matrix Fis given by the linearization of system dynamics around thecurrent estimation of state X applying the Jacobian to f function with respect to thestates. This procedure yields the linearized system dynamics matrix:

F =f(X, U)

X

X=X

=

(bp

)

bp

(bp

)

qpi

(bp

)

bpf

(bp

)

viP

(bp

)

Vbd

(bp

)

riP

(bp

)

Rbc

(qpi)

bp

(qpi)

qpi

(qpi)

bpf

(qpi)

viP

(qpi)

Vbd

(qpi)

riP

(qpi)

Rbc

(bp

f)

bp

(bp

f)

qp

i

(bp

f)

bp

f

(bp

f)

viP

(bp

f)

Vbd

(bp

f)

riP

(bp

f)

Rbc

(viP)

bp

(viP)

qpi

(viP)

bpf

(viP)

viP

(viP)

Vbd

(viP)

riP

(viP)

Rbc

(Vbd)bp

(Vbd)qpi

(Vbd)bp

f

(Vbd)vi

P

(Vbd)Vbd

(Vbd)ri

P

(Vbd)Rbc

(riP)

bp

(riP)

qpi

(riP)

bpf

(riP)

viP

(riP)

Vbd

(riP)

riP

(riP)

Rbc

(Rbc)bp

(Rbc)qp

i

(Rbc)bp

f

(Rbc)

viP

(Rbc)Vbd

(Rbc)

riP

(Rbc)Rbc

(61)

where the partial derivatives above are matrices containing derivatives of the respectivestate time derivative with respect to each of the states.



44/68



I ss ue: I ss ue 1

Page: 44 of 68

7.2.1. Derivative of Angular rate bias time Derivatives with respect to State

Space

Angular rate bias:

(bp

)

bp= 0

33 (62)

Quaternions:

(bp

)

qpi = 034 (63)


(bp

)

bpf= 0

33 (64)

Speed:

(b

p

)v iP

= 033

(65)

Speed due clock drift:

(bp

)

Vbd= 0

31 (66)

Position:

(b

p

)r iP

= 033

(67)

Range due clock bias:

(bp

)

Rbc= 0

31 (68)



45/68



I ss ue: I ss ue 1

Page: 45 of 68

7.2.2. Derivative of Quaternions time Derivatives with respect to State Space

Angular rate bias:

(qpi)

bp= 1

2

bp

q(bp) qpi

= 1

2A

q(qp

i) (69)

where

Aq

(q) =

q4 q3 q2

q3 q4 q1

q2 q1 q4

q1 q2 q3

(70)

Quaternions:

(qpi)

qpi=

1

2

q(pip bp) (71)


(qpi)

bpf= 0

43 (72)

Speed:

(qpi)

v iP= 0

43 (73)


(qpi)

Vbd= 0

41 (74)

Position:

(qpi)

r iP= 0

43 (75)



46/68



I ss ue: I ss ue 1

Page: 46 of 68


(qpi)

Rbc= 0

41 (76)



47/68



I ss ue: I ss ue 1

Page: 47 of 68

7.2.3. Derivative of Specific force bias time Derivatives with respect to State

Space

Angular rate bias:

(bp

f)

bp= 0

33 (77)

Quaternions:

(bp

f)

qpi= 034 (78)


(bp

f)

bpf= 0

33 (79)

Speed:

(bp

f)v iP

= 033

(80)


(bp

f)

Vbd= 0

31 (81)

Position:

(bpf)

r iP= 0

33 (82)


(bp

f)

Rbc= 0

31 (83)



48/68



I ss ue: I ss ue 1

Page: 48 of 68

7.2.4. Derivative of Speed time Derivatives with respect to State Space

Angular rate bias:

(viP)

bp= 0

33 (84)

Quaternions:

(viP)

qpi=

qpi(Tp

i(qp

i)T (fp

P bpf))

=

Tpi

q1

T (fp

P bpf)

Tp

i

q2

T (fp

P bpf)

Tp

i

q3

T (fp

P bpf)

Tp

i

q4

T (fp

P bpf)

(85)

where

Tpi

q1= 2

q1 q2 q3q2 q1 q4

q3 q4 q1

Tpi

q2= 2

q2 q1 q4q1 q2 q3

q4 q3 q2

Tpi

q3= 2

q3 q4 q1

q4

q3 q2q1 q2 q3

Tp

i

q4= 2

q4 q3 q2

q3 q4 q1

q2 q1 q4 (86)


(viP)

bpf=

bpf((Tp

i)T bpf) = (Tpi )T (87)

Speed:

(viP

)

v iP= 033 (88)


(viP)

Vbd= 0

31 (89)

Position:



49/68



I ss ue: I ss ue 1

Page: 49 of 68

(viP)r iP

= r iP

(g(riP)) =g(riP)ri

Px

g(riP)ri

Py

g(riP)ri

Pz

(90)


(viP)

Rbc= 0

31 (91)



50/68



I ss ue: I ss ue 1

Page: 50 of 68

7.2.5. Derivative of Clock drift speed time Derivatives with respect to State

Space

Angular rate bias:

(Vbd)

bp= 0

13 (92)

Quaternions:

(Vbd)

qpi = 014 (93)


(Vbd)

bpf= 0

13 (94)

Speed:

(Vbd)

v iP= 013 (95)


(Vbd)

Vbd= 0 (96)

Position:

(Vbd)

r iP= 013 (97)


(Vbd)

Rbc= 0 (98)



51/68



I ss ue: I ss ue 1

Page: 51 of 68

7.2.6. Derivative of Position time Derivatives with respect to State Space

Angular rate bias:

(riP)

bp= 0

33 (99)

Quaternions:

(riP)

qpi= 0

34 (100)


(riP)

bpf= 0

33 (101)

Speed:

(riP)

v

i

P

=I33

(102)


(riP)

Vbd= 0

31 (103)

Position:

(riP)

r iP

= 033

(104)


(riP)

Rbc= 0

31 (105)



52/68



I ss ue: I ss ue 1

Page: 52 of 68

7.2.7. Derivative of Clock bias range time Derivatives with respect to State

Space

Angular rate bias:

(Rbc)

bp= 0

13 (106)

Quaternions:

(Rbc)

qpi = 014 (107)


(Rbc)

bpf= 0

13 (108)

Speed:

(Rbc)

v iP= 013 (109)


(Rbc)

Vbd= 1 (110)

Position:

(Rbc)

r iP= 013 (111)


(Rbc)

Rbc= 0 (112)



53/68



I ss ue: I ss ue 1

Page: 53 of 68

7.3. System Noise Covariance Matrix

The System noise covariance matrix Q is given by the noise standard deviation acting

on the state space system dynamics and as function of the state Xsuch as

Q=

Qb

0 0 0 0 0 0

0 Qq

0 0 0 0 0

0 0 Qbf

0 0 0 0

0 0 0 Qv

0 0 0

0 0 0 0 QV bd

0 0

0 0 0 0 0 Qr 00 0 0 0 0 0 Q

Rbc

(113)

where the matrices above are diagonal matrices containing the respective system noisestandard deviations for each state. DenotingD(v) is the diagonal matrix resulting fromthe elements ofv the diagonal matrices are as following:

Qb

= 033

(114)

Q

q

=D(

1

2q(

)

qpi) (115)

where

is determined by averaging sensor output.

Qbf

= 033

(116)

Qv

=D(Tpi

(qpi)T

f) (117)

where f

is determined by averaging sensor output.

QV bd

= 0 (118)

Qr

= 033

(119)

QRbc

=bd (120)

where bd is determined by averaging clock drift speed estimation at receiver initializa-tion.



54/68



I ss ue: I ss ue 1

Page: 54 of 68

7.4. Measurement coupling Matrix

The Measurement coupling matrix H is given by the linearization of measurement dy-namics around the current estimation of state X applying the Jacobian to h functionwith respect to the states. This procedure yields the linearized observation matrix:

H=h(X, U)

X

X=X

=

( PRk)bp

( PRk)qp

i

( PRk)bp

f

( PRk)

viP

( PRk)Vbd

( PRk)

riP

( PRk)Rbc

...

( dPRk)bp

( dPRk)qp

i

( dPRk)bp

f

( dPRk)

viP

( dPRk)Vbd

( dPRk)

riP

( dPRk)Rbc

...

(121)where the partial derivatives above are matrices containing derivatives of the respectivemeasurement prediction with respect to each of the states that must be evaluated foreach allocced satellite k.



55/68



I ss ue: I ss ue 1

Page: 55 of 68

7.4.1. Derivative of Pseudorange with respect to State Space

Angular rate bias:

( P R)

bp= 0

13 (122)

Quaternions:

( P R)

qpi=

PRq1

PRq2

PRq3

PRq4 (123)

where( P R)

qi= (r

iS riR)T

R r

iR

qi,

r iRqi

=

Tp

i

qi

T LpPR (124)


( P R)

bpf= 0

13 (125)

Speed:

( P R)

v iP= 0

13 (126)


( P R)

Vbd= 0 (127)

Position:

( P R)

r iP= (r

iS riR)T

R (128)


( P R)

Rbc= 1 (129)



56/68



I ss ue: I ss ue 1

Page: 56 of 68

7.4.2. Derivative of Delta-Pseudorange with respect to State Space

Angular rate bias:

( dP R)

bp= 1

R

((Tp

i)T(LpPR))

T (riS riR)T

(130)

Quaternions:

( dP R)

qpi=

dPRq1

dPRq2

dPRq3

dPRq4

(131)

where

( dP R)

qi= (r

iS riR)T

R v

iR

qi(v

iS viR)T

R r

iR

qi

+ 2 (viS viR)T (riS riR)

R2

(riS riR)T

riRqi

v iRqi

=

Tp

i

qi

T ((pip bp) LpPR)

(132)


( dP R)

bpf= 0

13 (133)

Speed:

( dP R)

v iP= (r

iS riR)T

R (134)


( dP R)

Vbd= 1 (135)

Position:

( dP R)

r iP=

(v

iS viR)

R + 2 (v

iS viR)T (riS riR)

R2 (riS riR)

T(136)



57/68



I ss ue: I ss ue 1

Page: 57 of 68


( dP R)

Rbc= 0 (137)



58/68



I

Documents

Gps Imu Ekf Navigation