Coupled GPS/MEMS IMU Attitude Determination of Small UAVs ...
Gps Imu Ekf Navigation
-
Upload
pham-ngoc-hoa -
Category
Documents
-
view
244 -
download
1
Transcript of 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
GPS-IMU-EKF Navigation
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
-
8/11/2019 Gps Imu Ekf Navigation
2/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 2 of 68
-
8/11/2019 Gps Imu Ekf Navigation
3/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 3 of 68
-
8/11/2019 Gps Imu Ekf Navigation
4/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 4 of 68
-
8/11/2019 Gps Imu Ekf Navigation
5/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 5 of 68
-
8/11/2019 Gps Imu Ekf Navigation
6/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 6 of 68
-
8/11/2019 Gps Imu Ekf Navigation
7/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 7 of 68
-
8/11/2019 Gps Imu Ekf Navigation
8/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 8 of 68
-
8/11/2019 Gps Imu Ekf Navigation
9/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 9 of 68
-
8/11/2019 Gps Imu Ekf Navigation
10/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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]).
National Institute of Space Research - INPE Page 10 of 68
-
8/11/2019 Gps Imu Ekf Navigation
11/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
National Institute of Space Research - INPE Page 11 of 68
-
8/11/2019 Gps Imu Ekf Navigation
12/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 12 of 68
-
8/11/2019 Gps Imu Ekf Navigation
13/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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:
National Institute of Space Research - INPE Page 13 of 68
-
8/11/2019 Gps Imu Ekf Navigation
14/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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:
National Institute of Space Research - INPE Page 14 of 68
-
8/11/2019 Gps Imu Ekf Navigation
15/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
National Institute of Space Research - INPE Page 15 of 68
-
8/11/2019 Gps Imu Ekf Navigation
16/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 16 of 68
-
8/11/2019 Gps Imu Ekf Navigation
17/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 17 of 68
-
8/11/2019 Gps Imu Ekf Navigation
18/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 18 of 68
-
8/11/2019 Gps Imu Ekf Navigation
19/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 19 of 68
-
8/11/2019 Gps Imu Ekf Navigation
20/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 20 of 68
-
8/11/2019 Gps Imu Ekf Navigation
21/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 21 of 68
-
8/11/2019 Gps Imu Ekf Navigation
22/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 22 of 68
-
8/11/2019 Gps Imu Ekf Navigation
23/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 23 of 68
-
8/11/2019 Gps Imu Ekf Navigation
24/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 24 of 68
-
8/11/2019 Gps Imu Ekf Navigation
25/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
National Institute of Space Research - INPE Page 25 of 68
-
8/11/2019 Gps Imu Ekf Navigation
26/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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
National Institute of Space Research - INPE Page 26 of 68
-
8/11/2019 Gps Imu Ekf Navigation
27/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 27 of 68
-
8/11/2019 Gps Imu Ekf Navigation
28/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 28 of 68
-
8/11/2019 Gps Imu Ekf Navigation
29/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
National Institute of Space Research - INPE Page 29 of 68
-
8/11/2019 Gps Imu Ekf Navigation
30/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 30 of 68
-
8/11/2019 Gps Imu Ekf Navigation
31/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 31 of 68
-
8/11/2019 Gps Imu Ekf Navigation
32/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 32 of 68
-
8/11/2019 Gps Imu Ekf Navigation
33/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 33 of 68
-
8/11/2019 Gps Imu Ekf Navigation
34/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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-
National Institute of Space Research - INPE Page 34 of 68
-
8/11/2019 Gps Imu Ekf Navigation
35/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 35 of 68
-
8/11/2019 Gps Imu Ekf Navigation
36/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
National Institute of Space Research - INPE Page 36 of 68
-
8/11/2019 Gps Imu Ekf Navigation
37/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 37 of 68
-
8/11/2019 Gps Imu Ekf Navigation
38/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
National Institute of Space Research - INPE Page 38 of 68
-
8/11/2019 Gps Imu Ekf Navigation
39/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 39 of 68
-
8/11/2019 Gps Imu Ekf Navigation
40/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 40 of 68
-
8/11/2019 Gps Imu Ekf Navigation
41/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
National Institute of Space Research - INPE Page 41 of 68
-
8/11/2019 Gps Imu Ekf Navigation
42/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
National Institute of Space Research - INPE Page 42 of 68
-
8/11/2019 Gps Imu Ekf Navigation
43/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 43 of 68
-
8/11/2019 Gps Imu Ekf Navigation
44/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
(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)
National Institute of Space Research - INPE Page 44 of 68
-
8/11/2019 Gps Imu Ekf Navigation
45/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
(qpi)
bpf= 0
43 (72)
Speed:
(qpi)
v iP= 0
43 (73)
Speed due clock drift:
(qpi)
Vbd= 0
41 (74)
Position:
(qpi)
r iP= 0
43 (75)
National Institute of Space Research - INPE Page 45 of 68
-
8/11/2019 Gps Imu Ekf Navigation
46/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
I ss ue: I ss ue 1
Page: 46 of 68
Range due clock bias:
(qpi)
Rbc= 0
41 (76)
National Institute of Space Research - INPE Page 46 of 68
-
8/11/2019 Gps Imu Ekf Navigation
47/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
(bp
f)
bpf= 0
33 (79)
Speed:
(bp
f)v iP
= 033
(80)
Speed due clock drift:
(bp
f)
Vbd= 0
31 (81)
Position:
(bpf)
r iP= 0
33 (82)
Range due clock bias:
(bp
f)
Rbc= 0
31 (83)
National Institute of Space Research - INPE Page 47 of 68
-
8/11/2019 Gps Imu Ekf Navigation
48/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
(viP)
bpf=
bpf((Tp
i)T bpf) = (Tpi )T (87)
Speed:
(viP
)
v iP= 033 (88)
Speed due clock drift:
(viP)
Vbd= 0
31 (89)
Position:
National Institute of Space Research - INPE Page 48 of 68
-
8/11/2019 Gps Imu Ekf Navigation
49/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Range due clock bias:
(viP)
Rbc= 0
31 (91)
National Institute of Space Research - INPE Page 49 of 68
-
8/11/2019 Gps Imu Ekf Navigation
50/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
(Vbd)
bpf= 0
13 (94)
Speed:
(Vbd)
v iP= 013 (95)
Speed due clock drift:
(Vbd)
Vbd= 0 (96)
Position:
(Vbd)
r iP= 013 (97)
Range due clock bias:
(Vbd)
Rbc= 0 (98)
National Institute of Space Research - INPE Page 50 of 68
-
8/11/2019 Gps Imu Ekf Navigation
51/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
(riP)
bpf= 0
33 (101)
Speed:
(riP)
v
i
P
=I33
(102)
Speed due clock drift:
(riP)
Vbd= 0
31 (103)
Position:
(riP)
r iP
= 033
(104)
Range due clock bias:
(riP)
Rbc= 0
31 (105)
National Institute of Space Research - INPE Page 51 of 68
-
8/11/2019 Gps Imu Ekf Navigation
52/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
(Rbc)
bpf= 0
13 (108)
Speed:
(Rbc)
v iP= 013 (109)
Speed due clock drift:
(Rbc)
Vbd= 1 (110)
Position:
(Rbc)
r iP= 013 (111)
Range due clock bias:
(Rbc)
Rbc= 0 (112)
National Institute of Space Research - INPE Page 52 of 68
-
8/11/2019 Gps Imu Ekf Navigation
53/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 53 of 68
-
8/11/2019 Gps Imu Ekf Navigation
54/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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.
National Institute of Space Research - INPE Page 54 of 68
-
8/11/2019 Gps Imu Ekf Navigation
55/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
( P R)
bpf= 0
13 (125)
Speed:
( P R)
v iP= 0
13 (126)
Speed due clock drift:
( P R)
Vbd= 0 (127)
Position:
( P R)
r iP= (r
iS riR)T
R (128)
Range due clock bias:
( P R)
Rbc= 1 (129)
National Institute of Space Research - INPE Page 55 of 68
-
8/11/2019 Gps Imu Ekf Navigation
56/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
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)
Specific force bias:
( dP R)
bpf= 0
13 (133)
Speed:
( dP R)
v iP= (r
iS riR)T
R (134)
Speed due clock drift:
( 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)
National Institute of Space Research - INPE Page 56 of 68
-
8/11/2019 Gps Imu Ekf Navigation
57/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
I ss ue: I ss ue 1
Page: 57 of 68
Range due clock bias:
( dP R)
Rbc= 0 (137)
National Institute of Space Research - INPE Page 57 of 68
-
8/11/2019 Gps Imu Ekf Navigation
58/68
GPS-IMU-EKF Navigation
Doc.No.: GPSIMU-INPE-002
I