**Fundamentals of GNSS-Aided Inertial Navigation**

Ahmed Mohamed and Apostolos Mamatas *University of Florida USA*

## **1. Introduction**

GNSS-aided inertial navigation is a core technology in aerospace applications from military to civilian. It is the product of a confluence of disciplines, from those in engineering to the geodetic sciences and it requires a familiarity with numerous concepts within each field in order for its application to be understood and used effectively. Aided inertial navigation systems require the use of kinematic, dynamic and stochastic modeling, combined with optimal estimation techniques to ascertain a vehicle's navigation state (position, velocity and attitude). Moreover, these models are employed within different frames of reference, depending on the application. The goal of this chapter is to familiarize the reader with the relevant fundamental concepts.

## **2. Background**

## **2.1 Modeling motion**

The goal of a navigation system is to determine the state of the vehicle's trajectory in space relevant to guidance and control. These are namely its position, velocity and attitude at any time. In inertial navigation, a vehicle's path is modeled kinematically rather than dynamically, as the full relationship of forces acting on the body to its motion is quite complex. The kinematic model incorporates accelerations and turn rates from an inertial measurement unit (IMU) and accounts for effects on the measurements of the reference frame in which the model is formalized. The kinematic model relies solely on measurements and known physical properties of the reference frame, without regard to vehicle dynamic characteristics. On the other hand, in incorporating aiding systems like GNSS, a dynamic model is used to predict error states in the navigation parameters which are rendered observable through the external measurements of position and velocity. The dynamics model is therefore one in which the errors are related to the current navigation state. As will be shown, some errors are bounded while others are not. At this point, we make the distinction between the aided INS and free-navigating INS. Navigation using the latter method represents a form of "dead reckoning", that is the navigation parameters are derived through the integration of measurements from some defined initial state. For instance, given a measured linear acceleration, integration of the measurement leads to velocity and double integration results in the vehicle's position. Inertial sensors exhibit biases and noise that, when integrated, leads to computed positional drift over time. The goal of the aiding system is therefore to help estimate the errors and correct them.

equator

Fig. 2. Inertial Frame

Fig. 3. Earth-Fixed Frame

East completes the orthogonal system.

spin axis

Fundamentals of GNSS-Aided Inertial Navigation 5

*zi*

ecliptic

mean spin axis

*ze*

between native INS output and transformed values in another frame.

*xe*

• Local-Level Frame (*l*-frame). This frame is defined by a plane locally tangent to the surface of the earth at the position of the vehicle. This implies a constant direction for gravity (straight down). The coordinate system used is *easting, northing, up* (enu), where Up is the normal vector of the plane, North points toward the spin axis of Earth on the plane and

Finally, we remark that the implementation of the INS can be freely chosen to be formulated in any of the last three frames, and it is common to refer to the navigation frame (*n*-frame) once it is defined as being either the *i*-, *e*- or *l*-frames, especially when one must make the distinction

*ye*

Greenwich meridian

*xi*

*yi*

vernal equinox

## **2.2 Reference frames**

Proceeding from the sensor stratum up to more intuitively accessible reference systems, we define the following reference frames:

• Sensor Frame (*s*-frame). This is the reference system in which the inertial sensors operate. It is a frame of reference with a right-handed Cartesian coordinate system whose origin is at the center of the instrument cluster, with arbitrarily assigned principle axes as shown in figure 1.

Fig. 1. IMU measurements in the *s*-frame


2 Will-be-set-by-IN-TECH

Proceeding from the sensor stratum up to more intuitively accessible reference systems, we

• Sensor Frame (*s*-frame). This is the reference system in which the inertial sensors operate. It is a frame of reference with a right-handed Cartesian coordinate system whose origin is at the center of the instrument cluster, with arbitrarily assigned principle axes as shown in

*z*

*ωz*

**f***z*

• Body Frame (*b*-frame). This is the reference system of the vehicle whose motions are of interest. The *b*-frame is related to the *s*-frame through a rigid transformation (rotation and translation). This accounts for misalignment between the sensitive axes of the IMU and the primary axes of the vehicle which define roll, pitch and yaw. Two primary axis definitions are generally employed: one with +*y* pointing toward the front of the vehicle (+*z* pointing up), and the other with +*x* pointing toward the nose (+*z* pointing down). The latter is a common aerospace convention used to define heading as a clockwise rotation in

• Inertial Frame (*i*-frame). This is the canonical inertial frame for an object near the surface of the earth. It is a non-rotating, non-accelerating frame of reference with a Cartesian coordinate system whose *x* axis is aligned with the mean vernal equinox and whose *z* axis is coaxial with the spin axis of the earth. The y-axis completes the orthogonal basis

• Earth-Fixed Frame (*e*-frame). With some subtle differences that we shall overlook, this system's *z* axis is defined the same way as for the *i*-frame, but the *x* axis now points toward the mean Greenwich meridian, with *y* completing the right-handed system. The origin is at the earth's center of mass. This frame rotates with respect to the *i*-frame at the earth's

and the system's origin is located at the center of mass of the earth.

rotation rate of approximately 15 degrees per hour.

**f***x*

**f***y*

Fig. 1. IMU measurements in the *s*-frame

a right-handed system (Rogers, 2003).

*x*

*ωx*

*ωy*

*y*

**2.2 Reference frames**

figure 1.

define the following reference frames:

Fig. 3. Earth-Fixed Frame

• Local-Level Frame (*l*-frame). This frame is defined by a plane locally tangent to the surface of the earth at the position of the vehicle. This implies a constant direction for gravity (straight down). The coordinate system used is *easting, northing, up* (enu), where Up is the normal vector of the plane, North points toward the spin axis of Earth on the plane and East completes the orthogonal system.

Finally, we remark that the implementation of the INS can be freely chosen to be formulated in any of the last three frames, and it is common to refer to the navigation frame (*n*-frame) once it is defined as being either the *i*-, *e*- or *l*-frames, especially when one must make the distinction between native INS output and transformed values in another frame.

*x*, *y* plane

*x*, *y* plane

(<sup>1</sup> <sup>−</sup> *<sup>e</sup>*<sup>2</sup> sin<sup>2</sup> *<sup>φ</sup>*)3/2 (3)

*δn* = (*Re* + *h*)*δψ* (4)

(2)

*z*

*N*

*b*

*N*

**r**

*ψ φ*

*<sup>N</sup>* <sup>=</sup> *<sup>a</sup>* 

(<sup>1</sup> <sup>−</sup> *<sup>e</sup>*<sup>2</sup> sin<sup>2</sup> *<sup>φ</sup>*)

Another radius is defined, namely the radius of curvature in the meridian, *M*, which is given

The two parameters *N* and *M* are necessary for calculating the linear distances and velocity components from the geodetic coordinate system in the local-level frame. In order to relate geodetic position changes and linear distances, we begin with the simple case of a sphere of radius *Re*. Note that the linear distance between two points along a meridian (in the North

*<sup>M</sup>* <sup>=</sup> *<sup>a</sup>*(<sup>1</sup> <sup>−</sup> *<sup>e</sup>*2)

*z*

Fig. 5. Reference Ellipsoid

Fig. 6. Geodetic vs. Geocentric Latitude

value of *N* is obtained by

as

direction) is

*h*

*a*

*h*

P

*φ*

P

Fundamentals of GNSS-Aided Inertial Navigation 7

Fig. 4. Local-Level Frame

## **2.3 Geometric figure of the earth**

Having defined the common reference frames, we must consider the size and shape of the earth itself, an especially important topic when moving between the *l*- and *e*- frames or when converting Cartesian to geodetic (latitude, longitude, height) coordinates. The earth, though commonly imagined as a sphere, is in fact more accurately described as an ellipse revolved around its semi-major axis, an *ellipsoid*. Reference ellipsoids are generally defined by the magnitude of their semi-major axis (equatorial radius) and their flattening, which is the ratio of the polar radius to the equatorial radius. Since the discovery of the elipticity of the earth, many ellipsoids have been formulated, but today the most important one for global navigation is the WGS84 ellipsoid1, which forms the basis of the WGS84 system to which all GPS measurements and computations are tied (Hofmann-Wellenhof et al., 2001). The WGS84 ellipsoid is defined as having an equatorial radius of 6,378,137 m and a flattening of 1/298.257223563 centered at the earth's center of mass with 0 degrees longitude located 5.31 arc seconds east of the Greenwich meridian (NIMA, 2000; Rogers, 2003). It is worth defining another ellipsoidal parameter, the eccentricity e, as the distance of the ellipse focus from the axes center, and is calculated as

$$
\varepsilon^2 = \frac{a^2 - b^2}{a^2} \tag{1}
$$

Figure 5 shows a cross-sectional view of the reference ellipsoid with having semi-major and semi-minor dimensions *a* and *b*, respectively. Note that *b* is derivable from *a* and *f* . A point *P* is located at height *h* normal to the surface. *N* is the radius of curvature in the prime vertical of the ellipsoid at this point2. The angle between the *x*, *y* plane and the surface normal vector of *P* is the geodetic latitude *φ*. Note that the loci of normal vectors that pass through the centroid of the ellipsoid are constrained to the equator and the meridians. This means that, in general, the geodetic latitude *φ* is not the same as the *geocentric* latitude *ψ*, as shown in figure 6. The

<sup>1</sup> A variant of the GRS80 ellipsoid

<sup>2</sup> This is also called the normal radius of curvature, hence the symbol *N*.

Fig. 5. Reference Ellipsoid

4 Will-be-set-by-IN-TECH

*n*

Having defined the common reference frames, we must consider the size and shape of the earth itself, an especially important topic when moving between the *l*- and *e*- frames or when converting Cartesian to geodetic (latitude, longitude, height) coordinates. The earth, though commonly imagined as a sphere, is in fact more accurately described as an ellipse revolved around its semi-major axis, an *ellipsoid*. Reference ellipsoids are generally defined by the magnitude of their semi-major axis (equatorial radius) and their flattening, which is the ratio of the polar radius to the equatorial radius. Since the discovery of the elipticity of the earth, many ellipsoids have been formulated, but today the most important one for global navigation is the WGS84 ellipsoid1, which forms the basis of the WGS84 system to which all GPS measurements and computations are tied (Hofmann-Wellenhof et al., 2001). The WGS84 ellipsoid is defined as having an equatorial radius of 6,378,137 m and a flattening of 1/298.257223563 centered at the earth's center of mass with 0 degrees longitude located 5.31 arc seconds east of the Greenwich meridian (NIMA, 2000; Rogers, 2003). It is worth defining another ellipsoidal parameter, the eccentricity e, as the distance of the ellipse focus from the

*e*

<sup>2</sup> This is also called the normal radius of curvature, hence the symbol *N*.

<sup>2</sup> <sup>=</sup> *<sup>a</sup>*<sup>2</sup> <sup>−</sup> *<sup>b</sup>*<sup>2</sup>

Figure 5 shows a cross-sectional view of the reference ellipsoid with having semi-major and semi-minor dimensions *a* and *b*, respectively. Note that *b* is derivable from *a* and *f* . A point *P* is located at height *h* normal to the surface. *N* is the radius of curvature in the prime vertical of the ellipsoid at this point2. The angle between the *x*, *y* plane and the surface normal vector of *P* is the geodetic latitude *φ*. Note that the loci of normal vectors that pass through the centroid of the ellipsoid are constrained to the equator and the meridians. This means that, in general, the geodetic latitude *φ* is not the same as the *geocentric* latitude *ψ*, as shown in figure 6. The

*<sup>a</sup>*<sup>2</sup> (1)

Fig. 4. Local-Level Frame

**2.3 Geometric figure of the earth**

axes center, and is calculated as

<sup>1</sup> A variant of the GRS80 ellipsoid

*u*

*e*

mean spin axis

Fig. 6. Geodetic vs. Geocentric Latitude

value of *N* is obtained by

$$N = \frac{a}{\sqrt{(1 - e^2 \sin^2 \phi)}}\tag{2}$$

Another radius is defined, namely the radius of curvature in the meridian, *M*, which is given as

$$M = \frac{a(1 - e^2)}{(1 - e^2 \sin^2 \phi)^{3/2}} \tag{3}$$

The two parameters *N* and *M* are necessary for calculating the linear distances and velocity components from the geodetic coordinate system in the local-level frame. In order to relate geodetic position changes and linear distances, we begin with the simple case of a sphere of radius *Re*. Note that the linear distance between two points along a meridian (in the North direction) is

$$
\delta\mathfrak{n} = (\mathbb{R}\_{\mathfrak{e}} + h)\delta\mathfrak{y} \tag{4}
$$

**2.5 Normal gravity**

where

Uneven mass distributions within the earth, as well as departure of its actual shape from a perfect ellipsoid leads to a highly complex gravity field. It is therefore convenient for navigation purposes to approximate the gravity field using the so-called *normal gravity model*, computed in closed form using Somigliana's formula (Schwarz & Wei, 1990; Torge, 2001) :

Fundamentals of GNSS-Aided Inertial Navigation 9

*γa*, *γ<sup>b</sup>* = equatorial and polar gravity values

A computationally faster method to calculate gravity involves expanding (12) by power series

where *γ<sup>a</sup>* is the gravity at the equator, *β* is the "gravity flattening" term (Hofmann-Wellenhof

*<sup>β</sup>* <sup>=</sup> *<sup>γ</sup><sup>b</sup>* <sup>−</sup> *<sup>γ</sup><sup>a</sup> γa*

<sup>4</sup> <sup>−</sup> <sup>5</sup> *<sup>f</sup> <sup>ω</sup>*<sup>2</sup>

where *ω<sup>e</sup>* is the earth rotation constant. This approximation of (12) is accurate to approximately 0.1 *μ*g (Featherstone & Dentith, 1997), which is sufficient for navigation purposes. Higher-accuracy approximations are given in (Hofmann-Wellenhof & Moritz, 2005; Torge, 2001).Table 1 gives the relevant WGS84 parameters for computing normal gravity. The first four are the defining parameters of the system, while the last two are derived for use in computing normal gravity. Incorporating height *h* allows a more general formula for gravity

> *a* 6,378,137 m *f* 1/298.257223563 *<sup>ω</sup><sup>e</sup>* 7.292115<sup>×</sup> <sup>10</sup>−<sup>5</sup> rad/s GM 3.986004.418×1014m3/s2 *γ<sup>a</sup>* 9.7803253359 m/s<sup>2</sup> *γ<sup>b</sup>* 9.8321849378 m/s<sup>2</sup>

*e a* 8*γ<sup>a</sup>*

*<sup>γ</sup>* <sup>=</sup> *<sup>γ</sup>*<sup>0</sup> <sup>−</sup> (3.0877 <sup>×</sup> <sup>10</sup>−<sup>6</sup> <sup>−</sup> 4.4 <sup>×</sup> <sup>10</sup>−<sup>9</sup> sin2 *<sup>φ</sup>*)*<sup>h</sup>* <sup>+</sup> 0.72 <sup>×</sup> <sup>10</sup>−12*h*<sup>2</sup> (16)

*<sup>β</sup>*<sup>1</sup> <sup>=</sup> *<sup>f</sup>* <sup>2</sup>

1 + *k* sin<sup>2</sup> *φ*

<sup>1</sup> <sup>−</sup> *<sup>e</sup>*<sup>2</sup> sin2 *<sup>φ</sup>*

*γ*<sup>0</sup> = *γa*(1 + *β* sin<sup>2</sup> *φ* + *β*1*sin*22*φ*) (13)

(12)

(14)

(15)

*γ*<sup>0</sup> = *γ<sup>a</sup>*

*<sup>k</sup>* <sup>=</sup> *<sup>b</sup>γ<sup>b</sup> aγa* − 1

with respect to *e*<sup>2</sup> and truncating, yielding:

The second parameter in (13) *β*<sup>1</sup> is given by

& Moritz, 2005), defined as

Table 1. WGS84 parameters

away from the ellipsoidal surface:

and the distance along a parallel (in the East direction) is

$$
\delta e = (R\_\ell + h) \cos \psi \delta \lambda \tag{5}
$$

where *h* is the height above the sphere. In the case of the ellipsoidal earth, one radius does not suffice to reduce both directions of motion to linear distances and the equations become

$$
\delta n = (M + h)\delta \phi \tag{6}
$$

$$
\delta e = (N + h) \cos \phi \delta \lambda \tag{7}
$$

#### **2.4 Gravitation and gravity**

Inertial navigation relies on measurements made in an inertial reference frame, *i.e.* one free of acceleration or rotation. Vehicles near the earth's surface, of course, are subjected to both of these factors. As an accelerometer is not capable of distinguishing accelerations due to motion and accelerations arising from reaction forces in a gravity field, we must have *a priori* knowledge of the earth's gravitation in order to subtract its effects from sensor measurements. The gravitational field of the earth is described by its potential *V* at a point **P** such that

$$\mathbf{V}(\mathbf{P}) = G \iiint\limits\_{earth} \frac{\rho(\mathbf{Q})}{l} dv\_{\mathbf{Q}} \tag{8}$$

where **Q** is a point within the earth with mass density *ρ*(**Q**) and volume element *dv***Q**, located at a distance *l* from **P** and *G* is the gravitational constant (Hofmann-Wellenhof & Moritz, 2005). The gravitational vector field is defined as the gradient of the potential:

$$\bar{\mathbf{g}}^{\varepsilon} = \nabla \mathbf{V}^{\varepsilon} = \frac{\partial \mathbf{V}^{\varepsilon}}{\partial \mathbf{r}^{\varepsilon}} = \begin{pmatrix} V\_{\chi} \\ V\_{y} \\ V\_{z} \end{pmatrix} \tag{9}$$

where **¯g<sup>e</sup>** is the gravitational vector associated with the position vector **re**. In practice, sensor measurements are further conditioned by the rotation of the earth. The difference of gravitation and centripetal acceleration caused by Earth's rotation is embodied in the *gravity vector* and is more commonly used in practice. It is defined as

$$\mathbf{g}^{\ell} = \mathbf{\bar{g}}^{\ell} - \boldsymbol{\Omega}\_{\ell} \boldsymbol{\Omega}\_{\ell} \mathbf{r}^{\ell} \tag{10}$$

where Ω*ie* is the skew-symmetric representation of the earth's rotation rate (the skew-symmetric matrix representation is treated in section 2.6). In the local-level frame, the gravity vector is expressed as

$$\mathbf{g}^l = \begin{pmatrix} 0 \\ 0 \\ -(\gamma + \sigma \mathbf{g}\_u) \end{pmatrix} \tag{11}$$

where *γ* is normal gravity and *σgu* is a disturbance in the vertical component of the vector (Hofmann-Wellenhof & Moritz, 2005).3

<sup>3</sup> We leave out the non-vertical deflections here for brevity, but a more detailed treatment should include them.

#### **2.5 Normal gravity**

Uneven mass distributions within the earth, as well as departure of its actual shape from a perfect ellipsoid leads to a highly complex gravity field. It is therefore convenient for navigation purposes to approximate the gravity field using the so-called *normal gravity model*, computed in closed form using Somigliana's formula (Schwarz & Wei, 1990; Torge, 2001) :

$$\gamma\_0 = \gamma\_a \frac{1 + k \sin^2 \phi}{\sqrt{1 - e^2 \sin^2 \phi}} \tag{12}$$

where

6 Will-be-set-by-IN-TECH

where *h* is the height above the sphere. In the case of the ellipsoidal earth, one radius does not suffice to reduce both directions of motion to linear distances and the equations become

Inertial navigation relies on measurements made in an inertial reference frame, *i.e.* one free of acceleration or rotation. Vehicles near the earth's surface, of course, are subjected to both of these factors. As an accelerometer is not capable of distinguishing accelerations due to motion and accelerations arising from reaction forces in a gravity field, we must have *a priori* knowledge of the earth's gravitation in order to subtract its effects from sensor measurements. The gravitational field of the earth is described by its potential *V* at a point **P** such that

���

*ρ*(**Q**)

*earth*

where **Q** is a point within the earth with mass density *ρ*(**Q**) and volume element *dv***Q**, located at a distance *l* from **P** and *G* is the gravitational constant (Hofmann-Wellenhof & Moritz, 2005).

where **¯g<sup>e</sup>** is the gravitational vector associated with the position vector **re**. In practice, sensor measurements are further conditioned by the rotation of the earth. The difference of gravitation and centripetal acceleration caused by Earth's rotation is embodied in the *gravity*

**<sup>g</sup>***<sup>e</sup>* <sup>=</sup> **¯g***<sup>e</sup>* <sup>−</sup> **<sup>Ω</sup>***e***Ω***e***<sup>r</sup>**

where Ω*ie* is the skew-symmetric representation of the earth's rotation rate (the skew-symmetric matrix representation is treated in section 2.6). In the local-level frame, the

where *γ* is normal gravity and *σgu* is a disturbance in the vertical component of the vector

<sup>3</sup> We leave out the non-vertical deflections here for brevity, but a more detailed treatment should include

0 0 −(*γ* + *σgu*) ⎞

*<sup>∂</sup>***r***<sup>e</sup>* <sup>=</sup>

⎛ ⎝ *Vx Vy Vz* ⎞

**V**(**P**) = *G*

**¯g***<sup>e</sup>* <sup>=</sup> <sup>∇</sup>**V***<sup>e</sup>* <sup>=</sup> *<sup>∂</sup>***V***<sup>e</sup>*

**g***<sup>l</sup>* =

⎛ ⎝

The gravitational vector field is defined as the gradient of the potential:

*vector* and is more commonly used in practice. It is defined as

*δe* = (*Re* + *h*) cos *ψδλ* (5)

*δn* = (*M* + *h*)*δφ* (6) *δe* = (*N* + *h*) cos *φδλ* (7)

*<sup>l</sup> dv***<sup>Q</sup>** (8)

⎠ (9)

*<sup>e</sup>* (10)

⎠ (11)

and the distance along a parallel (in the East direction) is

**2.4 Gravitation and gravity**

gravity vector is expressed as

them.

(Hofmann-Wellenhof & Moritz, 2005).3

$$\begin{aligned} k &= \frac{b\gamma\_b}{a\gamma\_a} - 1\\ \gamma\_{a\prime}\gamma\_b &= \text{equatorial and polar gravity values} \end{aligned}$$

A computationally faster method to calculate gravity involves expanding (12) by power series with respect to *e*<sup>2</sup> and truncating, yielding:

$$\gamma\_0 = \gamma\_a (1 + \beta \sin^2 \phi + \beta\_1 \sin^2 2\phi) \tag{13}$$

where *γ<sup>a</sup>* is the gravity at the equator, *β* is the "gravity flattening" term (Hofmann-Wellenhof & Moritz, 2005), defined as

$$
\beta = \frac{\gamma\_b - \gamma\_a}{\gamma\_a} \tag{14}
$$

The second parameter in (13) *β*<sup>1</sup> is given by

$$
\beta\_1 = \frac{f^2}{4} - \frac{5f\omega\_\varepsilon^2 a}{8\gamma a} \tag{15}
$$

where *ω<sup>e</sup>* is the earth rotation constant. This approximation of (12) is accurate to approximately 0.1 *μ*g (Featherstone & Dentith, 1997), which is sufficient for navigation purposes. Higher-accuracy approximations are given in (Hofmann-Wellenhof & Moritz, 2005; Torge, 2001).Table 1 gives the relevant WGS84 parameters for computing normal gravity. The first four are the defining parameters of the system, while the last two are derived for use in computing normal gravity. Incorporating height *h* allows a more general formula for gravity


Table 1. WGS84 parameters

away from the ellipsoidal surface:

$$\gamma = \gamma\_0 - (3.0877 \times 10^{-6} - 4.4 \times 10^{-9} \sin^2 \phi)h + 0.72 \times 10^{-12} h^2 \tag{16}$$

The sequential angular rotations *ϕ*, *θ*, *ψ* are known as *Euler angles*; if *n* = *l*, the Euler angles are called roll, pitch, and yaw. Given a DCM, there is no unique decomposition into Euler angles without prior knowledge of the convention. For example, an equally valid DCM could be constructed from the sequence *z*, *x*, *z* or any of a number of permutations (Pio, 1966), but we remind the reader that unless the sequence is defined uniformly for the INS mechanization, the retrieval of heading, roll and pitch angles from a computed DCM may well be meaningless. We therefore stress the order given in (19) and will employ it exclusively

Fundamentals of GNSS-Aided Inertial Navigation 11

The representation of rotations as discussed up to now is tied to the historical simplicity of relating measurements of gimballed IMU axis encoders to the DCM. The careful reader will note however that singularities exist in this method. For instance, when a vehicle is pitched up 90 degrees, two axes respond to the same motion and a degree of freedom is lost, leaving no unique roll and heading values that will satisfy the DCM terms. In strap-down systems, this is mathematically equivalent to *gimbal lock* in gimballed INS. Mechanical and algorithmic solutions exist to the problem, but are beyond the scope of this writing. An alternative representation of rotations that does not suffer this problem is therefore sometimes

Quaternions are a four-dimensional extension of complex numbers having the form

**q** · **p** =

*e*<sup>1</sup> *e*<sup>2</sup> *e*<sup>3</sup>

the following relation holds for a rotation *φ* about this axis:

λ = �

⎛

⎜⎜⎝

where *a* is the real component and *b*, *c* and *d* are imaginary. Quaternion multiplication is defined as follows: let **q** and **p** be two quaternions having elements {*a*, *b*, *c*, *d*} and {*e*, *f* , *g*, *h*},

> *a* −*b* −*c* −*d b a* −*d c cd a* −*b d* −*cb a*

The Euler and Cayley-Hamilton Theorems can be employed to derive multiple formulations for rotations relying on the fact that any rotation matrix **R** encodes a single axis of rotation

cos *<sup>φ</sup>* <sup>=</sup> trace(**R**) <sup>−</sup> <sup>1</sup>

Through suitable derivation, we may define a rotation therefore by a four-parameter vector λ:

where the first element is the term involving the rotation and the last three define the *vector of the rotation matrix* which is sufficient for a single rotation but leaves the problem of propagating

cos *φ e*<sup>1</sup> sin *φ e*<sup>2</sup> sin *φ e*<sup>3</sup> sin *φ*

⎞

⎛

*e f g h* ⎞

⎟⎟⎠

�*<sup>T</sup>* associated with the eigenvalue +1. Along with this,

<sup>2</sup> (24)

�*<sup>T</sup>* (25)

⎜⎜⎝

⎟⎟⎠

sin−<sup>1</sup> (*R*32) tan−<sup>1</sup>

� <sup>−</sup> *<sup>R</sup>*<sup>12</sup> *<sup>R</sup>*<sup>22</sup> ��*<sup>T</sup>*

**q** = *a* + *b***i** + *c***j** + *d***k** (22)

(21)

(23)

moving forward. This being the case, we recover roll, pitch and yaw by

� tan−<sup>1</sup> � <sup>−</sup> *<sup>R</sup>*<sup>31</sup> *R*33 �

�

used employing quaternions.

which is the eigenvector **e** = �

**2.6.2 Quaternions**

respectively, then

*ϕθψ*�*<sup>T</sup>* =

#### **2.6 Mathematical treatment of rotations**

#### **2.6.1 Direction cosines matrix**

Before proceeding to linear and rotational models of motion, we must first discuss the formulation of the rigid-body transformations required to express vectors defined in a particular frame in terms of another frame. These are comprised of translations and rotations, the former being the straightforward operation of addition. We shall therefore direct our attention to rotations and their time-derivatives. In general a rotation matrix is an operator transforming vectors from one orthogonal basis to another. Let � *xyz* �*<sup>T</sup>* be a vector **p***<sup>a</sup>* in some frame *a* and � *x*� *y*� *z*� �*<sup>T</sup>* be a vector **p***<sup>b</sup>* in frame *b*, then

$$\begin{aligned} \mathbf{p}^b &= \mathbf{R}\_a^b \mathbf{p}^a\\ &= \begin{pmatrix} \mathbf{i}\_a \cdot \mathbf{i}\_b & \mathbf{j}\_a \cdot \mathbf{i}\_b & \mathbf{k}\_a \cdot \mathbf{i}\_b\\ \mathbf{i}\_a \cdot \mathbf{j}\_b & \mathbf{j}\_a \cdot \mathbf{j}\_b & \mathbf{k}\_a \cdot \mathbf{j}\_b\\ \mathbf{i}\_a \cdot \mathbf{k}\_b & \mathbf{j}\_a \cdot \mathbf{k}\_b & \mathbf{k}\_a \cdot \mathbf{k}\_b \end{pmatrix} \mathbf{p}^a \end{aligned} \tag{17}$$

where � **i***a* **j***a* **k***a* � and � **i***<sup>b</sup>* **j***<sup>b</sup>* **k***<sup>b</sup>* � are the orthonormal bases of *a* and *b*, respectively. Note the use of superscripts to indicate the reference frame. The rotation matrix notation indicates a transformation from the *a*-frame to the *b*-frame. Because the basis vectors are of unit length the dot products in **R***<sup>b</sup> <sup>a</sup>* define the cosines of the angles between the vector pairs, therefore the rotation matrix is also commonly known as the *direction cosines matrix* (DCM). The two properties of DCMs in a right-handed Cartesian system are:

$$\begin{aligned} 1. \ (\mathbf{R}\_a^b)^{-1} = (\mathbf{R}\_a^b)^T = \mathbf{R}\_b^a\\ 2. \ \det\left(\mathbf{R}\_a^b\right) = 1 \end{aligned}$$

DCMs in **R**<sup>3</sup> are decomposable into three elemental rotations performed sequentially about a principal axis in the originating frame, thence about the rotated and twice-rotated remaining axes(Kuipers, 1999). The elemental rotations about the *x*, *y* and *z* axes are defined as

$$\mathbf{R}\_{\mathbf{x}}(\boldsymbol{\varphi}) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos \boldsymbol{\varphi} & \sin \boldsymbol{\varphi} \\ 0 & -\sin \boldsymbol{\varphi} \cos \boldsymbol{\varphi} \end{pmatrix}, \mathbf{R}\_{\boldsymbol{\varphi}}(\boldsymbol{\theta}) = \begin{pmatrix} \cos \boldsymbol{\theta} \ 0 \ -\sin \boldsymbol{\theta} \\ 0 \ 1 \ 0 \\ \sin \boldsymbol{\theta} \ 0 \ \cos \boldsymbol{\theta} \end{pmatrix}, \mathbf{R}\_{\boldsymbol{z}}(\boldsymbol{\psi}) = \begin{pmatrix} \cos \boldsymbol{\psi} & \sin \boldsymbol{\psi} & 0 \\ -\sin \boldsymbol{\psi} \cos \boldsymbol{\varphi} \ 0 \\ 0 & 0 & 1 \end{pmatrix} \tag{18}$$

The choice of axis order is mathematically arbitrary, but while working in the ENU definition of the *l*-frame we must employ a *y*, *x*, *z* sequence when defining the rotation from the mechanization frame to the body frame. The transformation from the body frame to the navigation frame is therefore composed of the inverse (Titterton & Weston, 2004) , that is

$$\mathbf{R}\_b^n = \mathbf{R}\_n^{bT} = \mathbf{R}\_z(-\boldsymbol{\psi})\mathbf{R}\_x(-\boldsymbol{\theta})\mathbf{R}\_y(-\boldsymbol{\varphi})\tag{19}$$

where *n* is any of the valid mechanization frames given in section 2.2. More explicitly, the DCM in terms of the Euler angles is

$$\mathbf{R}\_{b}^{\eta} = \begin{pmatrix} \cos\psi\cos\varphi - \sin\psi\sin\theta\sin\varphi & -\sin\psi\cos\theta \cos\psi\sin\varphi + \sin\psi\sin\theta\cos\varphi\\ \sin\psi\cos\varphi + \cos\psi\sin\theta\sin\varphi & \cos\psi\cos\theta & \sin\psi\sin\varphi - \cos\psi\sin\theta\cos\varphi\\ -\cos\theta\sin\varphi & \sin\theta & \cos\theta\cos\varphi \end{pmatrix} \tag{20}$$

8 Will-be-set-by-IN-TECH

Before proceeding to linear and rotational models of motion, we must first discuss the formulation of the rigid-body transformations required to express vectors defined in a particular frame in terms of another frame. These are comprised of translations and rotations, the former being the straightforward operation of addition. We shall therefore direct our attention to rotations and their time-derivatives. In general a rotation matrix is an operator

> **i***<sup>a</sup>* · **i***<sup>b</sup>* **j***<sup>a</sup>* · **i***<sup>b</sup>* **k***<sup>a</sup>* · **i***<sup>b</sup>* **i***<sup>a</sup>* · **j***<sup>b</sup>* **j***<sup>a</sup>* · **j***<sup>b</sup>* **k***<sup>a</sup>* · **j***<sup>b</sup>* **i***<sup>a</sup>* · **k***<sup>b</sup>* **j***<sup>a</sup>* · **k***<sup>b</sup>* **k***<sup>a</sup>* · **k***<sup>b</sup>*

use of superscripts to indicate the reference frame. The rotation matrix notation indicates a transformation from the *a*-frame to the *b*-frame. Because the basis vectors are of unit length

the rotation matrix is also commonly known as the *direction cosines matrix* (DCM). The two

DCMs in **R**<sup>3</sup> are decomposable into three elemental rotations performed sequentially about a principal axis in the originating frame, thence about the rotated and twice-rotated remaining

The choice of axis order is mathematically arbitrary, but while working in the ENU definition of the *l*-frame we must employ a *y*, *x*, *z* sequence when defining the rotation from the mechanization frame to the body frame. The transformation from the body frame to the navigation frame is therefore composed of the inverse (Titterton & Weston, 2004) , that is

where *n* is any of the valid mechanization frames given in section 2.2. More explicitly, the

cos *ψ* cos *ϕ* − sin *ψ* sin *θ* sin *ϕ* − sin *ψ* cos *θ* cos *ψ* sin *ϕ* + sin *ψ* sin *θ* cos *ϕ* sin *ψ* cos *ϕ* + cos *ψ* sin *θ* sin *ϕ* cos *ψ* cos *θ* sin *ψ* sin *ϕ* − cos *ψ* sin *θ* cos *ϕ* − cos *θ* sin *ϕ* sin *θ* cos *θ* cos *ϕ*

cos *θ* 0 − sin *θ* 01 0 sin *θ* 0 cos *θ*

axes(Kuipers, 1999). The elemental rotations about the *x*, *y* and *z* axes are defined as

⎛ ⎝ ⎞

*<sup>a</sup>* define the cosines of the angles between the vector pairs, therefore

⎞

⎠ , **R***z*(*ψ*) =

*<sup>n</sup>* = **R***z*(−*ψ*)**R***x*(−*θ*)**R***y*(−*ϕ*) (19)

⎛ ⎝

cos *ψ* sin *ψ* 0 − sin *ψ* cos *ψ* 0 0 01

⎞

⎠ (20)

⎞ ⎠

(18)

are the orthonormal bases of *a* and *b*, respectively. Note the

*xyz* �*<sup>T</sup>* be a vector **p***<sup>a</sup>* in

<sup>⎠</sup> **<sup>p</sup>***<sup>a</sup>* (17)

transforming vectors from one orthogonal basis to another. Let �

**p***<sup>b</sup>* = **R***<sup>b</sup>*

= ⎛ ⎝

�

**i***<sup>b</sup>* **j***<sup>b</sup>* **k***<sup>b</sup>*

properties of DCMs in a right-handed Cartesian system are:

⎞

**R***n <sup>b</sup>* <sup>=</sup> **<sup>R</sup>***bT*

⎠ , **R***y*(*θ*) =

*x*� *y*� *z*� �*<sup>T</sup>* be a vector **p***<sup>b</sup>* in frame *b*, then

*a***p***a*

**2.6 Mathematical treatment of rotations**

**2.6.1 Direction cosines matrix**

some frame *a* and �

**i***a* **j***a* **k***a*

the dot products in **R***<sup>b</sup>*

*a*)−<sup>1</sup> = (**R***<sup>b</sup>*

*<sup>a</sup>*) = 1

⎛ ⎝ � and �

*<sup>a</sup>*)*<sup>T</sup>* = **R***<sup>a</sup> b*

10 0 0 cos *ϕ* sin *ϕ* 0 − sin *ϕ* cos *ϕ*

DCM in terms of the Euler angles is

where �

1. (**R***<sup>b</sup>*

2. det(**R***<sup>b</sup>*

**R***x*(*ϕ*) =

**R***n <sup>b</sup>* = ⎛ ⎝ The sequential angular rotations *ϕ*, *θ*, *ψ* are known as *Euler angles*; if *n* = *l*, the Euler angles are called roll, pitch, and yaw. Given a DCM, there is no unique decomposition into Euler angles without prior knowledge of the convention. For example, an equally valid DCM could be constructed from the sequence *z*, *x*, *z* or any of a number of permutations (Pio, 1966), but we remind the reader that unless the sequence is defined uniformly for the INS mechanization, the retrieval of heading, roll and pitch angles from a computed DCM may well be meaningless. We therefore stress the order given in (19) and will employ it exclusively moving forward. This being the case, we recover roll, pitch and yaw by

$$\left(\left(\boldsymbol{\varrho}\,\boldsymbol{\theta}\,\boldsymbol{\Psi}\right)^{T} = \left(\tan^{-1}\left(-\frac{R\_{31}}{R\_{33}}\right)\sin^{-1}\left(R\_{32}\right)\tan^{-1}\left(-\frac{R\_{12}}{R\_{22}}\right)\right)^{T}\tag{21}$$

The representation of rotations as discussed up to now is tied to the historical simplicity of relating measurements of gimballed IMU axis encoders to the DCM. The careful reader will note however that singularities exist in this method. For instance, when a vehicle is pitched up 90 degrees, two axes respond to the same motion and a degree of freedom is lost, leaving no unique roll and heading values that will satisfy the DCM terms. In strap-down systems, this is mathematically equivalent to *gimbal lock* in gimballed INS. Mechanical and algorithmic solutions exist to the problem, but are beyond the scope of this writing. An alternative representation of rotations that does not suffer this problem is therefore sometimes used employing quaternions.

#### **2.6.2 Quaternions**

Quaternions are a four-dimensional extension of complex numbers having the form

$$\mathbf{q} = a + b\mathbf{i} + c\mathbf{j} + d\mathbf{k} \tag{22}$$

where *a* is the real component and *b*, *c* and *d* are imaginary. Quaternion multiplication is defined as follows: let **q** and **p** be two quaternions having elements {*a*, *b*, *c*, *d*} and {*e*, *f* , *g*, *h*}, respectively, then

$$\mathbf{q} \cdot \mathbf{p} = \begin{pmatrix} a & -b & -c & -d \\ b & a & -d & c \\ c & d & a & -b \\ d & -c & b & a \end{pmatrix} \begin{pmatrix} e \\ f \\ g \\ h \end{pmatrix} \tag{23}$$

The Euler and Cayley-Hamilton Theorems can be employed to derive multiple formulations for rotations relying on the fact that any rotation matrix **R** encodes a single axis of rotation which is the eigenvector **e** = � *e*<sup>1</sup> *e*<sup>2</sup> *e*<sup>3</sup> �*<sup>T</sup>* associated with the eigenvalue +1. Along with this, the following relation holds for a rotation *φ* about this axis:

$$\cos \phi = \frac{\text{trace}(\mathbf{R}) - 1}{2} \tag{24}$$

Through suitable derivation, we may define a rotation therefore by a four-parameter vector λ:

$$\lambda = \begin{pmatrix} \cos \phi \ e\_1 \sin \phi \ e\_2 \sin \phi \ e\_3 \sin \phi \end{pmatrix}^T \tag{25}$$

where the first element is the term involving the rotation and the last three define the *vector of the rotation matrix* which is sufficient for a single rotation but leaves the problem of propagating

Over short periods of time for discrete measurements, the change in **R***<sup>n</sup>*

**R***n b* ≈

*<sup>b</sup>* (*<sup>t</sup>* <sup>+</sup> *<sup>δ</sup>t*) = **<sup>R</sup>***<sup>n</sup>*

Using Newton's second law, the motion of a particle in the *i*-frame is given as

particle. Incorporating the accelerations due to gravitation, we have

**r**¨ *<sup>i</sup>* = **f**

**r**¨ *<sup>i</sup>* = **f**

**r**˙

derivation of the model equations for navigating in the *i*-, *e*- and *l*-frames

*<sup>b</sup>* is used to resolve the forces in the *i*-frame:

<sup>4</sup> after the rigid transformation between the IMU and the vehicle has been applied

**f** *<sup>i</sup>* = **R***<sup>i</sup> b***f**

**v**˙ *<sup>i</sup>* = **f**

where **r**¨*<sup>i</sup>* is the second time derivative of position and **f<sup>i</sup>** is the specific force acting on the

Equation (39) represents a set of second-order differential equations which can be rewritten as

in which **r**˙, the first time derivative of position is equated with velocity **v**. We now turn to the

Because a vehicle is oriented arbitrarily with respect to the *i*-frame as defined above, the measurements of specific force will not be in this frame, but rather in the body-frame.4. A

In this notation, superscript of the measurement vector **f** and subscript of the rotation matrix cancel, yielding the representation of the vector in the desired frame. In navigation

**R***n*

is

so that

where **R***<sup>n</sup>*

important.

**3. Modeling**

**3.1 Linear motion**

a set of first-order equations:

**3.2.1 The** *i***-frame**

rotation matrix **R***<sup>i</sup>*

**3.2 State models for kinematic geodesy**

using the small angle approximation of (20), where sin *ϕ* ≈ *ϕ*, sin *θ* ≈ *θ* and sin *ψ* ≈ *ψ*, which

Fundamentals of GNSS-Aided Inertial Navigation 13

1 −*ψ θ ψ* 1 −*ϕ* −*θ ϕ* 1

*<sup>b</sup>* (*t*)**R***<sup>n</sup>*

*t* + *δt*. It is worth noting that under small incremental angles, the order of the rotations is not

*<sup>b</sup>* (*t*, *t* + *δt*) is the incremental rotation between the *b* and *n*-frames from time *t* to time

⎞

⎛ ⎝ *<sup>b</sup>* can be computed

⎠ (36)

*<sup>b</sup>* (*t*, *t* + *δt*) (37)

**<sup>i</sup>** (38)

*<sup>i</sup>* + **g**¯*<sup>i</sup>* (39)

*<sup>i</sup>* = **v<sup>i</sup>** (40)

*<sup>i</sup>* + **g**¯*<sup>i</sup>* (41)

*<sup>b</sup>* (42)

the transformation in time. A convenient relation between the elements of λ and quaternions exists, which allows us to take advantage of some felicitous properties of quaternions . Let **q** be the vector of the quaternion elements *a*, *b*, *c*, *d* as defined in (22). Then

$$a = \pm \sqrt{\frac{1 + \lambda\_1}{2}} \qquad \qquad \qquad \qquad = \cos(||\mathbf{e}||/2) \tag{26}$$

$$\frac{\kappa\_2}{2a} \tag{2a}$$

$$\frac{\hbar\_3}{2a} \tag{2a}$$

$$\frac{\hbar\_3}{2a} \tag{2b}$$

$$d = \frac{\lambda\_4}{2a} \tag{2a}$$

The parameters of the quaternion are properly called the *Euler-Rodrigues parameters* (Angeles, 2003) which define a unit quaternion. The rotation matrix in (20) in terms of Euler-Rodrigues parameters is

$$\mathbf{R}\_{b}^{n} = \begin{pmatrix} (a^2 + b^2 - c^2 - d^2) & 2(bc - ad) & 2(bd + ac) \\ 2(bc + ad) & (a^2 - b^2 + c^2 - d^2) & 2(cd - ab) \\ 2(bd - ac) & 2(cd + ab) & (a^2 - b^2 - c^2 + d^2) \end{pmatrix} \tag{30}$$

#### **2.6.3 Time-derivative of the DCM**

*<sup>b</sup>* <sup>=</sup> *<sup>λ</sup>*<sup>2</sup>

*<sup>c</sup>* <sup>=</sup> *<sup>λ</sup>*<sup>3</sup>

Let the vector ω*<sup>n</sup> nb* be the rotation rates of the body axes about the navigation system axes expressed in the *n*-frame given by

$$
\omega\_{nb}^{n} = \mathbf{R}\_{b}^{n} \omega\_{nb}^{b} \tag{31}
$$

and the resulting perpendicular linear velocity is given by

$$\begin{aligned} \dot{\mathbf{r}}^{\boldsymbol{n}} &= \boldsymbol{\omega}\_{nb}^{\boldsymbol{n}} \times \mathbf{r}^{b} \\ &= \mathbf{R}\_{b}^{\boldsymbol{n}} \boldsymbol{\omega}\_{nb}^{b} \times \mathbf{r}^{b} \\ &= \mathbf{R}\_{b}^{\boldsymbol{n}} \boldsymbol{\Omega}\_{nb}^{b} \mathbf{r}^{b} \end{aligned} \tag{32}$$

where **Ω***<sup>b</sup> nb* is the skew-symmetric form of <sup>ω</sup>*<sup>b</sup> nb*, with elements

$$
\boldsymbol{\Omega}\_{nb}^{b} = \begin{pmatrix} 0 & -\omega\_z & \omega\_y \\ \omega\_z & 0 & -\omega\_x \\ -\omega\_y & \omega\_x & 0 \end{pmatrix} \tag{33}
$$

We also have

$$\dot{\mathbf{r}}^n = \mathbf{R}\_b^n \mathbf{r}^b \tag{34}$$

Equating (32) and (34) yields the time derivative of the rotation matrix

$$\mathbf{R}\_b^n = \mathbf{R}\_b^n \mathbf{D}\_{nb}^b \tag{35}$$

Over short periods of time for discrete measurements, the change in **R***<sup>n</sup> <sup>b</sup>* can be computed using the small angle approximation of (20), where sin *ϕ* ≈ *ϕ*, sin *θ* ≈ *θ* and sin *ψ* ≈ *ψ*, which is

$$\mathbf{R}\_b^n \approx \begin{pmatrix} 1 & -\psi & \theta \\ \psi & 1 & -\varphi \\ -\theta & \varphi & 1 \end{pmatrix} \tag{36}$$

so that

10 Will-be-set-by-IN-TECH

the transformation in time. A convenient relation between the elements of λ and quaternions exists, which allows us to take advantage of some felicitous properties of quaternions . Let **q**

The parameters of the quaternion are properly called the *Euler-Rodrigues parameters* (Angeles, 2003) which define a unit quaternion. The rotation matrix in (20) in terms of Euler-Rodrigues

> (*a*<sup>2</sup> <sup>+</sup> *<sup>b</sup>*<sup>2</sup> <sup>−</sup> *<sup>c</sup>*<sup>2</sup> <sup>−</sup> *<sup>d</sup>*2) <sup>2</sup>(*bc* <sup>−</sup> *ad*) <sup>2</sup>(*bd* <sup>+</sup> *ac*) <sup>2</sup>(*bc* <sup>+</sup> *ad*) (*a*<sup>2</sup> <sup>−</sup> *<sup>b</sup>*<sup>2</sup> <sup>+</sup> *<sup>c</sup>*<sup>2</sup> <sup>−</sup> *<sup>d</sup>*2) <sup>2</sup>(*cd* <sup>−</sup> *ab*) <sup>2</sup>(*bd* <sup>−</sup> *ac*) <sup>2</sup>(*cd* <sup>+</sup> *ab*) (*a*<sup>2</sup> <sup>−</sup> *<sup>b</sup>*<sup>2</sup> <sup>−</sup> *<sup>c</sup>*<sup>2</sup> <sup>+</sup> *<sup>d</sup>*2)

> > *b*ω*<sup>b</sup>*

*nb* × **r** *b*

= **R***<sup>n</sup> b*ω*<sup>b</sup> nb* × **r** *b*

= **R***<sup>n</sup> <sup>b</sup>* **<sup>Ω</sup>***<sup>b</sup> nb***r**

> ⎛ ⎝

> > **r**˙ *<sup>n</sup>* = **R**˙ *<sup>n</sup> b* **r**

**R**˙ *n <sup>b</sup>* <sup>=</sup> **<sup>R</sup>***<sup>n</sup> <sup>b</sup>* **<sup>Ω</sup>***<sup>b</sup>*

<sup>2</sup> <sup>=</sup> cos(||**e**||/2) (26)

<sup>2</sup>*<sup>a</sup>* =(*e*1/||**e**||) sin(||**e**||/2) (27)

<sup>2</sup>*<sup>a</sup>* =(*e*2/||**e**||) sin(||**e**||/2) (28)

<sup>2</sup>*<sup>a</sup>* =(*e*3/||**e**||) sin(||**e**||/2) (29)

*nb* be the rotation rates of the body axes about the navigation system axes

*nb*, with elements

⎞

0 −*ω<sup>z</sup> ω<sup>y</sup> ω<sup>z</sup>* 0 −*ω<sup>x</sup>* −*ω<sup>y</sup> ω<sup>x</sup>* 0

*nb* (31)

*<sup>b</sup>* (32)

⎠ (33)

*<sup>b</sup>* (34)

*nb* (35)

⎞

⎠ (30)

be the vector of the quaternion elements *a*, *b*, *c*, *d* as defined in (22). Then

ω*n nb* <sup>=</sup> **<sup>R</sup>***<sup>n</sup>*

> **r**˙ *<sup>n</sup>* = ω*<sup>n</sup>*

**Ω***b nb* =

Equating (32) and (34) yields the time derivative of the rotation matrix

and the resulting perpendicular linear velocity is given by

*nb* is the skew-symmetric form of <sup>ω</sup>*<sup>b</sup>*

*a* = ±

*<sup>b</sup>* <sup>=</sup> *<sup>λ</sup>*<sup>2</sup>

*<sup>c</sup>* <sup>=</sup> *<sup>λ</sup>*<sup>3</sup>

*<sup>d</sup>* <sup>=</sup> *<sup>λ</sup>*<sup>4</sup>

⎛ ⎝

parameters is

Let the vector ω*<sup>n</sup>*

where **Ω***<sup>b</sup>*

We also have

**R***n <sup>b</sup>* =

**2.6.3 Time-derivative of the DCM**

expressed in the *n*-frame given by

�<sup>1</sup> <sup>+</sup> *<sup>λ</sup>*<sup>1</sup>

$$\mathbf{R}\_b^n(t+\delta t) = \mathbf{R}\_b^n(t)\mathbf{R}\_b^n(t, t+\delta t) \tag{37}$$

where **R***<sup>n</sup> <sup>b</sup>* (*t*, *t* + *δt*) is the incremental rotation between the *b* and *n*-frames from time *t* to time *t* + *δt*. It is worth noting that under small incremental angles, the order of the rotations is not important.

## **3. Modeling**

#### **3.1 Linear motion**

Using Newton's second law, the motion of a particle in the *i*-frame is given as

$$
\ddot{\mathbf{r}}^{\dot{l}} = \mathbf{f}^{\dot{l}} \tag{38}
$$

where **r**¨*<sup>i</sup>* is the second time derivative of position and **f<sup>i</sup>** is the specific force acting on the particle. Incorporating the accelerations due to gravitation, we have

$$
\ddot{\mathbf{r}}^i = \mathbf{f}^i + \bar{\mathbf{g}}^i \tag{39}
$$

Equation (39) represents a set of second-order differential equations which can be rewritten as a set of first-order equations:

$$\dot{\mathbf{r}}^{j} = \mathbf{v}^{\mathrm{i}} \tag{40}$$

$$
\dot{\mathbf{v}}^{\dot{l}} = \mathbf{f}^{\dot{l}} + \bar{\mathbf{g}}^{\dot{l}} \tag{41}
$$

in which **r**˙, the first time derivative of position is equated with velocity **v**. We now turn to the derivation of the model equations for navigating in the *i*-, *e*- and *l*-frames

#### **3.2 State models for kinematic geodesy**

#### **3.2.1 The** *i***-frame**

Because a vehicle is oriented arbitrarily with respect to the *i*-frame as defined above, the measurements of specific force will not be in this frame, but rather in the body-frame.4. A rotation matrix **R***<sup>i</sup> <sup>b</sup>* is used to resolve the forces in the *i*-frame:

$$\mathbf{f}^{i} = \mathbf{R}\_{b}^{i} \mathbf{f}^{b} \tag{42}$$

In this notation, superscript of the measurement vector **f** and subscript of the rotation matrix cancel, yielding the representation of the vector in the desired frame. In navigation

<sup>4</sup> after the rigid transformation between the IMU and the vehicle has been applied

**3.2.3 The** *l***-frame**

whose time rate is

them in the *enu* system:

or

where **Ω***<sup>l</sup>*

in (11). Finally, the transformation **R***<sup>l</sup>*

**x**˙ *<sup>l</sup>* = ⎛ ⎝ **r**˙ *l* **v**˙ *l* **R**˙ *l b*

equations. To begin with, we note

Navigation states expressed in the inertial and earth-fixed frames do not lend themselves to easy intuitive interpretation near the surface of the earth. Here, the more familiar concepts of latitude and longitude along with roll, pitch and heading are preferable. We therefore must mechanize the system in the *l*-frame, which necessitates a reformulation of the state-variable

Fundamentals of GNSS-Aided Inertial Navigation 15

*φ λ h*

*φ*˙ *λ*˙ ˙ *h*

*ve vn vu*

0 <sup>1</sup>

(*N*+*h*) cos *<sup>φ</sup>* 0 0 0 01

The first two non-zero elements of **D**−<sup>1</sup> are clearly derivable from the derivatives of equations

*ie* <sup>+</sup> **<sup>Ω</sup>***<sup>l</sup>*

*ie* <sup>+</sup> **<sup>Ω</sup>***<sup>l</sup>*

velocity of the *l*-frame with respect to the *e*-frame expressed in the *l*-frame and **g***<sup>l</sup>* is as defined

*<sup>b</sup>***f***<sup>b</sup>* <sup>−</sup> (2**Ω***<sup>l</sup>*

**R***l b*(**Ω***<sup>b</sup>*

*b*(**Ω***<sup>b</sup>*

*ib* <sup>−</sup> **<sup>Ω</sup>***<sup>b</sup>*

**D**−1**v***<sup>l</sup>*

*ie* <sup>+</sup> **<sup>Ω</sup>***<sup>l</sup>*

*ib* <sup>−</sup> **<sup>Ω</sup>***<sup>b</sup> il*)

*el*)**v***<sup>l</sup>* <sup>+</sup> **<sup>g</sup>***<sup>l</sup>*

⎞

*<sup>b</sup>* is the solution to

1

*<sup>i</sup>* <sup>−</sup> (2**Ω***<sup>l</sup>*

*<sup>b</sup>* <sup>−</sup> (2**Ω***<sup>l</sup>*

*ie* is the angular velocity of Earth's rotation expressed in the *<sup>l</sup>*-frame, **<sup>Ω</sup>***<sup>l</sup>*

(*M*+*h*) 0

*el*)**v***<sup>l</sup>* <sup>−</sup> **<sup>Ω</sup>***<sup>l</sup>*

⎞

⎛ ⎝ *ve vn vu*

*ie***Ω***<sup>l</sup> el***r** *l*

⎞

⎟⎠

Rather than express velocity in terms of the geodetic coordinates, it is preferable to represent

�*<sup>T</sup>* (52)

�*<sup>T</sup>* (53)

�*<sup>T</sup>* (54)

*el*)**v***<sup>l</sup>* <sup>+</sup> **<sup>g</sup>***<sup>l</sup>* (57)

*il*) (58)

⎠ (59)

⎠ (56)

*el* is the angular

*<sup>l</sup>* = **D**−1**v***<sup>l</sup>* (55)

**r***<sup>l</sup>* = �

**r**˙ *<sup>l</sup>* = �

**v***<sup>l</sup>* = �

Now, the time derivative of position in *φ*, *λ*, *h* is related to **v***<sup>l</sup>* through

⎛

⎜⎝

**r**˙

⎞ ⎠ =

(6) and (7) with respect to time. Acceleration is now given by

**v**˙ *<sup>l</sup>* = **R***<sup>l</sup> i***r**¨

> = **R***<sup>l</sup> b***f**

> > **R**˙ *l <sup>b</sup>* <sup>=</sup> **<sup>R</sup>***<sup>l</sup> b***Ω***<sup>b</sup> ib* <sup>=</sup> **<sup>R</sup>***<sup>l</sup>*

⎞ ⎠ =

The state-variable equations in the local-level reference frame are therefore

⎛ ⎝

**R***l*

⎛ ⎝ *φ*˙ *λ*˙ ˙ *h*

applications, the time derivative of **R<sup>i</sup> <sup>b</sup>** is a function of the angular velocity expressed by the vector ω*<sup>b</sup> ib* between the two reference frames. Here, <sup>ω</sup>*<sup>b</sup> ib* is the representation of the rotation rate expressed in the body frame whose skew-symmetric form is the matrix **Ω***<sup>b</sup> ib*, giving

$$
\dot{\mathbf{R}}\_b^i = \mathbf{R}\_b^i \mathbf{D}\_{ib}^b \tag{43}
$$

which is, of course, a particular realization of (35), in which *n* = *i*. Assuming now that the gravity vector is computed by (11), we must apply a transformation from the *l*-frame to the *i*-frame to formulate (41) as

$$
\dot{\mathbf{v}} = \mathbf{R}\_b^i \mathbf{f}^b + \mathbf{R}\_l^i \bar{\mathbf{g}}^l \tag{44}
$$

The time derivatives of position, velocity and attitude are now represented as

$$\dot{\mathbf{x}}^{i} = \begin{pmatrix} \dot{\mathbf{r}}^{i} \\ \dot{\mathbf{v}}^{i} \\ \dot{\mathbf{R}}\_{b}^{i} \end{pmatrix} = \begin{pmatrix} \mathbf{v}^{i} \\ \mathbf{R}\_{b}^{i}\mathbf{f}^{b} + \mathbf{R}\_{l}^{i}\bar{\mathbf{g}}^{l} \\ \mathbf{R}\_{b}^{i}\mathbf{D}\_{lb}^{b} \end{pmatrix} \tag{45}$$

The solution to (45) is the navigation state of the vehicle: position, velocity and attitude in the inertial frame. The equations in (45) are known as the *mechanization equations* for the inertial navigation system.

#### **3.2.2 The** *e***-frame**

To arrive at the state equations in the *e*-frame, we begin by considering the transformation of the position vector in the *e*-frame into the *i*-frame:

$$\mathbf{r}^i = \mathbf{R}^i\_\varepsilon \mathbf{r}^\varepsilon \tag{46}$$

The first and second derivatives are then

$$\dot{\mathbf{r}}^l = \mathbf{R}\_\varepsilon^l \left( \dot{\mathbf{r}}^\varepsilon + \mathbf{D}\_{\dot{\imath}\varepsilon}^\varepsilon \mathbf{r}^\varepsilon \right) \tag{47}$$

$$\ddot{\mathbf{r}}^i = \mathbf{R}\_e^i \left( \ddot{\mathbf{r}}^\varepsilon + 2\boldsymbol{\Omega}\_{ie}^\varepsilon \dot{\mathbf{r}}^\varepsilon + \dot{\mathbf{\Omega}}\_{ie}^\varepsilon \mathbf{r}^\varepsilon + \boldsymbol{\Omega}\_{ie}^\varepsilon \boldsymbol{\Omega}\_{ie}^\varepsilon \mathbf{r}^\varepsilon \right) \tag{48}$$

As the rotation rate between the *<sup>e</sup>* and *<sup>i</sup>* frames is constant, we see that **<sup>Ω</sup>**˙ *<sup>e</sup> ie* is zero leaving

$$\ddot{\mathbf{r}}^{\dot{i}} = \mathbf{R}\_{\varepsilon}^{\dot{i}} \left( \ddot{\mathbf{r}}^{\varepsilon} + 2\mathbf{D}\_{\mathrm{ie}}^{\varepsilon} \dot{\mathbf{r}}^{\varepsilon} + \mathbf{D}\_{\mathrm{ie}}^{\varepsilon} \mathbf{D}\_{\mathrm{ie}}^{\varepsilon} \mathbf{r}^{\varepsilon} \right) \tag{49}$$

The second derivative of position in the *e*-frame is obtained using (10) and (39), yielding

$$\ddot{\mathbf{r}}^{\varepsilon} = \mathbf{R}\_b^{\varepsilon}\mathbf{f}^b - 2\mathbf{D}\_{i\varepsilon}^{\varepsilon}\dot{\mathbf{r}}^{\varepsilon} + \mathbf{g}^{\varepsilon} \tag{50}$$

The second term in (50) is due to the Coriolis force, whereas the last term is the gravity vector represented in the *e*-frame, which can be found in (Schwarz & Wei, 1990).

Putting this together to form the state-variable equations, we have

$$\dot{\mathbf{x}}^{\varepsilon} = \begin{pmatrix} \dot{\mathbf{r}}^{\varepsilon} \\ \dot{\mathbf{v}}^{\varepsilon} \\ \dot{\mathbf{R}}\_{b}^{\varepsilon} \end{pmatrix} = \begin{pmatrix} \mathbf{v}^{\varepsilon} \\ \mathbf{R}\_{b}^{\varepsilon} \mathbf{f}^{b} - 2\boldsymbol{\Omega}\_{i\varepsilon}^{\varepsilon} \mathbf{v}^{\varepsilon} + \mathbf{g}^{\varepsilon} \\ \mathbf{R}\_{b}^{\varepsilon} \left( \boldsymbol{\Omega}\_{i\varepsilon}^{b} - \boldsymbol{\Omega}\_{i b}^{b} \right) \end{pmatrix} \tag{51}$$

#### **3.2.3 The** *l***-frame**

12 Will-be-set-by-IN-TECH

which is, of course, a particular realization of (35), in which *n* = *i*. Assuming now that the gravity vector is computed by (11), we must apply a transformation from the *l*-frame to the

> ⎛ ⎝

The solution to (45) is the navigation state of the vehicle: position, velocity and attitude in the inertial frame. The equations in (45) are known as the *mechanization equations* for the inertial

To arrive at the state equations in the *e*-frame, we begin by considering the transformation of

**r** *<sup>i</sup>* = **R***<sup>i</sup> e***r**

*<sup>e</sup>* + 2**Ω***<sup>e</sup> ie***r**˙ *<sup>e</sup>* + **Ω***<sup>e</sup> ie***Ω***<sup>e</sup> ie***r** *e*

The second derivative of position in the *e*-frame is obtained using (10) and (39), yielding

⎛

⎜⎝

**R***e*

**R***e b* � **Ω***b ie* <sup>−</sup> **<sup>Ω</sup>***<sup>b</sup> ib* �

*<sup>b</sup>* <sup>−</sup> <sup>2</sup>**Ω***<sup>e</sup> ie***r**˙

The second term in (50) is due to the Coriolis force, whereas the last term is the gravity vector

**v***e*

*ie***v***<sup>e</sup>* + **<sup>g</sup>***<sup>e</sup>*

⎞

*<sup>b</sup>***f***<sup>b</sup>* <sup>−</sup> <sup>2</sup>**Ω***<sup>e</sup>*

*<sup>e</sup>* + **Ω***<sup>e</sup> ie***r** *e*

As the rotation rate between the *<sup>e</sup>* and *<sup>i</sup>* frames is constant, we see that **<sup>Ω</sup>**˙ *<sup>e</sup>*

**r**¨ *<sup>e</sup>* = **R***<sup>e</sup> b***f**

represented in the *e*-frame, which can be found in (Schwarz & Wei, 1990).

⎞ ⎠ =

Putting this together to form the state-variable equations, we have

**x**˙ *<sup>e</sup>* = ⎛ ⎝ **r**˙ *e* **v**˙ *e* **R**˙ *e b* **R***i <sup>b</sup>***f***<sup>b</sup>* <sup>+</sup> **<sup>R</sup>***<sup>i</sup> l* **g**¯*l*

**v***i*

⎞

**R***i b***Ω***<sup>b</sup> ib*

**<sup>b</sup>** is a function of the angular velocity expressed by the

*ib* is the representation of the rotation

*ib* (43)

**g**¯*<sup>l</sup>* (44)

*<sup>e</sup>* (46)

(48)

*ie* is zero leaving

) (49)

⎟⎠ (51)

*<sup>e</sup>* + **g***<sup>e</sup>* (50)

) (47)

⎠ (45)

*ib*, giving

applications, the time derivative of **R<sup>i</sup>**

*i*-frame to formulate (41) as

navigation system.

**3.2.2 The** *e***-frame**

*ib* between the two reference frames. Here, <sup>ω</sup>*<sup>b</sup>*

**x**˙ *<sup>i</sup>* = ⎛ ⎝ **r**˙ *i* **v**˙ *i* **R**˙ *i b*

the position vector in the *e*-frame into the *i*-frame:

**r**˙ *<sup>i</sup>* = **R***<sup>i</sup> <sup>e</sup>* (**r**˙

**r**¨ *<sup>i</sup>* = **R***<sup>i</sup> e* � **r**¨ *<sup>e</sup>* + 2**Ω***<sup>e</sup> ie***r**˙ *<sup>e</sup>* + **<sup>Ω</sup>**˙ *<sup>e</sup> ie***r** *<sup>e</sup>* + **Ω***<sup>e</sup> ie***Ω***<sup>e</sup> ie***r** *e* �

> **r**¨ *<sup>i</sup>* = **R***<sup>i</sup> <sup>e</sup>* (**r**¨

The first and second derivatives are then

rate expressed in the body frame whose skew-symmetric form is the matrix **Ω***<sup>b</sup>*

**R**˙ *i <sup>b</sup>* <sup>=</sup> **<sup>R</sup>***<sup>i</sup> b***Ω***<sup>b</sup>*

**v**˙ = **R***<sup>i</sup> b***f** *<sup>b</sup>* + **R***<sup>i</sup> l*

> ⎞ ⎠ =

The time derivatives of position, velocity and attitude are now represented as

vector ω*<sup>b</sup>*

Navigation states expressed in the inertial and earth-fixed frames do not lend themselves to easy intuitive interpretation near the surface of the earth. Here, the more familiar concepts of latitude and longitude along with roll, pitch and heading are preferable. We therefore must mechanize the system in the *l*-frame, which necessitates a reformulation of the state-variable equations. To begin with, we note

$$\mathbf{r}^l = \begin{pmatrix} \phi \ \lambda \ h \end{pmatrix}^T \tag{52}$$

whose time rate is

$$\dot{\mathbf{r}}^l = \begin{pmatrix} \phi \ \dot{\lambda} \ h \end{pmatrix}^T \tag{53}$$

Rather than express velocity in terms of the geodetic coordinates, it is preferable to represent them in the *enu* system:

$$\mathbf{v}^l = \begin{pmatrix} v\_\varepsilon \ v\_\mathfrak{n} \ v\_\mathfrak{u} \end{pmatrix}^T \tag{54}$$

Now, the time derivative of position in *φ*, *λ*, *h* is related to **v***<sup>l</sup>* through

$$\dot{\mathbf{r}}^l = \mathbf{D}^{-1} \mathbf{v}^l \tag{55}$$

or

$$
\begin{pmatrix} \dot{\phi} \\ \dot{\lambda} \\ \dot{h} \end{pmatrix} = \begin{pmatrix} 0 & \frac{1}{(M+h)} \ 0 \\ \frac{1}{(N+h)\cos\phi} & 0 & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} v\_{\varepsilon} \\ v\_{\eta} \\ v\_{\eta} \end{pmatrix} \tag{56}
$$

The first two non-zero elements of **D**−<sup>1</sup> are clearly derivable from the derivatives of equations (6) and (7) with respect to time. Acceleration is now given by

$$\begin{split} \dot{\mathbf{v}}^{l} &= \mathbf{R}\_{i}^{l} \ddot{\mathbf{r}}^{i} - (2\boldsymbol{\Omega}\_{i\varepsilon}^{l} + \boldsymbol{\Omega}\_{el}^{l}) \mathbf{v}^{l} - \boldsymbol{\Omega}\_{i\varepsilon}^{l} \boldsymbol{\Omega}\_{el}^{l} \mathbf{r}^{l} \\ &= \mathbf{R}\_{b}^{l} \mathbf{f}^{b} - (2\boldsymbol{\Omega}\_{i\varepsilon}^{l} + \boldsymbol{\Omega}\_{el}^{l}) \mathbf{v}^{l} + \mathbf{g}^{l} \end{split} \tag{57}$$

where **Ω***<sup>l</sup> ie* is the angular velocity of Earth's rotation expressed in the *<sup>l</sup>*-frame, **<sup>Ω</sup>***<sup>l</sup> el* is the angular velocity of the *l*-frame with respect to the *e*-frame expressed in the *l*-frame and **g***<sup>l</sup>* is as defined in (11). Finally, the transformation **R***<sup>l</sup> <sup>b</sup>* is the solution to

$$\mathbf{R}\_b^l = \mathbf{R}\_b^l \mathbf{T}\_{ib}^b = \mathbf{R}\_b^l (\mathbf{T}\_{ib}^b - \mathbf{T}\_{il}^b) \tag{58}$$

The state-variable equations in the local-level reference frame are therefore

$$\dot{\mathbf{x}}^{l} = \begin{pmatrix} \dot{\mathbf{r}}^{l} \\ \dot{\mathbf{v}}^{l} \\ \mathbf{R}^{l}\_{b} \end{pmatrix} = \begin{pmatrix} \mathbf{D}^{-1} \mathbf{v}^{l} \\ \mathbf{R}^{l}\_{b} \mathbf{f}^{b} - (\mathbf{2} \mathbf{D}^{l}\_{i\varepsilon} + \mathbf{D}^{l}\_{el}) \mathbf{v}^{l} + \mathbf{g}^{l} \\ \mathbf{R}^{l}\_{b} (\mathbf{D}^{b}\_{ib} - \mathbf{D}^{b}\_{il}) \end{pmatrix} \tag{59}$$

**3.4 Updating the transformation R***<sup>l</sup>*

The powers of **Ω**¯ are expressed as

expansion to obtain

**3.4.1 Quaternion update**

we have

time dependent, no closed form solution exists

**R***k*+<sup>1</sup> = **R***ke*

**R***k*+<sup>1</sup> = **R***<sup>k</sup>*

Letting **Ω**¯ *<sup>q</sup>* = **Ω***qδt*, the discrete solution to (**??**) is

 ∞ ∑ *n*=0

= **q***<sup>k</sup>* +

1 2*nn*! **Ω**¯ *n q* **q***k*

numerical integration techniques, which we shall not cover here.

1 2 2 

**q***k*+<sup>1</sup> =

 **I** +

*b*

(Kohler & Johnson, 2006). During a small time interval *δt* relative to the dynamics of the vehicle, however, we may assume a constant angular rate ω. The angular changes of the *b*-frame with respect to the *l*-frame are expressed as α = ω*δt*. The skew-symmetric form **Ω***δt*

Fundamentals of GNSS-Aided Inertial Navigation 17

1 *n*! (**Ω***nδt*

**<sup>Ω</sup>**¯ <sup>3</sup> <sup>=</sup> −||α||2**Ω**¯ **<sup>Ω</sup>**¯ <sup>4</sup> <sup>=</sup> −||α||2**Ω**¯ <sup>2</sup> **<sup>Ω</sup>**¯ <sup>5</sup> <sup>=</sup> ||α||4**Ω**¯ **<sup>Ω</sup>**¯ <sup>6</sup> <sup>=</sup> ||α||4**Ω**¯ <sup>2</sup>

which allows us to collect the terms in (60) in sine and cosine components of the series


The quaternion parametrization requires a four-element vector as we have seen. A discrete solution to the quaternion update follows similarly to that of the DCM. The difference here is that now the skew-symmetric representation of the rotation rate vector is 4×4. In block form,

> <sup>−</sup>**<sup>Ω</sup>** <sup>ω</sup> <sup>−</sup>ω*<sup>T</sup>* <sup>0</sup>

*<sup>n</sup>*) = **R***<sup>k</sup>*

∞ ∑ *n*=0

1 − cos(||α||) ||α||<sup>2</sup> **<sup>Ω</sup>**¯ <sup>2</sup>

1 *n*!

is now constant over a short time. This presents the discrete closed form solution

. . .

1

**Ω***<sup>q</sup>* =

cos ||α||

<sup>2</sup> <sup>−</sup> <sup>1</sup>

Implementation in either the DCM or the quaternion parametrizations involves employing

 **I** +

2


<sup>2</sup> **<sup>Ω</sup>**¯ *<sup>q</sup>* 

∞ ∑ *n*=0

**<sup>Ω</sup>***δ<sup>t</sup>* = **R***<sup>k</sup>*

*<sup>b</sup>* in time. As both **<sup>R</sup>***<sup>l</sup>*

*<sup>b</sup>* and **Ω** are

(61)

(62)

**q***<sup>k</sup>* (63)

**Ω**¯ *<sup>n</sup>* (60)

The solution of (35) propagates the transformation matrix **R***<sup>l</sup>*

Fig. 7. Mechanization in the *l*-frame

## **3.3 Mechanization in the** *l***-frame**

Because of its wide applicability and intuitiveness, we shall focus on the mechanization of the state-variable equations described above in the local-level frame. To begin with, an initial position in geodetic coordinates *φ*, *λ*, *h* must be known, along with an initial velocity and transformation **R***<sup>b</sup> <sup>l</sup>* in order for the integration of the measurements from the accelerometers and gyroscopes to give proper navigation parameters. We shall consider initial position and velocity to be given by GPS, for example, and will treat the problem of resolving initial attitude later. The block diagram in figure 7 shows the relationships among the components of the state-variable equations in the context of an algorithmic implementation.

Given an initial attitude, velocity and the earth's rotation rate ω*e*, the rotation of the *l*-frame with respect to the *e*-frame and thence the rotation between the *e*-frame and the *i*-frame is computed and transformed into a representation of the rotation of the *l*-frame with respect to the *i*-frame *expressed in the b-frame* (ω*<sup>b</sup> il*). The quantities of this vector are subtracted from the body angular rate measurements to yield angular rates between the *l*-frame and the *b*-frame expressed in the *b*-frame (ω*<sup>b</sup> lb*). Given fast enough measurements relative to the dynamics of the vehicle, the small angle approximation can be used and **R***<sup>b</sup> <sup>l</sup>* can be integrated over the time {*t*, *<sup>t</sup>* <sup>+</sup> *<sup>δ</sup>t*} to provide the next **<sup>R</sup>***<sup>b</sup> <sup>l</sup>* which is used to transform the accelerometer measurements into the *l* frame. The normal gravity *γ* computed via Somigliana's formula (12) is added while the quantities arising from the Coriolis force are subtracted, yielding the acceleration in the *l*-frame. This, in turn, is integrated to provide velocity and again to yield position, which are fed back into the system to update the necessary parameters and propagate the navigation state forward in time.

 

#### **3.4 Updating the transformation R***<sup>l</sup> b*

14 Will-be-set-by-IN-TECH

ݎሺߛ ሻ

+ +

+ ܴ

ܴ ሾ݇ െ ͳሿ

ȳ 

> ܴ ሾ݇ െ ͳሿ

ܴ ݎ൫ ൯ ൌ ൭

Fig. 7. Mechanization in the *l*-frame

the *i*-frame *expressed in the b-frame* (ω*<sup>b</sup>*

expressed in the *b*-frame (ω*<sup>b</sup>*

state forward in time.

{*t*, *<sup>t</sup>* <sup>+</sup> *<sup>δ</sup>t*} to provide the next **<sup>R</sup>***<sup>b</sup>*

**3.3 Mechanization in the** *l***-frame**


݂ ݂

× ߱ 

߱ 


߱ 

transformation **R***<sup>b</sup>*

߱   ܴ ሾ݇ሿ ሶ ሾ݇ሿ

݂ ൌ ሺȳ

∫

߱ 

+ +

Ͳ ߣݏܿ ߣ݅݊ݏെ ߶ݏܿ ߣ݅݊ݏ߶݅݊ݏെ ߣݏܿ߶݅݊ݏെ ߶݅݊ݏ ߣ݅݊ݏ߶ݏܿ ߣݏܿ߶ݏܿ

state-variable equations in the context of an algorithmic implementation.

the vehicle, the small angle approximation can be used and **R***<sup>b</sup>*

ଵିݖ

+ +

ȳ 

ʹȳ 

ȳ ʹȳ 

2

×

Because of its wide applicability and intuitiveness, we shall focus on the mechanization of the state-variable equations described above in the local-level frame. To begin with, an initial position in geodetic coordinates *φ*, *λ*, *h* must be known, along with an initial velocity and

and gyroscopes to give proper navigation parameters. We shall consider initial position and velocity to be given by GPS, for example, and will treat the problem of resolving initial attitude later. The block diagram in figure 7 shows the relationships among the components of the

Given an initial attitude, velocity and the earth's rotation rate ω*e*, the rotation of the *l*-frame with respect to the *e*-frame and thence the rotation between the *e*-frame and the *i*-frame is computed and transformed into a representation of the rotation of the *l*-frame with respect to

body angular rate measurements to yield angular rates between the *l*-frame and the *b*-frame

into the *l* frame. The normal gravity *γ* computed via Somigliana's formula (12) is added while the quantities arising from the Coriolis force are subtracted, yielding the acceleration in the *l*-frame. This, in turn, is integrated to provide velocity and again to yield position, which are fed back into the system to update the necessary parameters and propagate the navigation

߱ 

ሷݎ

 ʹȳ ݒሻ

∫

ݒ

∫

ݎ ൌ ൭ ߶ ߣ ݄ ൱

ݒ ൌ ൭ ݒ ݒ ௨ݒ

߰ ൌ ൭ ߮ ߠ ߰ ൱

൱

ሶݎ

ݎଵሺିܦ ሻ

߱ 

൱

ȳ 

×

߱ ݎ൫ ݒ ǡ ൯ ൌ

*<sup>l</sup>* in order for the integration of the measurements from the accelerometers

ۉ ۈۈ ۇ

*il*). The quantities of this vector are subtracted from the

*<sup>l</sup>* can be integrated over the time

*lb*). Given fast enough measurements relative to the dynamics of

*<sup>l</sup>* which is used to transform the accelerometer measurements

ݒെ ݒ ݄ܯ ݄ܰ ݒ ی߶ܽ݊ݐ ݄ܰ

ۋۋ ۊ

 

Gyros

The solution of (35) propagates the transformation matrix **R***<sup>l</sup> <sup>b</sup>* in time. As both **<sup>R</sup>***<sup>l</sup> <sup>b</sup>* and **Ω** are time dependent, no closed form solution exists

(Kohler & Johnson, 2006). During a small time interval *δt* relative to the dynamics of the vehicle, however, we may assume a constant angular rate ω. The angular changes of the *b*-frame with respect to the *l*-frame are expressed as α = ω*δt*. The skew-symmetric form **Ω***δt* is now constant over a short time. This presents the discrete closed form solution

$$\mathbf{R}\_{k+1} = \mathbf{R}\_k e^{\mathbf{D}\delta t} = \mathbf{R}\_k \sum\_{n=0}^{\infty} \frac{1}{n!} (\boldsymbol{\Omega}^n \delta t^n) = \mathbf{R}\_k \sum\_{n=0}^{\infty} \frac{1}{n!} \bar{\boldsymbol{\Omega}}^n \tag{60}$$

The powers of **Ω**¯ are expressed as

$$\begin{aligned} \bar{\Omega}^3 &= -||\alpha||^2 \bar{\Omega} \\ \bar{\Omega}^4 &= -||\alpha||^2 \bar{\Omega}^2 \\ \bar{\Omega}^5 &= ||\alpha||^4 \bar{\Omega} \\ \bar{\Omega}^6 &= ||\alpha||^4 \bar{\Omega}^2 \\ \vdots \end{aligned}$$

which allows us to collect the terms in (60) in sine and cosine components of the series expansion to obtain

$$\mathbf{R}\_{k+1} = \mathbf{R}\_k \left( \mathbf{I} + \frac{1}{||\alpha||} \sin(||\alpha||) \bar{\mathbf{D}} + \frac{1 - \cos(||\alpha||)}{||\alpha||^2} \hat{\mathbf{D}}^2 \right) \tag{61}$$

#### **3.4.1 Quaternion update**

The quaternion parametrization requires a four-element vector as we have seen. A discrete solution to the quaternion update follows similarly to that of the DCM. The difference here is that now the skew-symmetric representation of the rotation rate vector is 4×4. In block form, we have

$$\boldsymbol{\Omega}\_{\boldsymbol{\theta}} = \begin{pmatrix} -\boldsymbol{\Omega} & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^T & 0 \end{pmatrix} \tag{62}$$

Letting **Ω**¯ *<sup>q</sup>* = **Ω***qδt*, the discrete solution to (**??**) is

$$\begin{split} \mathbf{q}\_{k+1} &= \left( \sum\_{n=0}^{\infty} \frac{1}{2^n n!} \bar{\mathbf{D}}\_q^n \right) \mathbf{q}\_k \\ &= \mathbf{q}\_k + \frac{1}{2} \left( 2 \left( \cos \frac{||\boldsymbol{\alpha}||}{2} - 1 \right) \mathbf{I} + \frac{2}{||\boldsymbol{\alpha}||} \sin \frac{||\boldsymbol{\alpha}||}{2} \bar{\mathbf{D}}\_q \right) \mathbf{q}\_k \end{split} \tag{63}$$

Implementation in either the DCM or the quaternion parametrizations involves employing numerical integration techniques, which we shall not cover here.

We have thereby resolved the planar tilt of the body frame with respect to the local-level frame as well as the rotation about the leveled *z* axis in the body frame that would bring about a zero-rate measurement along the transformed *x* axis of the IMU. In practice, sensor noise of vehicle disturbance would not allow for the exact solutions presented above, leading to an initial alignment error. One way to minimize the error is to collect stationary measurements over an extended period of time and compute the mean values or apply another type of low-pass filter. It is worth noting here that the estimate of this alignment step is considered coarse and can be further improved through a fine alignment process in which external aiding,

Fundamentals of GNSS-Aided Inertial Navigation 19

The state-variable equations described up to now for determining the navigation parameters

where **x** are the physical parameters of the system and **u** are inputs to the system. The true values of **x** are generally not known, with only an approximation available. For example, in an INS, the approximation comes from the integration of sensor output over time. Let **x**˜ represent

*<sup>δ</sup>***x**˙(*t*) = *<sup>∂</sup>***<sup>f</sup>**

*∂***x**

which is the linearized form of the state equations in terms of the errors, called the *error state*

Where **F***<sup>x</sup>* is the dynamics matrix. So far, we have a model for a purely deterministic system (one free of sensor errors). Taking the input as part of the (noisy) sensor output, the second

Where **F***<sup>u</sup>* is the dynamics matrix for the sensor errors, **w**(*t*) is a random Gaussian sequence with a shaping matrix **G**. The general state-variable form of the error model is therefore

 *δ***x** *δ***u** 

 **x**(*t*)

+ **0 G** 

> **G**

**x**˙(*t*) = **f**(*t*, **x**(*t*), **u**(*t*)) (70)

**x**˜ = **x** + *δ***x** (71)

*δ***x**(*t*) (73)

**w** (76)

*δ***x**˙(*t*) = **F***xδ***x**(*t*) (74)

*δ***u**˙ = **F***uδ***u**(*t*) + **Gw**(*t*) (75)

**x**˜(**t**) = **f**(*t*, **x**˜(*t*), **u**(*t*)) = **f**(*t*, **x**(*t*) + *δ***x**(*t*), **u**(*t*)) (72)

of the vehicle represent non-linear dynamic system with the general form

in the form of position and/or velocity updates, are used.

the approximation, then the true parameters are

˙

*equations*. It takes the familiar form

set of differential equations is formed:

Taylor series approximation to the error term yields

where *δ***x** are the error states. Replacing **x** with **x**˜, we have

 *δ***x**˙ *δ***u**˙ 

 **x**˙(*t*)

=

 **F***<sup>x</sup>* **F***xu* **0 F***u*

 **F**

**3.6 Error dynamics**

#### **3.5 Initialization**

As stated above, the implementation of an INS requires the knowledge of initial position, velocity and attitude. Initial position and velocity can be provided through any appropriate means, but most commonly are retrieved through GPS measurements. The initial attitude, on the other hand, can be resolved using the raw measurements of the IMU and the known or computed gravity and earth rotation rate. """'Initialization can be performed from a static position or during maneuvers, the latter being more complex and beyond the scope of this chapter.

## **3.5.1 Alignment of a static platform**

Alignment refers to the process of determining the initial orientation of the INS body axes with respect to the navigation frame by rotating the system until expected measurements are observed in the transformed output. Specifically, with respect to the *x* and *y* axes, we define the process as *leveling*, while the heading (about the *z* axis) is termed *gyro-compassing*. First, we begin by noting that the measured specific forces in the body frame are related to gravity in the local-level frame through

$$\mathbf{g}^l = \mathbf{R}\_b^l \mathbf{f}^b \tag{64}$$

Through the orthogonality of **R***<sup>l</sup> <sup>b</sup>*, we see that

$$
\begin{pmatrix} R\_{31} \\ R\_{32} \\ R\_{33} \end{pmatrix} = -\frac{1}{\mathcal{S}} \begin{pmatrix} f\_x \\ f\_y \\ f\_z \end{pmatrix} \tag{65}
$$

which defines one basis vector of the transformation. The sensed rotation rates are similarly related to the earth rotation rate through

$$
\omega^l\_{ie} = \mathbf{R}^l\_b \omega^b\_{ie} \tag{66}
$$

or

$$
\begin{pmatrix} 0\\ \omega\_{\ell}\cos\phi\\ \omega\_{\ell}\sin\phi \end{pmatrix} = \begin{pmatrix} R\_{11} & R\_{12} & R\_{13} \\ R\_{21} & R\_{22} & R\_{23} \\ R\_{31} & R\_{32} & R\_{33} \end{pmatrix} \begin{pmatrix} \omega\_{\chi} \\ \omega\_{y} \\ \omega\_{z} \end{pmatrix} \tag{67}
$$

It is easy to show that

$$
\begin{pmatrix} R\_{21} \\ R\_{22} \\ R\_{23} \end{pmatrix} = \frac{1}{\omega\_\ell \cos \phi} \begin{pmatrix} \omega\_x \\ \omega\_y \\ \omega\_z \end{pmatrix} - \tan \phi \begin{pmatrix} R\_{31} \\ R\_{32} \\ R\_{33} \end{pmatrix} \tag{68}
$$

from which the second basis of the transformation is found. To complete the rotation matrix, we take advantage again of its orthogonality, arriving at

$$
\begin{pmatrix} R\_{11} \\ R\_{12} \\ R\_{13} \end{pmatrix} = \begin{pmatrix} R\_{21} \\ R\_{22} \\ R\_{23} \end{pmatrix} \times \begin{pmatrix} R\_{31} \\ R\_{32} \\ R\_{33} \end{pmatrix} \tag{69}
$$

We have thereby resolved the planar tilt of the body frame with respect to the local-level frame as well as the rotation about the leveled *z* axis in the body frame that would bring about a zero-rate measurement along the transformed *x* axis of the IMU. In practice, sensor noise of vehicle disturbance would not allow for the exact solutions presented above, leading to an initial alignment error. One way to minimize the error is to collect stationary measurements over an extended period of time and compute the mean values or apply another type of low-pass filter. It is worth noting here that the estimate of this alignment step is considered coarse and can be further improved through a fine alignment process in which external aiding, in the form of position and/or velocity updates, are used.

## **3.6 Error dynamics**

16 Will-be-set-by-IN-TECH

As stated above, the implementation of an INS requires the knowledge of initial position, velocity and attitude. Initial position and velocity can be provided through any appropriate means, but most commonly are retrieved through GPS measurements. The initial attitude, on the other hand, can be resolved using the raw measurements of the IMU and the known or computed gravity and earth rotation rate. """'Initialization can be performed from a static position or during maneuvers, the latter being more complex and beyond the scope of this

Alignment refers to the process of determining the initial orientation of the INS body axes with respect to the navigation frame by rotating the system until expected measurements are observed in the transformed output. Specifically, with respect to the *x* and *y* axes, we define the process as *leveling*, while the heading (about the *z* axis) is termed *gyro-compassing*. First, we begin by noting that the measured specific forces in the body frame are related to gravity

> **g***<sup>l</sup>* =**R***<sup>l</sup> b***f**

which defines one basis vector of the transformation. The sensed rotation rates are similarly

⎛ ⎝ *fx fy fz* ⎞

⎞ ⎠ ⎛ ⎝ *ωx ωy ωz*

> ⎛ ⎝ *R*<sup>31</sup> *R*<sup>32</sup> *R*<sup>33</sup>

⎞

⎞

⎞

⎞ <sup>⎠</sup> <sup>=</sup> <sup>−</sup> <sup>1</sup> *g*

*<sup>b</sup>*, we see that ⎛ ⎝ *R*<sup>31</sup> *R*<sup>32</sup> *R*<sup>33</sup>

> ω*l ie* <sup>=</sup>**R***<sup>l</sup> b*ω*<sup>b</sup>*

⎞ ⎠ = ⎛ ⎝

> ⎛ ⎝ *ωx ωy ωz*

from which the second basis of the transformation is found. To complete the rotation matrix,

⎞ ⎠ ×

⎛ ⎝ *R*<sup>21</sup> *R*<sup>22</sup> *R*<sup>23</sup>

*R*<sup>11</sup> *R*<sup>12</sup> *R*<sup>13</sup> *R*<sup>21</sup> *R*<sup>22</sup> *R*<sup>23</sup> *R*<sup>31</sup> *R*<sup>32</sup> *R*<sup>33</sup>

⎞

⎠ − tan *φ*

⎛ ⎝ *R*<sup>31</sup> *R*<sup>32</sup> *R*<sup>33</sup>

⎛ ⎝

⎛ ⎝ *R*<sup>21</sup> *R*<sup>22</sup> *R*<sup>23</sup>

we take advantage again of its orthogonality, arriving at

0 *ω<sup>e</sup>* cos *φ ω<sup>e</sup>* sin *φ*

> ⎞ <sup>⎠</sup> <sup>=</sup> <sup>1</sup> *ω<sup>e</sup>* cos *φ*

⎛ ⎝ *R*<sup>11</sup> *R*<sup>12</sup> *R*<sup>13</sup> ⎞ ⎠ = *<sup>b</sup>* (64)

⎠ (65)

⎠ (67)

⎠ (68)

⎠ (69)

*ie* (66)

**3.5 Initialization**

chapter.

or

It is easy to show that

**3.5.1 Alignment of a static platform**

in the local-level frame through

Through the orthogonality of **R***<sup>l</sup>*

related to the earth rotation rate through

The state-variable equations described up to now for determining the navigation parameters of the vehicle represent non-linear dynamic system with the general form

$$\dot{\mathbf{x}}(t) = \mathbf{f}(t, \mathbf{x}(t), \mathbf{u}(t)) \tag{70}$$

where **x** are the physical parameters of the system and **u** are inputs to the system. The true values of **x** are generally not known, with only an approximation available. For example, in an INS, the approximation comes from the integration of sensor output over time. Let **x**˜ represent the approximation, then the true parameters are

$$
\tilde{\mathbf{x}} = \mathbf{x} + \delta \mathbf{x} \tag{71}
$$

where *δ***x** are the error states. Replacing **x** with **x**˜, we have

$$\dot{\mathbf{x}}(t) = \mathbf{f}(t, \mathbf{\dot{x}}(t), \mathbf{u}(t)) = \mathbf{f}(t, \mathbf{x}(t) + \delta \mathbf{x}(t), \mathbf{u}(t)) \tag{72}$$

Taylor series approximation to the error term yields

$$
\delta \dot{\mathbf{x}}(t) = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} \delta \mathbf{x}(t) \tag{73}
$$

which is the linearized form of the state equations in terms of the errors, called the *error state equations*. It takes the familiar form

$$
\delta \dot{\mathbf{x}}(t) = \mathbf{F}\_{\mathbf{x}} \delta \mathbf{x}(t) \tag{74}
$$

Where **F***<sup>x</sup>* is the dynamics matrix. So far, we have a model for a purely deterministic system (one free of sensor errors). Taking the input as part of the (noisy) sensor output, the second set of differential equations is formed:

$$
\delta\dot{\mathbf{u}} = \mathbf{F}\_{\mathrm{ll}}\delta\mathbf{u}(t) + \mathbf{G}\mathbf{w}(t) \tag{75}
$$

Where **F***<sup>u</sup>* is the dynamics matrix for the sensor errors, **w**(*t*) is a random Gaussian sequence with a shaping matrix **G**. The general state-variable form of the error model is therefore

$$
\underbrace{\begin{pmatrix} \delta\dot{\mathbf{x}} \\ \delta\dot{\mathbf{u}} \end{pmatrix}}\_{\mathbf{x}(t)} = \underbrace{\begin{pmatrix} \mathbf{F}\_{\mathcal{X}} \ \mathbf{F}\_{\mathcal{X}\mathcal{U}} \\ \mathbf{0} \ \mathbf{F}\_{\mathcal{U}} \end{pmatrix}}\_{\mathbf{F}} \underbrace{\begin{pmatrix} \delta\mathbf{x} \\ \delta\mathbf{u} \end{pmatrix}}\_{\mathbf{x}(t)} + \underbrace{\begin{pmatrix} \mathbf{0} \\ \mathbf{G} \end{pmatrix}}\_{\mathbf{G}} \mathbf{w} \tag{76}
$$

**3.6.3 Alignment errors**

where **Ω***<sup>l</sup>*

**x**˙ *l* (*t*) =

(89).

⎛

*δ***r**˙ *l δ***v**˙ *<sup>l</sup>* �˙ *l* **d**˙ **b**˙

⎞

⎟⎟⎟⎟⎠ =

⎛

⎜⎜⎜⎜⎜⎜⎜⎜⎝

<sup>−</sup>**F***<sup>l</sup>*

⎜⎜⎜⎜⎝

transformation between frames is

Here, **d** is the vector of gyro drift biases.

**3.6.5 Error state equations in the** *l* **frame**

**3.6.4 Gyroscope drifts and accelerometer biases**

The alignment errors �*<sup>l</sup>* represent misalignment between the *b* and *l* frames expressed in the

Fundamentals of GNSS-Aided Inertial Navigation 21

*<sup>b</sup>* = (**<sup>I</sup>** <sup>+</sup> **<sup>E</sup>***<sup>l</sup>*

*il*�*<sup>l</sup>* <sup>−</sup> *<sup>δ</sup>*ω*<sup>l</sup>*

Gyroscopes and accelerometers exhibit noise behavior that is characterizeable at different time scales such that one can generally separate errors that are long-term stable and those that behave stochastically during the period of interest. Errors of the former type are characterized in a laboratory setting, prior to field deployment and their effects can generally be removed

The noise in gyro and accelerometer measurements exhibit varying degrees of temporal correlation, depending on the quality of the devices. The underlying random processes are therefore conveniently modeled as first-order Gauss-Markov processes. Their equations are

where α and β are diagonal matrices whose non-zero elements are the reciprocals of the

Combining the derivations for the error equations of position, velocity, alignment, gyro drift

<sup>−</sup>**Ω***<sup>l</sup>*

To derive the elements of the dynamics matrix **F**, we need to specify all the matrix elements in

**<sup>D</sup>**−1*δ***v***<sup>l</sup>* <sup>−</sup> **<sup>D</sup>**−1**D***rδ***r***<sup>l</sup>*

(2ω*<sup>l</sup>*

*il* <sup>−</sup> **<sup>R</sup>***<sup>l</sup> b***d**

*ie* <sup>+</sup> *<sup>δ</sup>*ω*<sup>l</sup>*

*el*) + *δγ<sup>l</sup>* <sup>+</sup> **<sup>R</sup>***<sup>l</sup>*

*b***b**

⎞

⎟⎟⎟⎟⎟⎟⎟⎟⎠

(89)

*el*)*δ***v***<sup>l</sup>* <sup>+</sup> **<sup>V</sup>***<sup>l</sup>*

*il*�*<sup>l</sup>* <sup>−</sup> *<sup>δ</sup>*ω*<sup>l</sup>*

−*α***d** + **w***<sup>d</sup>* −*β***b** + **w***<sup>b</sup>*

and accelerometer bias gives the state-variable equations for the navigation errors:

*ie* <sup>+</sup> **<sup>Ω</sup>***<sup>l</sup>*

)**R***<sup>l</sup>*

*il* <sup>−</sup> **<sup>R</sup>***<sup>l</sup>*

, so that the approximate

*il*.

*<sup>b</sup>* (85)

*<sup>b</sup>***d** (86)

*il* with corresponding errors *<sup>δ</sup>*ω*<sup>l</sup>*

**<sup>d</sup>**˙ <sup>=</sup> <sup>−</sup>α**<sup>d</sup>** <sup>+</sup> **<sup>w</sup>***<sup>d</sup>* (87) **<sup>b</sup>**˙ <sup>=</sup> <sup>−</sup>β**<sup>b</sup>** <sup>+</sup> **<sup>w</sup>***<sup>b</sup>* (88)

*l*-frame. The vector �*<sup>l</sup>* can be expressed in skew-symmetric form as **E***<sup>l</sup>*

�˙ *<sup>l</sup>* <sup>=</sup> <sup>−</sup>**Ω***<sup>l</sup>*

*il* is the skew-symmetric form of the angular rates <sup>ω</sup>*<sup>l</sup>*

The differential equations for the alignment errors are

**R**˜ *l*

from the measurements, leaving residual errors that are modeled stochastically.

correlation time constants and **w***<sup>d</sup>* and **w***<sup>b</sup>* are white noise sequences.

�*<sup>l</sup>* <sup>−</sup> (2**Ω***<sup>l</sup>*

The terms in **F***xu* account for the dependence of the navigational errors upon the sensor errors and the full state vector includes the elements of **u**. In an INS mechanized in the *l*-frame, the error state vector is explicitly written

$$\mathbf{x}(t) = \begin{pmatrix} \delta\phi \ \delta\lambda \ \delta\hbar \ \delta\upsilon\_{\varepsilon} \ \delta\upsilon\_{\mathfrak{u}} \ \delta\upsilon\_{\mathfrak{u}} \ \mathfrak{e}\_{\mathfrak{u}} \ \mathfrak{e}\_{\mathfrak{u}} \ \mathfrak{e}\_{\mathfrak{u}} \ \mathfrak{e}\_{\mathfrak{u}} \ d\_{\mathfrak{x}} \ d\_{\mathfrak{y}} \ d\_{\mathfrak{z}} \ b\_{\mathfrak{x}} \ b\_{\mathfrak{y}} \ b\_{\mathfrak{z}} \end{pmatrix}^{T} \tag{77}$$

where the first three elements are position errors, the next three are velocity errors and after which come alignment errors, gyro drifts and accelerometer biases. We shall now derive the equations governing each.

#### **3.6.1 Position errors**

Recall that in section 3.2.3, we preferentially expressed velocities in the *l*-frame in terms of *ve*, *vn*, *vu* rather than directly as functions of *φ*, *λ*, *h*, using

$$\mathbf{v}^l = \mathbf{D}\dot{\mathbf{r}}^l\tag{78}$$

where

$$\mathbf{D} = \begin{pmatrix} 0 & (N+h)\cos\phi \ 0 \\ (M+h) & 0 & 0 \\ 0 & 0 & 1 \end{pmatrix} \tag{79}$$

which, after linearization, yields the error equation

$$
\delta \mathbf{v}^l = \mathbf{D} \delta \dot{\mathbf{r}}^l + \delta \mathbf{D} \dot{\mathbf{r}}^l \tag{80}
$$

Recognizing that the first term in (80) is simply **D***δ***v***<sup>l</sup>* , we see that it is the second term that contains the position errors. We can rewrite (80) then as

$$
\delta \mathbf{v}^{l} = \mathbf{D} \delta \dot{\mathbf{r}}^{l} + \mathbf{D}\_{l} \delta \mathbf{r}^{l} \tag{81}
$$

where **D***<sup>r</sup>* is a coefficient matrix. The position error equation is then

$$
\delta \dot{\mathbf{r}}^l = \mathbf{D}^{-1} \delta \mathbf{v}^l - \mathbf{D}^{-1} \mathbf{D}\_l \delta \mathbf{r}^l \tag{82}
$$

#### **3.6.2 Velocity errors**

The approximate form of (57) is

$$\begin{split} \dot{\mathbf{v}}^{1} &= \tilde{\mathbf{R}}\_{b}^{l} \tilde{\mathbf{f}}^{b} - (\mathbf{2} \tilde{\mathbf{D}}\_{i\varepsilon}^{l} + \tilde{\mathbf{D}}\_{el}^{l}) \tilde{\mathbf{v}}^{l} + \tilde{\gamma}^{l} \\ &= (\mathbf{I} + \mathbf{E}^{l}) \mathbf{R}\_{b}^{l} (\mathbf{f}^{b} + \delta \mathbf{f}^{b}) - (\mathbf{2} (\mathbf{D}\_{i\varepsilon}^{l} + \delta \mathbf{D}\_{i\varepsilon}^{l}) + \mathbf{D}\_{el}^{l} + \delta \mathbf{D}\_{el}^{l}) (\mathbf{v}^{l} + \delta \mathbf{v}^{l}) + (\gamma^{l} + \delta \gamma^{l}) \end{split} \tag{83}$$

where **E***<sup>l</sup>* is the skew-symmetric form of the alignment error. After subtracting the true acceleration and ignoring second-order terms, we have

$$\begin{split} \delta \dot{\mathbf{v}}^{l} &= \mathbf{E}^{l} \mathbf{R}\_{b}^{l} \mathbf{f}^{b} - (2 \boldsymbol{\Omega}\_{i\varepsilon}^{l} + \boldsymbol{\Omega}\_{el}^{l}) \delta \mathbf{v}^{l} - (2 \delta \boldsymbol{\Omega}\_{i\varepsilon}^{l} + \delta \mathbf{D}\_{el}^{l}) \mathbf{v}^{l} + \delta \boldsymbol{\gamma}^{l} + \mathbf{R}\_{b}^{l} \delta \mathbf{f}^{b} - \delta \mathbf{g}^{l} \\ &= -\mathbf{F}^{l} \boldsymbol{\epsilon}^{l} - (2 \boldsymbol{\Omega}\_{i\varepsilon}^{l} + \boldsymbol{\Omega}\_{el}^{l}) \delta \mathbf{v}^{l} + \mathbf{V}^{l} (2 \delta \boldsymbol{\omega}\_{i\varepsilon}^{l} + \delta \boldsymbol{\omega}\_{el}^{l}) + \delta \boldsymbol{\gamma}^{l} + \mathbf{R}\_{b}^{l} \mathbf{b} \end{split} \tag{84}$$

where **F***<sup>l</sup>* is the skew-symmetric matrix representation of **f***<sup>l</sup>* , �*<sup>l</sup>* is the misalignment vector, **V***<sup>l</sup>* is the skew-symmetric form of **v***<sup>l</sup>* and **b** is the vector of accelerometer biases, in which the gravity disturbance vector *δ***g***<sup>l</sup>* is also included. The terms *δ*ω*<sup>l</sup> el* and *<sup>δ</sup>*ω*<sup>l</sup> ie* are the errors in the transport rate and the Earth rotation rate, respectively, both of which are dependent on errors in position and velocity. Finally, *δγ<sup>l</sup>* is the error in the calculation of normal gravity, which is dependent on the position error and any errors in the model.

#### **3.6.3 Alignment errors**

18 Will-be-set-by-IN-TECH

The terms in **F***xu* account for the dependence of the navigational errors upon the sensor errors and the full state vector includes the elements of **u**. In an INS mechanized in the *l*-frame, the

where the first three elements are position errors, the next three are velocity errors and after which come alignment errors, gyro drifts and accelerometer biases. We shall now derive the

Recall that in section 3.2.3, we preferentially expressed velocities in the *l*-frame in terms of

**v***<sup>l</sup>* = **Dr**˙

*δ***v***<sup>l</sup>* = **D***δ***r**˙

*δ***v***<sup>l</sup>* = **D***δ***r**˙

*ie* <sup>+</sup> *<sup>δ</sup>***Ω***<sup>l</sup>*

0 (*N* + *h*) cos *φ* 0 (*M* + *h*) 0 0 0 01

*<sup>l</sup>* + *δ***Dr**˙

*<sup>l</sup>* + **D***rδ***r**

*<sup>l</sup>* <sup>=</sup> **<sup>D</sup>**−1*δ***v***<sup>l</sup>* <sup>−</sup> **<sup>D</sup>**−1**D***rδ***<sup>r</sup>**

*ie*) + **<sup>Ω</sup>***<sup>l</sup>*

(2*δ*ω*<sup>l</sup>*

where **E***<sup>l</sup>* is the skew-symmetric form of the alignment error. After subtracting the true

is the skew-symmetric form of **v***<sup>l</sup>* and **b** is the vector of accelerometer biases, in which the

transport rate and the Earth rotation rate, respectively, both of which are dependent on errors in position and velocity. Finally, *δγ<sup>l</sup>* is the error in the calculation of normal gravity, which is

*el*)*δ***v***<sup>l</sup>* <sup>−</sup> (2*δ***Ω***<sup>l</sup>*

*el*)*δ***v***<sup>l</sup>* <sup>+</sup> **<sup>V</sup>***<sup>l</sup>*

*el* <sup>+</sup> *<sup>δ</sup>***Ω***<sup>l</sup>*

*ie* <sup>+</sup> *<sup>δ</sup>***Ω***<sup>l</sup>*

*ie* <sup>+</sup> *<sup>δ</sup>*ω*<sup>l</sup>*

*el*)(**v***<sup>l</sup>* <sup>+</sup> *<sup>δ</sup>***v***<sup>l</sup>*

*el*)**v***<sup>l</sup>* <sup>+</sup> *δγ<sup>l</sup>* <sup>+</sup> **<sup>R</sup>***<sup>l</sup>*

*el*) + *δγ<sup>l</sup>* <sup>+</sup> **<sup>R</sup>***<sup>l</sup>*

*el* and *<sup>δ</sup>*ω*<sup>l</sup>*

*δφ δλ δh δve δvn δvu �e �n �u dx dy dz bx by bz*

�*<sup>T</sup>* (77)

*<sup>l</sup>* (78)

⎠ (79)

*<sup>l</sup>* (80)

, we see that it is the second term that

*<sup>l</sup>* (81)

*<sup>l</sup>* (82)

)+(*γ<sup>l</sup>* + *δγ<sup>l</sup>*

*<sup>b</sup>* <sup>−</sup> *<sup>δ</sup>***g***<sup>l</sup>*

*<sup>b</sup>***b** (84)

*ie* are the errors in the

*<sup>b</sup>δ***f**

, �*<sup>l</sup>* is the misalignment vector, **V***<sup>l</sup>*

) (83)

⎞

error state vector is explicitly written

equations governing each.

**3.6.1 Position errors**

**3.6.2 Velocity errors**

= (**I** + **E***<sup>l</sup>*

˙ **v**˜**<sup>l</sup>** = **R**˜ *<sup>l</sup> b* ˜ **f** *<sup>b</sup>* <sup>−</sup> (2**Ω**˜ *<sup>l</sup>*

The approximate form of (57) is

*δ***v**˙ *<sup>l</sup>* = **E***<sup>l</sup>*

)**R***<sup>l</sup> b*(**f** *<sup>b</sup>* + *δ***f**

> **R***l b***f**

<sup>=</sup> <sup>−</sup>**F***<sup>l</sup>*

where

**x**(*t*) = �

*ve*, *vn*, *vu* rather than directly as functions of *φ*, *λ*, *h*, using

which, after linearization, yields the error equation

Recognizing that the first term in (80) is simply **D***δ***v***<sup>l</sup>*

*ie* <sup>+</sup> **<sup>Ω</sup>**˜ *<sup>l</sup>*

acceleration and ignoring second-order terms, we have

*<sup>b</sup>* <sup>−</sup> (2**Ω***<sup>l</sup>*

where **F***<sup>l</sup>* is the skew-symmetric matrix representation of **f***<sup>l</sup>*

gravity disturbance vector *δ***g***<sup>l</sup>* is also included. The terms *δ*ω*<sup>l</sup>*

dependent on the position error and any errors in the model.

�*<sup>l</sup>* <sup>−</sup> (2**Ω***<sup>l</sup>*

contains the position errors. We can rewrite (80) then as

**D** =

where **D***<sup>r</sup>* is a coefficient matrix. The position error equation is then *δ***r**˙

*el*)**v**˜*<sup>l</sup>* + *<sup>γ</sup>*˜*<sup>l</sup>*

*<sup>b</sup>*) <sup>−</sup> (2(**Ω***<sup>l</sup>*

*ie* <sup>+</sup> **<sup>Ω</sup>***<sup>l</sup>*

*ie* <sup>+</sup> **<sup>Ω</sup>***<sup>l</sup>*

⎛ ⎝ The alignment errors �*<sup>l</sup>* represent misalignment between the *b* and *l* frames expressed in the *l*-frame. The vector �*<sup>l</sup>* can be expressed in skew-symmetric form as **E***<sup>l</sup>* , so that the approximate transformation between frames is

$$\mathbf{R}\_b^l = (\mathbf{I} + \mathbf{E}^l)\mathbf{R}\_b^l \tag{85}$$

The differential equations for the alignment errors are

$$\dot{\epsilon}^{l} = -\mathbf{D}\_{il}^{l}\mathbf{e}^{l} - \delta\omega\_{il}^{l} - \mathbf{R}\_{b}^{l}\mathbf{d} \tag{86}$$

where **Ω***<sup>l</sup> il* is the skew-symmetric form of the angular rates <sup>ω</sup>*<sup>l</sup> il* with corresponding errors *<sup>δ</sup>*ω*<sup>l</sup> il*. Here, **d** is the vector of gyro drift biases.

#### **3.6.4 Gyroscope drifts and accelerometer biases**

Gyroscopes and accelerometers exhibit noise behavior that is characterizeable at different time scales such that one can generally separate errors that are long-term stable and those that behave stochastically during the period of interest. Errors of the former type are characterized in a laboratory setting, prior to field deployment and their effects can generally be removed from the measurements, leaving residual errors that are modeled stochastically.

The noise in gyro and accelerometer measurements exhibit varying degrees of temporal correlation, depending on the quality of the devices. The underlying random processes are therefore conveniently modeled as first-order Gauss-Markov processes. Their equations are

$$
\dot{\mathbf{d}} = -\alpha \mathbf{d} + \mathbf{w}\_d \tag{87}
$$

$$
\dot{\bf b} = -\beta \bf b + \mathbf{w}\_{\varnothing} \tag{88}
$$

where α and β are diagonal matrices whose non-zero elements are the reciprocals of the correlation time constants and **w***<sup>d</sup>* and **w***<sup>b</sup>* are white noise sequences.

#### **3.6.5 Error state equations in the** *l* **frame**

Combining the derivations for the error equations of position, velocity, alignment, gyro drift and accelerometer bias gives the state-variable equations for the navigation errors:

$$\mathbf{x}'(t) = \begin{pmatrix} \delta \mathbf{r}' \\ \delta \mathbf{v}' \\ \dot{\mathbf{c}}' \\ \dot{\mathbf{d}} \\ \dot{\mathbf{b}} \end{pmatrix} \qquad = \begin{pmatrix} \mathbf{D}^{-1} \delta \mathbf{v}^{l} - \mathbf{D}^{-1} \mathbf{D}\_{r} \delta \mathbf{r}^{l} \\\\ -\mathbf{F}^{l} \mathbf{c}^{l} - (2\boldsymbol{\Omega}\_{i\varepsilon}^{l} + \boldsymbol{\Omega}\_{el}^{l}) \delta \mathbf{v}^{l} + \mathbf{V}^{l} (2\boldsymbol{\omega}\_{i\varepsilon}^{l} + \delta \boldsymbol{\omega}\_{el}^{l}) + \delta \gamma^{l} + \mathbf{R}\_{b}^{l} \mathbf{b} \\\ -\boldsymbol{\Omega}\_{il}^{l} \mathbf{c}^{l} - \delta \boldsymbol{\omega}\_{il}^{l} - \mathbf{R}\_{b}^{l} \mathbf{d} \\\ \mathbf{-a} \mathbf{d} + \mathbf{w}\_{d} \\\ -\beta \mathbf{b} + \mathbf{w}\_{b} \end{pmatrix} \tag{89}$$

To derive the elements of the dynamics matrix **F**, we need to specify all the matrix elements in (89).

The fourth term is

Combining all the terms, we have

*δ***v**˙ *<sup>l</sup>* =

⎛

⎜⎜⎝

⎛

−˙

*h*+*φ*˙ tan *φ*(*M*+*h*)

0 *fu* −*fn* −*fu* 0 *fe fn* −*fe* 0

� �� � **F**<sup>23</sup>

The third equation in (89) can be derived similarly as

<sup>−</sup>2(*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) sin *<sup>φ</sup>* <sup>−</sup>˙

⎞ ⎠ �*<sup>l</sup>* + ⎛ ⎝

⎜⎝

⎛ ⎝

**3.6.8 Matrix formulation of alignment errors**

⎛

⎜⎜⎝

⎛

⎜⎝

�˙ *<sup>l</sup>* = ⎛ ⎝ *δ*γ*<sup>l</sup>* =

<sup>2</sup>*ωe*(*vu* sin *<sup>φ</sup>* <sup>+</sup> *vn* cos *<sup>φ</sup>*) + *vnλ*˙

<sup>−</sup>2*ωeve* cos *<sup>φ</sup>* <sup>−</sup> *veλ*˙

� �� � **F**<sup>21</sup>

⎛ ⎝

0 0 −*∂γ <sup>∂</sup><sup>h</sup> δh*

<sup>−</sup>2*ωeve* sin *<sup>φ</sup>* <sup>0</sup> <sup>2</sup>*<sup>γ</sup>*

2(*ω<sup>e</sup>* + *λ*˙ ) cos *φ* 2*φ*˙ 0

⎞ ⎠ =

Fundamentals of GNSS-Aided Inertial Navigation 23

where *Re* is the mean Earth radius. The last term is the transformed accelerometer biases.

⎛ ⎝

cos *<sup>φ</sup>* 0 0

*Re*

*<sup>M</sup>*+*<sup>h</sup>* <sup>−</sup>*φ*˙

⎞ ⎠

⎞

⎟⎟⎠

*δ***r** *l* +

cos *<sup>φ</sup>* 0 0

*<sup>N</sup>*+*<sup>h</sup>* (2*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) sin *<sup>φ</sup>* <sup>−</sup>(2*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) cos *<sup>φ</sup>*

*R*<sup>11</sup> *R*<sup>12</sup> *R*<sup>13</sup> *R*<sup>21</sup> *R*<sup>22</sup> *R*<sup>23</sup> *R*<sup>31</sup> *R*<sup>32</sup> *R*<sup>33</sup>

� �� � **F**<sup>25</sup>

<sup>0</sup> (*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) sin *<sup>φ</sup>* <sup>−</sup>(*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) cos *<sup>φ</sup>*

⎞

⎟⎟⎠

*δ***r** *l* +

*R*<sup>11</sup> *R*<sup>12</sup> *R*<sup>13</sup> *R*<sup>21</sup> *R*<sup>22</sup> *R*<sup>23</sup> *R*<sup>31</sup> *R*<sup>32</sup> *R*<sup>33</sup>

� �� � **F**<sup>34</sup>

⎞ ⎠

<sup>−</sup>(*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ )*sin<sup>φ</sup>* <sup>0</sup> <sup>−</sup>*φ*˙ (*ω<sup>e</sup>* + *λ*˙ ) cos *φ φ*˙ 0

0 0 *<sup>φ</sup>*˙

<sup>−</sup>*ω<sup>e</sup>* sin *<sup>φ</sup>* <sup>0</sup> <sup>−</sup>*λ*˙ cos *<sup>φ</sup>*

� �� � **F**<sup>31</sup>

⎞

⎟⎠

*<sup>ω</sup><sup>e</sup>* cos *<sup>φ</sup>* <sup>+</sup> *<sup>λ</sup>*˙

0 <sup>−</sup><sup>1</sup> *<sup>M</sup>*+*<sup>h</sup>* 0

� �� � **F**<sup>32</sup>

1 *<sup>N</sup>*+*<sup>h</sup>* 0 0 tan *φ <sup>N</sup>*+*<sup>h</sup>* 0 0

� �� � **F**<sup>33</sup>

*M*+*h*

*N*+*h*

⎛ ⎝

cos *<sup>φ</sup>* <sup>0</sup> <sup>−</sup>*λ*˙ sin *<sup>φ</sup> N*+*h*

*δ***v***<sup>l</sup>* +

*h*

� �� � **F**<sup>22</sup>

0 0 2*γ Reδh* ⎞

⎠ (96)

⎞

⎟⎠

⎞ ⎠

�*l* +

**d** (98)

*δ***v***l* +

**b** (97)

#### **3.6.6 Matrix formulation of position errors**

Assuming *M* and *N* to be constant over small distances

$$\begin{aligned} \delta \mathbf{v}^l &= \mathbf{D} \delta \dot{\mathbf{r}}^l + \mathbf{D}\_l \delta \mathbf{r}^l \\ &= \begin{pmatrix} 0 & (N+h)\cos\phi \ 0 \\ M+h & 0 & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} \delta \dot{\phi} \\ \delta \dot{\lambda} \\ \delta h \end{pmatrix} + \begin{pmatrix} -\dot{\lambda}(N+h)\sin\phi \ 0 \ \dot{\lambda}\cos\phi \\ 0 & 0 \\ 0 & 0 \end{pmatrix} \begin{pmatrix} \delta \phi \\ \delta \lambda \\ \delta h \end{pmatrix} \end{aligned} \tag{90}$$

With **D***<sup>r</sup>* in hand,

$$
\delta\dot{\mathbf{r}}^l = \underbrace{\begin{pmatrix} 0 & \frac{1}{M+h} & 0 \\ \frac{1}{(N+h)} \cos \phi & 0 & 0 \\ 0 & 0 & 1 \end{pmatrix}}\_{\mathbf{F}\_{12}} \begin{pmatrix} \delta v\_\varepsilon \\ \delta v\_n \end{pmatrix} - \underbrace{\begin{pmatrix} 0 & 0 & \frac{-\dot{\phi}}{M+h} \\ \dot{\lambda} \tan \phi & 0 & \frac{-\dot{\lambda}}{N+h} \\ 0 & 0 & 0 \end{pmatrix}}\_{\mathbf{F}\_{11}} \begin{pmatrix} \delta \phi \\ \delta h \end{pmatrix} \tag{91}
$$

where **F**<sup>11</sup> and **F**<sup>12</sup> are the first two 3×3 sub-matrices of **F**.

#### **3.6.7 Matrix formulation of velocity errors**

Next, we turn to the first term in the second equation of (89):

$$-\mathbf{F}^{l}\boldsymbol{\epsilon}^{l} = \underbrace{\begin{pmatrix} 0 & f\_{\mu} & -f\_{n} \\ -f\_{\mu} & 0 & f\_{\mathcal{E}} \\ f\_{n} & -f\_{\mathcal{E}} & 0 \end{pmatrix}}\_{\mathbf{F}\_{23}} \begin{pmatrix} \varepsilon\_{\mathcal{E}} \\ \varepsilon\_{\mathcal{U}} \\ \varepsilon\_{\mu} \end{pmatrix} \tag{92}$$

The second term is

$$-(2\boldsymbol{\Omega}\_{i\varepsilon}^{l} + \boldsymbol{\Omega}\_{el}^{l})\delta\mathbf{v}^{l} = \begin{pmatrix} 0 & (2\boldsymbol{\omega}\_{\varepsilon} + \dot{\lambda})\sin\phi & -(2\boldsymbol{\omega}\_{\varepsilon} + \dot{\lambda})\cos\phi\\ -(2\boldsymbol{\omega}\_{\varepsilon} + \dot{\lambda})\sin\phi & 0 & -\dot{\phi}\\ (2\boldsymbol{\omega}\_{\varepsilon} + \dot{\lambda})\cos\phi & \dot{\phi} & 0 \end{pmatrix} \begin{pmatrix} \delta v\_{\varepsilon} \\ \delta v\_{\eta} \\ \delta v\_{u} \end{pmatrix} \tag{93}$$

The third term is as follows:

$$\begin{aligned} \begin{pmatrix} 2\delta\omega^{l}\_{\ell\varepsilon} + \delta\omega^{l}\_{\varepsilon l} = \begin{pmatrix} -\delta\phi\\ -(2\omega\_{\varepsilon} + \dot{\lambda})\sin\phi\delta\phi + \cos\phi\delta\dot{\lambda} \\ (2\omega\_{\varepsilon} + \dot{\lambda})\cos\phi\delta\phi + \sin\phi\delta\dot{\lambda} \end{pmatrix} \\ = \begin{pmatrix} 0 & 0 & \frac{\dot{\phi}}{M+h} \\\ -2\omega\_{\varepsilon}\sin\phi & 0 & \frac{-\lambda\cos\phi}{N+h} \\\ 2\omega\_{\varepsilon}\cos\phi + \frac{\dot{\lambda}}{\cos\phi} & 0 & \frac{-\lambda\sin\phi}{N+h} \end{pmatrix} \begin{pmatrix} \delta\phi\\ \delta\lambda \end{pmatrix} + \begin{pmatrix} 0 & \frac{-1}{M+h} & 0 \\ \frac{1}{N+h} & 0 & 0 \\\ \frac{\sin\phi}{N+h} & 0 & 0 \end{pmatrix} \begin{pmatrix} \delta v\_{\varepsilon} \\ \delta v\_{\imath} \end{pmatrix} \end{aligned} \tag{94}$$

Multiplying by **V***<sup>l</sup>* yields

$$\begin{aligned} \mathbf{V}^{l}(2\delta\omega\_{\rm it}^{l}+\delta\omega\_{\rm el}^{l}) &= \\ \begin{pmatrix} 2\omega\_{\varepsilon}(v\_{\rm u}\sin\phi+v\_{\rm u}\cos\phi)+v\_{\rm u}\frac{\lambda}{\cos\phi} & 0 \\ -2\omega\_{\varepsilon}v\_{\varepsilon}\cos\phi-\frac{v\_{\rm u}\lambda}{\cos\phi} & 0 \\ -2\omega\_{\varepsilon}v\_{\varepsilon}\sin\phi & 0 \end{pmatrix} \begin{pmatrix} \delta\phi\\ \delta\lambda\\ \delta\hbar \end{pmatrix} + \begin{pmatrix} \frac{-v\_{\rm u}+v\_{\rm u}\tan\phi}{N+h} & 0 & 0\\ \frac{-v\_{\rm u}\tan\phi}{N+h} & \frac{-v\_{\rm u}}{M+h} & 0\\ \frac{v\_{\rm e}}{N+h} & \frac{v\_{\rm e}}{M+h} & 0 \end{pmatrix} \begin{pmatrix} \delta v\_{\rm e}\\ \delta v\_{\rm u} \end{pmatrix} \end{aligned} \tag{95}$$

The fourth term is

20 Will-be-set-by-IN-TECH

**3.6.6 Matrix formulation of position errors**

*<sup>l</sup>* + **D***rδ***r** *l*

⎛

⎜⎝

**3.6.7 Matrix formulation of velocity errors**

*δ***v***<sup>l</sup>* = **D***δ***r**˙

= ⎛ ⎝

With **D***<sup>r</sup>* in hand,

The second term is

*ie* <sup>+</sup> **<sup>Ω</sup>***<sup>l</sup>*

*ie* <sup>+</sup> *<sup>δ</sup>*ω*<sup>l</sup>*

Multiplying by **V***<sup>l</sup>* yields

*ie* <sup>+</sup> *<sup>δ</sup>*ω*<sup>l</sup>*

The third term is as follows:

*el*)*δ***v***<sup>l</sup>* <sup>=</sup>

⎛ ⎝

⎛

⎜⎜⎝

*el* =

=

*el*) =

<sup>2</sup>*ωe*(*vu* sin *<sup>φ</sup>* <sup>+</sup> *vn* cos *<sup>φ</sup>*) + *vn <sup>λ</sup>*˙

<sup>−</sup>2*ωeve* cos *<sup>φ</sup>* <sup>−</sup> *veλ*˙

⎛ ⎝

<sup>−</sup>(2**Ω***<sup>l</sup>*

2*δ*ω*<sup>l</sup>*

**V***l* (2*δ*ω*<sup>l</sup>*

⎛

⎜⎝

*δ***r**˙ *<sup>l</sup>* =

Assuming *M* and *N* to be constant over small distances

⎞ ⎠

*<sup>M</sup>*+*<sup>h</sup>* 0

⎞

⎛ ⎝ *δve δvn δvu*

⎟⎠

⎛ ⎝

<sup>−</sup>*δφ*˙ <sup>−</sup>(2*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) sin *φδφ* <sup>+</sup> cos *φδλ*˙ (2*ω<sup>e</sup>* + *λ*˙ ) cos *φδφ* + sin *φδλ*˙

0 0 *<sup>φ</sup>*˙

<sup>−</sup>2*ω<sup>e</sup>* sin *<sup>φ</sup>* <sup>0</sup> <sup>−</sup>*λ*˙ cos *<sup>φ</sup>*

*cos<sup>φ</sup>* 0 0

cos *<sup>φ</sup>* 0 0

−2*ωeve* sin *φ* 0 0

<sup>2</sup>*ω<sup>e</sup>* cos *<sup>φ</sup>* <sup>+</sup> *<sup>λ</sup>*˙

0 <sup>1</sup>

(*N*+*h*) cos *φ* 0 0 0 01

� �� � **F**<sup>12</sup>

where **F**<sup>11</sup> and **F**<sup>12</sup> are the first two 3×3 sub-matrices of **F**.

Next, we turn to the first term in the second equation of (89):

<sup>−</sup>**F***<sup>l</sup>* �*<sup>l</sup>* =

⎛ ⎝ *δφ*˙ *δλ*˙ *δ* ˙ *h*

⎞ ⎠ + ⎛ ⎝

> ⎞ ⎠ −

0 *fu* −*fn* −*fu* 0 *fe fn* −*fe* 0

� �� � **F**<sup>23</sup>

<sup>−</sup>(2*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) sin *<sup>φ</sup>* <sup>0</sup> <sup>−</sup>*φ*˙ (2*ω<sup>e</sup>* + *λ*˙ ) cos *φ φ*˙ 0

*M*+*h*

*N*+*h*

cos *<sup>φ</sup>* <sup>0</sup> <sup>−</sup>*λ*˙ sin *<sup>φ</sup> N*+*h*

⎞

⎛ ⎝ *δφ δλ δh*

⎞ ⎠ + ⎛

⎜⎝

⎟⎠

⎛

⎜⎝

⎞ ⎠

<sup>0</sup> (2*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) sin *<sup>φ</sup>* <sup>−</sup>(2*ω<sup>e</sup>* <sup>+</sup> *<sup>λ</sup>*˙ ) cos *<sup>φ</sup>*

⎞ ⎠

⎛ ⎝ *δφ δλ δh*

⎞ ⎠ +

−*vu*+*vn* tan *φ*

−*ve* tan *φ N*+*h*

*N*+*h*

⎛

0 <sup>−</sup><sup>1</sup> *<sup>M</sup>*+*<sup>h</sup>* 0

1 *<sup>N</sup>*+*<sup>h</sup>* 0 0 tan *φ <sup>N</sup>*+*<sup>h</sup>* 0 0

*<sup>N</sup>*+*<sup>h</sup>* 0 0

−*vu <sup>M</sup>*+*<sup>h</sup>* <sup>0</sup> *ve*

⎞

⎛ ⎝ *δve δvn δvu* ⎞

⎟⎠

*vn <sup>M</sup>*+*<sup>h</sup>* 0

⎜⎝

⎞

⎟⎟⎠

⎛ ⎝ *�e �n �u* ⎞

<sup>−</sup>*λ*˙ (*<sup>N</sup>* <sup>+</sup> *<sup>h</sup>*) sin *<sup>φ</sup>* <sup>0</sup> *<sup>λ</sup>*˙ cos *<sup>φ</sup>* 0 0 *φ*˙ 0 00

> 0 0 <sup>−</sup>*φ*˙ *M*+*h*

*N*+*h* 0 00

⎞

⎛ ⎝ *δφ δλ δh*

⎟⎠

*<sup>λ</sup>*˙ tan *<sup>φ</sup>* <sup>0</sup> <sup>−</sup>*λ*˙

� �� � **F**<sup>11</sup>

⎞ ⎠ ⎛ ⎝ *δφ δλ δh*

⎞

⎠ (92)

⎞ ⎠

⎞

⎛ ⎝ *δve δvn δvu* ⎞

⎠ (94)

⎠ (95)

⎟⎠

⎛ ⎝ *δve δvn δvu* ⎞

⎠ (93)

⎞

⎠ (91)

⎠ (90)

0 (*N* + *h*) cos *φ* 0 *M* + *h* 0 0 0 01

1

$$
\delta\gamma^l = \begin{pmatrix} 0 \\ 0 \\ -\frac{\partial\gamma}{\partial h}\delta h \end{pmatrix} = \begin{pmatrix} 0 \\ 0 \\ \frac{2\gamma}{R\_e\delta h} \end{pmatrix} \tag{96}
$$

where *Re* is the mean Earth radius. The last term is the transformed accelerometer biases. Combining all the terms, we have

$$
\delta\dot{\mathbf{v}}^{\dot{\lambda}} = \underbrace{\begin{pmatrix} 2\omega\_{\ell}(v\_{u}\sin\phi + v\_{l}\cos\phi) + \frac{v\_{r}\dot{\lambda}}{\cos\phi}\Big{0} & 0\\ -2\omega\_{\ell}v\_{v}\cos\phi - \frac{v\_{r}\dot{\lambda}}{\cos\phi} & 0 & 0\\ -2\omega\_{\ell}v\_{\ell}\sin\phi & 0 & \frac{2\gamma}{R\_{\ell}} \end{pmatrix}}\_{\mathbf{F}\_{21}} \delta\dot{\mathbf{v}}^{\dot{\lambda}} + \\\\ \underbrace{\begin{pmatrix} \frac{-\dot{h} + \dot{\phi}\tan\phi(M+h)}{N+h} & (2\omega\_{\ell} + \dot{\lambda})\sin\phi - (2\omega\_{\ell} + \dot{\lambda})\cos\phi\\ -2(\omega\_{\ell} + \dot{\lambda})\sin\phi & \frac{-h}{M+h} & -\dot{\phi} \\ 2(\omega\_{\ell} + \dot{\lambda})\cos\phi & 2\dot{\phi} & 0 \end{pmatrix}}\_{\mathbf{F}\_{22}} \delta\mathbf{v}^{\dot{\lambda}} + \\\\ \underbrace{\begin{pmatrix} 0 & f\_{u} & -f\_{n} \\ -f\_{u} & 0 & f\_{\ell} \\ f\_{n} & -f\_{\ell} & 0 \end{pmatrix}}\_{\mathbf{F}\_{23}} \epsilon^{\ell} + \underbrace{\begin{pmatrix} R\_{11} & R\_{12} & R\_{13} \\ R\_{21} & R\_{22} & R\_{23} \\ R\_{31} & R\_{32} & R\_{33} \end{pmatrix}}\_{\mathbf{F}\_{25}} \mathbf{b} \end{pmatrix} \tag{97}$$

#### **3.6.8 Matrix formulation of alignment errors**

The third equation in (89) can be derived similarly as

$$
\boldsymbol{\epsilon}^{I} = \underbrace{\begin{pmatrix} 0 & (\omega\_{\varepsilon} + \dot{\lambda})\sin\phi & -(\omega\_{\varepsilon} + \dot{\lambda})\cos\phi \\ -(\omega\_{\varepsilon} + \dot{\lambda})\sin\phi & 0 & -\dot{\phi} \\ (\omega\_{\varepsilon} + \dot{\lambda})\cos\phi & \dot{\phi} & 0 \end{pmatrix}}\_{\mathbf{F}\_{33}} \boldsymbol{\epsilon}^{I} + \\ \underbrace{\begin{pmatrix} 0 & 0 & \frac{\dot{\phi}}{M+h} \\ -\omega\_{\varepsilon}\sin\phi & 0 & \frac{-\dot{\lambda}\cos\phi}{N+h} \\ \omega\_{\varepsilon}\cos\phi + \frac{\dot{\lambda}}{\cos\phi} & 0 & \frac{-\dot{\lambda}\sin\phi}{N+h} \end{pmatrix}}\_{\mathbf{F}\_{31}} \delta\mathbf{r}^{I} + \\ \underbrace{\begin{pmatrix} 0 & \frac{-1}{M+h} & 0 \\ \frac{1}{N+h} & 0 & 0 \\ \frac{1}{N+h} & 0 & 0 \end{pmatrix}}\_{\mathbf{F}\_{32}} \delta\mathbf{v}^{I} + \underbrace{\begin{pmatrix} R\_{11} & R\_{12} & R\_{13} \\ R\_{21} & R\_{22} & R\_{23} \\ R\_{31} & R\_{32} & R\_{33} \end{pmatrix}}\_{\mathbf{F}\_{34}} \mathbf{d} \tag{98}
$$

and is called the Schüler oscillation, after Maximilian Schüler who showed that the bob of a hypothetical pendulum whose string was the length of the Earth's radius would not be displaced under sudden motions of its support. The period of such a pendulum (and of the Schüler oscillation) is 84.4 minutes. This implies that positional errors caused by either accelerometer bias or initial velocity errors are bounded over this period. On the other hand,

Fundamentals of GNSS-Aided Inertial Navigation 25

Characterization of INS errors in each channel (East, North, Up) can be performed analytically in the case of a level platform traveling at a constant velocity and height where there is no coupling between them. For example, under the conditions stated above, a derivation of error propagation for the North channel proceeds from formulating an error dynamics matrix composed only of terms affecting the position, velocity and error states relating to it and

where <sup>L</sup>−<sup>1</sup> denotes the inverse Laplace transform. The effect of a particular error source upon the error state under investigation is simply the term in **Φ**(*t*) whose row index corresponds to the index of the state whose column index corresponds to the index of the error source. For

where *ω*<sup>0</sup> is the Schüler frequency. Because the errors in position due to gyro drift and misalignment are unbounded, as previously mentioned, the largest single quantity of merit in the sensor specifications of an IMU is in the gyro drift rate. We finish by noting that for more general trajectories, characterization of error propagation is best done through simulation.

IMU measurements are made from two triads of orthogonally-mounted accelerometers and gyros; one sensor for each degree of freedom in three-dimensional space. Accelerometers measure *specific force* along a sensitive axis. Gyroscopes measure either rotations or rotation *rates* along a sensitive axis. Although the current state of the art in sensor design makes use of the principles of lasers and quantum mechanics, Newtonian mechanics gives us the tools to design both accelerometers and gyroscopes. Presented below is a brief discussion of the basis

**F***<sup>i</sup>* = *m***r**¨

where *i* represents the inertial frame, expresses the fact that force is proportional to the acceleration of a constant proof mass. Conversely, the force needed to keep this mass from accelerating is a measure of linear acceleration, a principle employed in most accelerometers.

It can be seen as a realization of the law of conservation of linear momentum:

sin *ω*0*t ω*0

example, the effect of a constant velocity error *δv* upon the North position is

*δrn*(*t*) =*δv*

**<sup>Φ</sup>**(*t*) =L−1{(*s***<sup>I</sup>** <sup>−</sup> **<sup>F</sup>**)−1} (104)

*<sup>i</sup>* (106)

(105)

positional errors due to misalignment or gyro drift are not bounded.

deriving the state transition matrix **Φ** by

**4. Sensors**

of such classical designs.

Recalling Newton's second law:

**4.1 Accelerometers**

#### **3.6.9 Matrix formulation of sensor errors**

The matrices associated with the gyro drift and accelerometer bias equations in (89) are diagonal, given as

$$\mathbf{F}\_{44} = \begin{pmatrix} -\alpha\_{\mathcal{X}} & 0 & 0 \\ 0 & -\alpha\_{\mathcal{Y}} & 0 \\ 0 & 0 & -\alpha\_{z} \end{pmatrix} \tag{99}$$

$$\mathbf{F}\_{55} = \begin{pmatrix} -\beta\_x & 0 & 0 \\ 0 & -\beta\_y & 0 \\ 0 & 0 & -\beta\_z \end{pmatrix} \tag{100}$$

where the *α* and *β* terms are the reciprocals of the time constants associated with the first-order Gauss-Markov model of each sensor.

Finally, we can define the error dynamics matrix **F** in terms of the sub-matrices derived above as

$$\mathbf{F} = \begin{pmatrix} \mathbf{F}\_{11} & \mathbf{F}\_{12} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{F}\_{21} & \mathbf{F}\_{22} & \mathbf{F}\_{23} & \mathbf{0} & \mathbf{F}\_{25} \\ \mathbf{F}\_{32} & \mathbf{F}\_{32} & \mathbf{F}\_{33} & \mathbf{F}\_{34} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{F}\_{44} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{F}\_{55} \end{pmatrix} \tag{101}$$

#### **3.7 Error analysis and Schüler oscillation**

In figure 7, it is shown that the rotation rate of the *l*-frame with respect to the *i*-frame expressed as a vector in the *b*-frame is subtracted from the raw gyroscope measurements when the system is to be mechanized in the *l* frame. The relative rotations between the frames, or the transport rate, itself is a function of the computed velocity and misalignment between the *l* and *e* frames. If there is an error in the computed transformation **R***<sup>e</sup> <sup>l</sup>* or in the initial values in **R***<sup>b</sup> <sup>l</sup>* , the computation of **<sup>R</sup>***<sup>l</sup> <sup>b</sup>* will be in error. In the simple case that the vehicle is actually perfectly level and either stationary or at a constant velocity, but the computed value of **R***<sup>l</sup> <sup>b</sup>* indicates that it is not level, a component of the gravity vector will be resolved in the horizontal axes of the system. This component is integrated and provides an erroneous velocity value, which is fed back to compute *ω<sup>l</sup> il*, which, in turn is transformed through **<sup>R</sup>***<sup>b</sup> l* and is subtracted from the angular rate measurements. Finally, a second integration occurs using the "corrected" measurements to update **R***<sup>l</sup> <sup>b</sup>* and the process repeats.

The dynamics of the system described above are described by the characteristic equation in the Laplace domain as

$$s^2 + \frac{\mathcal{g}}{R\_\varepsilon} = 0\tag{102}$$

where *g* is gravity and *Re* is the mean radius of the earth. This represents a simple oscillation whose natural frequency is

$$
\omega\_0 = \sqrt{\frac{\mathcal{g}}{R\_\varepsilon}}\tag{103}
$$

22 Will-be-set-by-IN-TECH

The matrices associated with the gyro drift and accelerometer bias equations in (89) are

−*α<sup>x</sup>* 0 0 0 −*α<sup>y</sup>* 0 0 0 −*α<sup>z</sup>*

−*β<sup>x</sup>* 0 0 0 −*β<sup>y</sup>* 0 0 0 −*β<sup>z</sup>*

where the *α* and *β* terms are the reciprocals of the time constants associated with the first-order

Finally, we can define the error dynamics matrix **F** in terms of the sub-matrices derived above

In figure 7, it is shown that the rotation rate of the *l*-frame with respect to the *i*-frame expressed as a vector in the *b*-frame is subtracted from the raw gyroscope measurements when the system is to be mechanized in the *l* frame. The relative rotations between the frames, or the transport rate, itself is a function of the computed velocity and misalignment between

is actually perfectly level and either stationary or at a constant velocity, but the computed

the horizontal axes of the system. This component is integrated and provides an erroneous

and is subtracted from the angular rate measurements. Finally, a second integration occurs

The dynamics of the system described above are described by the characteristic equation in

where *g* is gravity and *Re* is the mean radius of the earth. This represents a simple oscillation

� *g Re*

*ω*<sup>0</sup> =

*s* <sup>2</sup> <sup>+</sup> *<sup>g</sup> Re*

*<sup>b</sup>* indicates that it is not level, a component of the gravity vector will be resolved in

**F**<sup>11</sup> **F**<sup>12</sup> **000 F**<sup>21</sup> **F**<sup>22</sup> **F**<sup>23</sup> **0 F**<sup>25</sup> **F**<sup>32</sup> **F**<sup>32</sup> **F**<sup>33</sup> **F**<sup>34</sup> **0 0 0 0F**<sup>44</sup> **0 0 0 0 0F**<sup>55</sup>

⎞

⎞

⎞

⎟⎟⎟⎟⎠

*<sup>b</sup>* will be in error. In the simple case that the vehicle

*<sup>b</sup>* and the process repeats.

*il*, which, in turn is transformed through **<sup>R</sup>***<sup>b</sup>*

= 0 (102)

⎠ (99)

⎠ (100)

(101)

*l*

(103)

*<sup>l</sup>* or in the initial

**F**<sup>44</sup> =

**F**<sup>55</sup> =

**F** =

⎛

⎜⎜⎜⎜⎝

the *l* and *e* frames. If there is an error in the computed transformation **R***<sup>e</sup>*

⎛ ⎝

⎛ ⎝

**3.6.9 Matrix formulation of sensor errors**

Gauss-Markov model of each sensor.

**3.7 Error analysis and Schüler oscillation**

*<sup>l</sup>* , the computation of **<sup>R</sup>***<sup>l</sup>*

velocity value, which is fed back to compute *ω<sup>l</sup>*

using the "corrected" measurements to update **R***<sup>l</sup>*

diagonal, given as

as

values in **R***<sup>b</sup>*

value of **R***<sup>l</sup>*

the Laplace domain as

whose natural frequency is

and is called the Schüler oscillation, after Maximilian Schüler who showed that the bob of a hypothetical pendulum whose string was the length of the Earth's radius would not be displaced under sudden motions of its support. The period of such a pendulum (and of the Schüler oscillation) is 84.4 minutes. This implies that positional errors caused by either accelerometer bias or initial velocity errors are bounded over this period. On the other hand, positional errors due to misalignment or gyro drift are not bounded.

Characterization of INS errors in each channel (East, North, Up) can be performed analytically in the case of a level platform traveling at a constant velocity and height where there is no coupling between them. For example, under the conditions stated above, a derivation of error propagation for the North channel proceeds from formulating an error dynamics matrix composed only of terms affecting the position, velocity and error states relating to it and deriving the state transition matrix **Φ** by

$$\Phi(t) = \mathcal{L}^{-1}\{(s\mathbf{I} - \mathbf{F})^{-1}\}\tag{104}$$

where <sup>L</sup>−<sup>1</sup> denotes the inverse Laplace transform. The effect of a particular error source upon the error state under investigation is simply the term in **Φ**(*t*) whose row index corresponds to the index of the state whose column index corresponds to the index of the error source. For example, the effect of a constant velocity error *δv* upon the North position is

$$
\delta r\_n(t) = \delta v \frac{\sin \omega\_0 t}{\omega\_0} \tag{105}
$$

where *ω*<sup>0</sup> is the Schüler frequency. Because the errors in position due to gyro drift and misalignment are unbounded, as previously mentioned, the largest single quantity of merit in the sensor specifications of an IMU is in the gyro drift rate. We finish by noting that for more general trajectories, characterization of error propagation is best done through simulation.

## **4. Sensors**

IMU measurements are made from two triads of orthogonally-mounted accelerometers and gyros; one sensor for each degree of freedom in three-dimensional space. Accelerometers measure *specific force* along a sensitive axis. Gyroscopes measure either rotations or rotation *rates* along a sensitive axis. Although the current state of the art in sensor design makes use of the principles of lasers and quantum mechanics, Newtonian mechanics gives us the tools to design both accelerometers and gyroscopes. Presented below is a brief discussion of the basis of such classical designs.

#### **4.1 Accelerometers**

Recalling Newton's second law:

$$\mathbf{F}^{\dot{i}} = m\ddot{\mathbf{r}}^{\dot{i}}\tag{106}$$

where *i* represents the inertial frame, expresses the fact that force is proportional to the acceleration of a constant proof mass. Conversely, the force needed to keep this mass from accelerating is a measure of linear acceleration, a principle employed in most accelerometers. It can be seen as a realization of the law of conservation of linear momentum:

where *c* is the damping or viscosity constant. If we assume that the sensor is located on the Earth with the *x* axis facing opposite the direction of the pull of gravity, Newton's second law

Fundamentals of GNSS-Aided Inertial Navigation 27

where *g* is present to account for the reaction force of the case against the surface of the Earth.

This system will therefore have an output of *g* − *r*¨0 = *f* , which is the **specific force**. This is the observable obtained from accelerometers near the surface of Earth. In this case, without extra

It is not possible to separate the effects of inertia and gravity in a non-inertial frame, a consequence of Einstein's equivalence principle. In other words, forces applied to an accelerometer through accelerations of the vehicle are indistinguishable from the acceleration caused by the gravity field of the planet. Without knowledge of the vehicle's acceleration at a particular time, it is not possible to measure the local gravitational vector and vice versa. The forces acting on the vehicle other than gravity include those induced by Earth's rotation, so we must be careful in how we eliminate instrument biases depending upon which reference

Equation (109) is an open-loop mechanization of the mass-spring-damper system, where the displacement is directly measured. Modern high-accuracy designs are by contrast closed-loop systems, where the mass is kept at the null position by a coil in a magnetic field. The force required to keep the mass stationary under various accelerations is then the quantity that is measured. Several other realizations of accelerometers are possible, but most are still modeled

Finally, we note that though the observable we shall deal with is specific force, the actual output of the sensor is change in velocity Δ*v*. This is a consequence of the internal mechanisms of modern accelerometers, where several measurements are integrated over a short period of time (usually a few milliseconds) to smooth out measurement noise. The general form of the measurement model of specific force from an accelerometer triad is given by the observation

**S**<sup>1</sup> and **S**<sup>2</sup> represent the linear and non-linear matrix of scale factor errors, respectively

*<sup>a</sup>* = **f+b** + (**S**<sup>1</sup> + **S**2)**f +Nf** + *γ* + *δ***g** + *�<sup>f</sup>* (112)

*x*¨*<sup>C</sup>* + *c m x*˙*<sup>C</sup>* + *k m*

*mr*¨*<sup>C</sup>* + *cx*˙*<sup>C</sup>* + *kxC* − *mg* = 0 (109)

*rC* = *xC* + *r*<sup>0</sup> (110)

*xC* = *g* − *r*¨0 (111)

gives the second-order differential equation

applied force, the output is simply *g*.

frame we are to work in.

equation

where

by similar differential equations.

*<sup>a</sup>* is the measurement **f** is the specific force

**b** is the accelerometer bias

Letting

we have

$$\mathbf{F}^{\dot{i}} = \dot{\mathbf{p}}^{\dot{i}} = m\ddot{\mathbf{r}}^{\dot{i}} = 0 \tag{107}$$

where **p** is the momentum of the proof mass, i.e. the rate of change of the momentum is equal to the applied force. The external forces acting on the system are balanced by internal forces, so the motion of the proof mass remains constant in an inertial frame. In theory there is a problem realizing such a sensor on Earth because the planet is undergoing constant acceleration in its orbit around the Sun and so forth. This makes defining zero acceleration impossible in an inertial frame, but we can simply treat any signal arising from these conditions as a constant instrument bias and remove it from the measurements. From here on, we will only consider this situation.

To see how (107) can be realized in a measurement device, consider the classical spring-mass-damper system shown in figure 8. A mass *m* is constrained to move along the

Fig. 8. a spring-mass-damper system

*x* axis of the device (the sensitive axis). It is restrained by a spring and its motion is damped by a damping device. Finally, there is a scale and a housing for the assembly. Point C is the center of mass of the sensitive element and point O indicates the equilibrium position when the device is not subjected to any external force along the sensitive axis. The output of the device is measured along the *r* scale, which is made proportional to the internal signal along *x*. The spring provides a restoring force proportional to the displacement of the proof mass by Hooke's Law:

$$F = -k\mathbf{x} \tag{108}$$

where *k* is the spring constant. The damper is present to minimize oscillations in response to sudden changes in applied force and can be made of a viscous fluid-filled piston or the like. The force produced by the damper is proportional to the velocity of the proof mass or −*cx*˙, where *c* is the damping or viscosity constant. If we assume that the sensor is located on the Earth with the *x* axis facing opposite the direction of the pull of gravity, Newton's second law gives the second-order differential equation

$$m\ddot{\mathbf{r}}\_{\mathbb{C}} + c\dot{\mathbf{x}}\_{\mathbb{C}} + k\mathbf{x}\_{\mathbb{C}} - mg = \mathbf{0} \tag{109}$$

where *g* is present to account for the reaction force of the case against the surface of the Earth. Letting

$$
\mathbf{r}\_{\mathbb{C}} = \mathbf{x}\_{\mathbb{C}} + \mathbf{r}\_0 \tag{110}
$$

we have

24 Will-be-set-by-IN-TECH

where **p** is the momentum of the proof mass, i.e. the rate of change of the momentum is equal to the applied force. The external forces acting on the system are balanced by internal forces, so the motion of the proof mass remains constant in an inertial frame. In theory there is a problem realizing such a sensor on Earth because the planet is undergoing constant acceleration in its orbit around the Sun and so forth. This makes defining zero acceleration impossible in an inertial frame, but we can simply treat any signal arising from these conditions as a constant instrument bias and remove it from the measurements. From here on, we will only consider

To see how (107) can be realized in a measurement device, consider the classical spring-mass-damper system shown in figure 8. A mass *m* is constrained to move along the

*x*

0

O O

Fig. 8. a spring-mass-damper system

by Hooke's Law:

*r*

*m*

C

*k c*

case

*F* = −*kx* (108)

*x* axis of the device (the sensitive axis). It is restrained by a spring and its motion is damped by a damping device. Finally, there is a scale and a housing for the assembly. Point C is the center of mass of the sensitive element and point O indicates the equilibrium position when the device is not subjected to any external force along the sensitive axis. The output of the device is measured along the *r* scale, which is made proportional to the internal signal along *x*. The spring provides a restoring force proportional to the displacement of the proof mass

where *k* is the spring constant. The damper is present to minimize oscillations in response to sudden changes in applied force and can be made of a viscous fluid-filled piston or the like. The force produced by the damper is proportional to the velocity of the proof mass or −*cx*˙,

*<sup>i</sup>* = 0 (107)

**F***<sup>i</sup>* = **p**˙ *<sup>i</sup>* = *m***r**¨

this situation.

$$
\ddot{\mathbf{x}}\_{\mathbb{C}} + \frac{c}{m}\dot{\mathbf{x}}\_{\mathbb{C}} + \frac{k}{m}\mathbf{x}\_{\mathbb{C}} = \mathbf{g} - \ddot{r}\_{0} \tag{111}
$$

This system will therefore have an output of *g* − *r*¨0 = *f* , which is the **specific force**. This is the observable obtained from accelerometers near the surface of Earth. In this case, without extra applied force, the output is simply *g*.

It is not possible to separate the effects of inertia and gravity in a non-inertial frame, a consequence of Einstein's equivalence principle. In other words, forces applied to an accelerometer through accelerations of the vehicle are indistinguishable from the acceleration caused by the gravity field of the planet. Without knowledge of the vehicle's acceleration at a particular time, it is not possible to measure the local gravitational vector and vice versa. The forces acting on the vehicle other than gravity include those induced by Earth's rotation, so we must be careful in how we eliminate instrument biases depending upon which reference frame we are to work in.

Equation (109) is an open-loop mechanization of the mass-spring-damper system, where the displacement is directly measured. Modern high-accuracy designs are by contrast closed-loop systems, where the mass is kept at the null position by a coil in a magnetic field. The force required to keep the mass stationary under various accelerations is then the quantity that is measured. Several other realizations of accelerometers are possible, but most are still modeled by similar differential equations.

Finally, we note that though the observable we shall deal with is specific force, the actual output of the sensor is change in velocity Δ*v*. This is a consequence of the internal mechanisms of modern accelerometers, where several measurements are integrated over a short period of time (usually a few milliseconds) to smooth out measurement noise. The general form of the measurement model of specific force from an accelerometer triad is given by the observation equation

$$\ell\_a = \mathbf{f} + \mathbf{b} + (\mathbf{S}\_1 + \mathbf{S}\_2)\mathbf{f} + \mathbf{N}\mathbf{f} + \gamma + \delta\mathbf{g} + \epsilon\_f \tag{112}$$

where

*<sup>a</sup>* is the measurement

**f** is the specific force

**b** is the accelerometer bias

**S**<sup>1</sup> and **S**<sup>2</sup> represent the linear and non-linear matrix of scale factor errors, respectively

For a particle moving in a central field (*i.e.* any point we chose on the disc), **F** and **r** are parallel and thus **L** is constant. This means that the direction of the spin axis of the rotating disc is fixed in inertial space. In a two axis gyroscope any rotation *ω<sup>t</sup>* about *t* (the input axis) in figure 10 would give rise to a rotation *ω<sup>p</sup>* about *p* (the output axis). This phenomenon is known as precession. Measuring the torque about *p* leads us to the angular velocity about *t*, which is the observable under consideration. As with accelerometers, the actual

Fundamentals of GNSS-Aided Inertial Navigation 29

*s*

physical implementation of gyroscopes has taken on many forms, depending on purpose and performance considerations. In the example given above, measurements of gimbal rotation (in an open-loop system) are angular measurements. In a closed-loop system, motors are used to keep the gimbals from moving and the required torque to do so is measured. These measurements are therefore of the angular *rates* of the system. Sensing angular velocity in modern strap-down navigation systems is actually accomplished through exploiting the Sagnac effect rather than the mechanical properties of rotating masses. In this case, the interference patterns generated by light traveling along opposing closed paths is used as a measure of the angular rotation of the system. In any case, the measurements obtained from

*t*

*ωt*

*ωp*

*p*

*<sup>ω</sup>* = *ω* + *d* + **S***ω* + **N***ω* + *�ω* (118)

*�* = *�<sup>w</sup>* + *�<sup>c</sup>* + *�<sup>r</sup>* + *�<sup>q</sup>* + *�<sup>d</sup>* (119)

*s*

*ωs*

**F**,**r**

a gyroscope triad can modeled by the observation equation

**S** is a matrix representing the gyroscope scale factor

**N** is a matrix representing the non-orthogonality of the axes

The noise terms of both accelerometers and gyroscopes can be further decomposed as

the five terms representing white, correlated, random walk, quantization and dither noise, respectively. Some IMU errors associated with scale factor and non-orthogonality are characterized in the factory, while others, including bias and noise are removed by the

Fig. 10. gyroscopic precession

*<sup>ω</sup>* is the measurement *ω* is the angular velocity *d* is the gyroscope bias

*�ω* is noise

estimation process.

where

**N** is a matrix representing the non-orthogonality of the sensor axes


## **4.2 Gyroscopes**

Gyroscopes measure angular velocity with respect to an inertial reference frame. A schematic of a simple two-axis gyroscope is shown in figure 9. In this device, a spinning disc is mounted within a set of gimbals which allow it to pivot in response to an applied torque, a behavior known as **precession**.

#### Fig. 9. a two-axis rigid rotor gyroscope

We can analyze the behavior of this system beginning with Newton's second law in terms of momentum again:

$$\mathbf{F}^{\bar{l}} = \dot{\mathbf{p}}^{\bar{l}} \tag{113}$$

The cross product with the vector **r** gives the moment of this force about the origin, or

$$\mathbf{r}^i \times \mathbf{F}^i = \mathbf{r}^i \times \mathbf{\dot{p}}^i \tag{114}$$

Now, we observe that the angular momentum, **L** of the spinning disc is

$$\mathbf{L} = \mathbf{r}^{i} \times \mathbf{p}^{i} \tag{115}$$

the time derivative of which is

$$\dot{\mathbf{L}} = \dot{\mathbf{r}}^i \times \mathbf{p}^i + \mathbf{r}^i \times \dot{\mathbf{p}}^i \tag{116}$$

Now, because **r**˙ *<sup>i</sup>* and **p***<sup>i</sup>* are parallel, the first cross product on the right hand side of (116) is zero, thus

$$\mathbf{r}^i \times \mathbf{F}^i = \mathbf{L} \tag{117}$$

26 Will-be-set-by-IN-TECH

Gyroscopes measure angular velocity with respect to an inertial reference frame. A schematic of a simple two-axis gyroscope is shown in figure 9. In this device, a spinning disc is mounted within a set of gimbals which allow it to pivot in response to an applied torque, a behavior

spin axis

inner gimbal axis outer

Base

The cross product with the vector **r** gives the moment of this force about the origin, or

*<sup>i</sup>* <sup>×</sup> **<sup>F</sup>***<sup>i</sup>* <sup>=</sup> **<sup>r</sup>**

**L** = **r**

*<sup>i</sup>* <sup>×</sup> **<sup>p</sup>***<sup>i</sup>* <sup>+</sup> **<sup>r</sup>**

*<sup>i</sup>* and **p***<sup>i</sup>* are parallel, the first cross product on the right hand side of (116) is

**r**

**L**˙ = **r**˙

**r**

Now, we observe that the angular momentum, **L** of the spinning disc is

We can analyze the behavior of this system beginning with Newton's second law in terms of

inner gimbal

gimbal axis

**F***<sup>i</sup>* = **p**˙ *<sup>i</sup>* (113)

*<sup>i</sup>* <sup>×</sup> **<sup>p</sup>**˙ *<sup>i</sup>* (114)

*<sup>i</sup>* <sup>×</sup> **<sup>p</sup>***<sup>i</sup>* (115)

*<sup>i</sup>* <sup>×</sup> **<sup>F</sup>***<sup>i</sup>* <sup>=</sup> **<sup>L</sup>**˙ (117)

*<sup>i</sup>* <sup>×</sup> **<sup>p</sup>**˙ *<sup>i</sup>* (116)

outer gimbal

**N** is a matrix representing the non-orthogonality of the sensor axes

*γ* is the vector of normal gravity *δ***g** is the anomalous gravity vector

Fig. 9. a two-axis rigid rotor gyroscope

the time derivative of which is

Now, because **r**˙

zero, thus

momentum again:

*�<sup>f</sup>* is noise

**4.2 Gyroscopes**

known as **precession**.

For a particle moving in a central field (*i.e.* any point we chose on the disc), **F** and **r** are parallel and thus **L** is constant. This means that the direction of the spin axis of the rotating disc is fixed in inertial space. In a two axis gyroscope any rotation *ω<sup>t</sup>* about *t* (the input axis) in figure 10 would give rise to a rotation *ω<sup>p</sup>* about *p* (the output axis). This phenomenon is known as precession. Measuring the torque about *p* leads us to the angular velocity about *t*, which is the observable under consideration. As with accelerometers, the actual

Fig. 10. gyroscopic precession

physical implementation of gyroscopes has taken on many forms, depending on purpose and performance considerations. In the example given above, measurements of gimbal rotation (in an open-loop system) are angular measurements. In a closed-loop system, motors are used to keep the gimbals from moving and the required torque to do so is measured. These measurements are therefore of the angular *rates* of the system. Sensing angular velocity in modern strap-down navigation systems is actually accomplished through exploiting the Sagnac effect rather than the mechanical properties of rotating masses. In this case, the interference patterns generated by light traveling along opposing closed paths is used as a measure of the angular rotation of the system. In any case, the measurements obtained from a gyroscope triad can modeled by the observation equation

$$\ell\_{\omega} = \omega + d + \mathbf{S}\omega + \mathbf{N}\omega + \varepsilon\_{\omega} \tag{118}$$

where

*<sup>ω</sup>* is the measurement

*ω* is the angular velocity

*d* is the gyroscope bias

**S** is a matrix representing the gyroscope scale factor

**N** is a matrix representing the non-orthogonality of the axes

*�ω* is noise

The noise terms of both accelerometers and gyroscopes can be further decomposed as

$$
\epsilon = \epsilon\_w + \epsilon\_\mathfrak{c} + \mathfrak{e}\_r + \mathfrak{e}\_q + \mathfrak{e}\_d \tag{119}
$$

the five terms representing white, correlated, random walk, quantization and dither noise, respectively. Some IMU errors associated with scale factor and non-orthogonality are characterized in the factory, while others, including bias and noise are removed by the estimation process.

Initially, there will, of course, be no prior states or measurements, so we define the initial prior as *p*(**x**0), which is simply our best estimate given what we know generally about such

Fundamentals of GNSS-Aided Inertial Navigation 31

Linear filtering attempts to find the optimal linear combination of the predicted state and the the state implied by the measurements. In recursive form, a linear filter can be written as

the assumption of Gaussian noise, the resulting pdf of the estimate will also be Gaussian. The Kalman filter, which we shall describe next, is the optimal estimator under these

The problem as described in its general form in section 5.1 is rarely analytically tractable in practice. For one thing, state vectors may be very large, having high- (or infinite-) dimensional pdfs which cannot be integrated in the denominator of (125); the well-known "curse of dimensionality"(Duda et al., 2001). Secondly, the nonlinear models themselves may be unavailable or too complex to deal with analytically. For this reason, the error dynamics in 3.6 have been approximated to a linear system. We have also made many linearizing assumptions in the state equations such as the assumption of fast sampling rates relative to vehicle dynamics. We have also made the assumption of Gaussian-distributed noise in the sensors and shall further assume that disturbances to the error dynamics model take the same

Under these conditions, that is linearity and Gaussianity, a realizable solution to the Bayesian estimation problem is the Kalman filter, first described by Rudolph Kalman in 1960. Let the

where **<sup>Φ</sup>***k*,*k*−<sup>1</sup> is the state transition matrix, **<sup>w</sup>***k*−<sup>1</sup> is the zero-mean process noise with shaping matrix **<sup>G</sup>***k*−1, **<sup>H</sup>***<sup>k</sup>* is the design matrix mapping state parameters to measurements in **<sup>z</sup>***<sup>k</sup>* and **<sup>v</sup>***<sup>k</sup>* is zero-mean measurement noise (uncorrelated with process noise). **<sup>Φ</sup>***k*,*k*−<sup>1</sup> and **<sup>H</sup>** are linear functions, so **x***<sup>k</sup>* and **z***<sup>k</sup>* are Gaussian random vectors whose pdfs are completely described by their means and covariances. In addition to the requirements of (122) and (123), we have for

*<sup>E</sup>*{**x**0**w***<sup>T</sup>*

*<sup>E</sup>*{**x**0**v***<sup>T</sup>*

We seek an efficient estimator (if it exists), which is to say it is unbiased and its covariance attains the Cramér-Rao Lower Bound (CRLB). Such an estimator, if it exists, is both the minimum variance unbiased estimator (MVUE) and the maximum-likelihood estimator

**<sup>x</sup>***<sup>k</sup>* = **<sup>Φ</sup>***k*,*k*−1**x***k*−<sup>1</sup> + **<sup>G</sup>***k*−1**w***k*−<sup>1</sup> (127) **z***<sup>k</sup>* = **H***k***x***<sup>k</sup>* + **v***<sup>k</sup>* (128)

*<sup>k</sup>* } = 0 (129)

*<sup>k</sup>* } = 0 (130)

discrete-time system and its measurement equation be defined as

*<sup>k</sup>* and **K***<sup>k</sup>* are weight or gain matrices to be computed at each instant *k*. Under

*<sup>k</sup>***x**<sup>ˆ</sup> *<sup>k</sup>*|*k*−<sup>1</sup> <sup>+</sup> **<sup>K</sup>***k***z***<sup>k</sup>* (126)

**<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*<sup>k</sup>* <sup>=</sup> **<sup>K</sup>**�

systems.

where **K**�

circumstances (Kalman, 1960).

(though uncorrelated) form.

the initial state **x**<sup>0</sup> the following constraints:

**5.3 The Kalman filter**

**5.2 Linear filters**

## **5. Estimation**

#### **5.1 Bayesian estimation**

We now turn to the treatment of the stochastic aspects of INS design. In general, the dynamic system derived in the previous sections which describes the navigation and error states evolves in discrete time according to

$$\mathbf{x}\_{k} = \mathbf{f}\_{k-1}(\mathbf{x}\_{k-1}, \mathbf{w}\_{k-1}) \tag{120}$$

where **<sup>f</sup>***k*−<sup>1</sup> is some (possibly nonlinear) function of the previous state and its process noise **<sup>w</sup>***k*−1, which accounts for errors in the model or disturbances to it. Also, generally speaking, we have no direct knowledge of the states themselves, but can only access them through measurements **z** which are related through

$$\mathbf{z}\_k = \mathbf{h}\_k(\mathbf{x}\_{k'} \mathbf{v}\_{\mathcal{X}}) \tag{121}$$

where **h***<sup>k</sup>* is also a possibly nonlinear function of the state and the measurement noise **v***k*. We assume the process noise and the measurement noise are white and statistically independent. The second criterion is very difficult to prove, in which case, for practical purposes we accept that they are at least uncorrelated. Succinctly,

$$E\{\mathbf{w}\_{i}\mathbf{w}\_{j}^{T}\} = E\{\mathbf{v}\_{i}\mathbf{v}\_{j}^{T}\} = \begin{cases} \delta(\mathbf{0})\_{\prime} \,\,\forall i = j\\ \mathbf{0}\_{\prime} \quad \forall i \neq j \end{cases} \tag{122}$$

$$E\{\mathbf{w}\_i\mathbf{v}\_j^T\} = 0, \forall i, j \tag{123}$$

where *E*{·} is the expectation operator and *δ*(·) is the Dirac delta function. At any point, **x** will be a random sample associated with a particular probability density function (pdf). More specifically, given all the measurements of the system up to time *k* − 1, we will have the conditional pdf *<sup>p</sup>*(**x***k*|**Z***k*−1) where **<sup>Z</sup>***k*−<sup>1</sup> = {**z**1, **<sup>z</sup>**2,..., **<sup>z</sup>***k*−1}. The goal is to find *<sup>p</sup>*(**x***k*|**Z***k*) once new measurements are available. Because the current state is dependent only on the state immediately preceding it, it is first-order Markovian and we apply the Chapman-Kolmogorov equation (Duda et al., 2001; Ristic et al., 2004):

$$p(\mathbf{x}\_{k}|\mathbf{Z}\_{k-1}) = \int p(\mathbf{x}\_{k}|\mathbf{x}\_{k-1}) p(\mathbf{x}\_{k-1}|\mathbf{Z}\_{k-1}) d\mathbf{x}\_{k-1} \tag{124}$$

where *<sup>p</sup>*(**x***k*|**x***k*−1) is the transition density, which allows us to calculate the probability that a state will evolve in a particular way from one instant to the next. The result of (124) is essentially a *prediction* of the state vector given all previous information (the Bayesian prior pdf) . Once new measurements become available, we seek to *update* the estimate of **x***<sup>k</sup>* using **z***<sup>k</sup>* (or obtain the Bayesian posterior pdf). Using Bayes' formula

$$\begin{split}p(\mathbf{x}\_{k}|\mathbf{Z}\_{k}) &= \frac{p(\mathbf{z}\_{k}|\mathbf{x}\_{k})p(\mathbf{x}\_{k}|\mathbf{Z}\_{k-1})}{p(\mathbf{z}\_{k}|\mathbf{Z}\_{k-1})}\\ &= \frac{p(\mathbf{z}\_{k}|\mathbf{x}\_{k})p(\mathbf{x}\_{k}|\mathbf{Z}\_{k-1})}{\int p(\mathbf{z}\_{k}|\mathbf{x}\_{k})p(\mathbf{x}\_{k}|\mathbf{Z}\_{k-1})d\mathbf{x}\_{k}}\end{split} \tag{125}$$

We can recursively employ (124) and (125) to estimate the state pdf at any time, from which estimates of the state vector itself can be obtained using any optimality criterion we chose. Initially, there will, of course, be no prior states or measurements, so we define the initial prior as *p*(**x**0), which is simply our best estimate given what we know generally about such systems.

## **5.2 Linear filters**

28 Will-be-set-by-IN-TECH

We now turn to the treatment of the stochastic aspects of INS design. In general, the dynamic system derived in the previous sections which describes the navigation and error states

where **<sup>f</sup>***k*−<sup>1</sup> is some (possibly nonlinear) function of the previous state and its process noise **<sup>w</sup>***k*−1, which accounts for errors in the model or disturbances to it. Also, generally speaking, we have no direct knowledge of the states themselves, but can only access them through

where **h***<sup>k</sup>* is also a possibly nonlinear function of the state and the measurement noise **v***k*. We assume the process noise and the measurement noise are white and statistically independent. The second criterion is very difficult to prove, in which case, for practical purposes we accept

*<sup>j</sup>* } =

where *E*{·} is the expectation operator and *δ*(·) is the Dirac delta function. At any point, **x** will be a random sample associated with a particular probability density function (pdf). More specifically, given all the measurements of the system up to time *k* − 1, we will have the conditional pdf *<sup>p</sup>*(**x***k*|**Z***k*−1) where **<sup>Z</sup>***k*−<sup>1</sup> = {**z**1, **<sup>z</sup>**2,..., **<sup>z</sup>***k*−1}. The goal is to find *<sup>p</sup>*(**x***k*|**Z***k*) once new measurements are available. Because the current state is dependent only on the state immediately preceding it, it is first-order Markovian and we apply the Chapman-Kolmogorov

where *<sup>p</sup>*(**x***k*|**x***k*−1) is the transition density, which allows us to calculate the probability that a state will evolve in a particular way from one instant to the next. The result of (124) is essentially a *prediction* of the state vector given all previous information (the Bayesian prior pdf) . Once new measurements become available, we seek to *update* the estimate of **x***<sup>k</sup>* using

*<sup>p</sup>*(**x***k*|**Z***k*) = *<sup>p</sup>*(**z***k*|**x***k*)*p*(**x***k*|**Z***k*−1)

We can recursively employ (124) and (125) to estimate the state pdf at any time, from which estimates of the state vector itself can be obtained using any optimality criterion we chose.

*<sup>p</sup>*(**z***k*|**Z***k*−1) <sup>=</sup> *<sup>p</sup>*(**z***k*|**x***k*)*p*(**x***k*|**Z***k*−1) *<sup>p</sup>*(**z***k*|**x***k*)*p*(**x***k*|**Z***k*−1)*d***x***<sup>k</sup>*

*<sup>δ</sup>*(0), <sup>∀</sup>*<sup>i</sup>* <sup>=</sup> *<sup>j</sup>*

*<sup>j</sup>* } <sup>=</sup> *<sup>E</sup>*{**v***i***v***<sup>T</sup>*

*<sup>E</sup>*{**w***i***v***<sup>T</sup>*

**<sup>x</sup>***<sup>k</sup>* =**f***k*−1(**x***k*−1, **<sup>w</sup>***k*−1) (120)

**z***<sup>k</sup>* =**h***k*(**x***k*, **v***x*) (121)

*<sup>j</sup>* } = 0, ∀*i*, *j* (123)

*<sup>p</sup>*(**x***k*|**x***k*−1)*p*(**x***k*−1|**Z***k*−1)*d***x***k*−<sup>1</sup> (124)

0, <sup>∀</sup>*<sup>i</sup>* �<sup>=</sup> *<sup>j</sup>* (122)

(125)

**5. Estimation**

**5.1 Bayesian estimation**

evolves in discrete time according to

measurements **z** which are related through

that they are at least uncorrelated. Succinctly,

equation (Duda et al., 2001; Ristic et al., 2004):

*<sup>E</sup>*{**w***i***w***<sup>T</sup>*

*<sup>p</sup>*(**x***k*|**Z***k*−1) =

**z***<sup>k</sup>* (or obtain the Bayesian posterior pdf). Using Bayes' formula

Linear filtering attempts to find the optimal linear combination of the predicted state and the the state implied by the measurements. In recursive form, a linear filter can be written as

$$\hat{\mathbf{x}}\_{k|k} = \mathbf{K}\_k' \hat{\mathbf{x}}\_{k|k-1} + \mathbf{K}\_k \mathbf{z}\_k \tag{126}$$

where **K**� *<sup>k</sup>* and **K***<sup>k</sup>* are weight or gain matrices to be computed at each instant *k*. Under the assumption of Gaussian noise, the resulting pdf of the estimate will also be Gaussian. The Kalman filter, which we shall describe next, is the optimal estimator under these circumstances (Kalman, 1960).

#### **5.3 The Kalman filter**

The problem as described in its general form in section 5.1 is rarely analytically tractable in practice. For one thing, state vectors may be very large, having high- (or infinite-) dimensional pdfs which cannot be integrated in the denominator of (125); the well-known "curse of dimensionality"(Duda et al., 2001). Secondly, the nonlinear models themselves may be unavailable or too complex to deal with analytically. For this reason, the error dynamics in 3.6 have been approximated to a linear system. We have also made many linearizing assumptions in the state equations such as the assumption of fast sampling rates relative to vehicle dynamics. We have also made the assumption of Gaussian-distributed noise in the sensors and shall further assume that disturbances to the error dynamics model take the same (though uncorrelated) form.

Under these conditions, that is linearity and Gaussianity, a realizable solution to the Bayesian estimation problem is the Kalman filter, first described by Rudolph Kalman in 1960. Let the discrete-time system and its measurement equation be defined as

$$\mathbf{x}\_{k} = \Phi\_{k,k-1}\mathbf{x}\_{k-1} + \mathbf{G}\_{k-1}\mathbf{w}\_{k-1} \tag{127}$$

$$\mathbf{z}\_k = \mathbf{H}\_k \mathbf{x}\_k + \mathbf{v}\_k \tag{128}$$

where **<sup>Φ</sup>***k*,*k*−<sup>1</sup> is the state transition matrix, **<sup>w</sup>***k*−<sup>1</sup> is the zero-mean process noise with shaping matrix **<sup>G</sup>***k*−1, **<sup>H</sup>***<sup>k</sup>* is the design matrix mapping state parameters to measurements in **<sup>z</sup>***<sup>k</sup>* and **<sup>v</sup>***<sup>k</sup>* is zero-mean measurement noise (uncorrelated with process noise). **<sup>Φ</sup>***k*,*k*−<sup>1</sup> and **<sup>H</sup>** are linear functions, so **x***<sup>k</sup>* and **z***<sup>k</sup>* are Gaussian random vectors whose pdfs are completely described by their means and covariances. In addition to the requirements of (122) and (123), we have for the initial state **x**<sup>0</sup> the following constraints:

$$E\{\mathbf{x}\_0 \mathbf{w}\_k^T\} = 0\tag{129}$$

$$E\{\mathbf{x}\_0 \mathbf{v}\_k^T\} = \mathbf{0} \tag{130}$$

We seek an efficient estimator (if it exists), which is to say it is unbiased and its covariance attains the Cramér-Rao Lower Bound (CRLB). Such an estimator, if it exists, is both the minimum variance unbiased estimator (MVUE) and the maximum-likelihood estimator

Let <sup>μ</sup>*<sup>A</sup>* <sup>=</sup> **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*k*−1, that is the MLE prior to update, and **<sup>Σ</sup>**−<sup>1</sup>

**<sup>P</sup>***k*|*<sup>k</sup>* <sup>=</sup> **<sup>Σ</sup>***A*,*<sup>B</sup>*

**<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*<sup>k</sup>* = (**Σ**−<sup>1</sup>

which, after some simplification, becomes

= (**Σ**−<sup>1</sup>

The estimate after the update is **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*<sup>k</sup>* <sup>=</sup> <sup>μ</sup>*A*,*B*, which by (138), is

*<sup>A</sup>* <sup>+</sup> **<sup>Σ</sup>**−<sup>1</sup>

*<sup>A</sup>* <sup>+</sup> **<sup>H</sup>***T***Σ**−<sup>1</sup>

*<sup>B</sup>* )†(**Σ**−<sup>1</sup>

<sup>=</sup> **<sup>P</sup>***k*|*k*(**P***k*|*k*−1**x**<sup>ˆ</sup> *<sup>k</sup>*|*k*−<sup>1</sup> <sup>+</sup> **<sup>H</sup>***T***Σ**−<sup>1</sup>

estimate and its covariance is propagated in time using (141) and (142)

both the MLE and the MVUE. Finally, in terms of pdfs,

which provides a Bayesian interpretation of the Kalman filter.

and

and **Σ**−<sup>1</sup>

covariance matrix of the MLE prior to update, whose propagation in time is given by

Fundamentals of GNSS-Aided Inertial Navigation 33

**<sup>P</sup>***k*|*k*−<sup>1</sup> <sup>=</sup> **<sup>Φ</sup>***k*,*k*−1**P***k*−1|*k*−1**Φ***<sup>T</sup>*

*<sup>B</sup>* <sup>=</sup> **<sup>Σ</sup>**−<sup>1</sup> **<sup>x</sup>** as defined in (140). Now, the covariance after the update **<sup>P</sup>***k*|*<sup>k</sup>* is

**<sup>z</sup> <sup>H</sup>**)−<sup>1</sup>

<sup>=</sup> **<sup>Σ</sup>***<sup>A</sup>* <sup>−</sup> **<sup>Σ</sup>***A***H***T*(**HΣ***A***H***<sup>T</sup>* <sup>+</sup> **<sup>Σ</sup>z**)−1**HΣ***<sup>A</sup>*

*<sup>A</sup>* <sup>μ</sup>*<sup>A</sup>* <sup>+</sup> **<sup>Σ</sup>**−<sup>1</sup>

**<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*<sup>k</sup>* <sup>=</sup> **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*k*−<sup>1</sup> <sup>+</sup> **<sup>P</sup>***k*|*k*−1**H***T*(**HP***k*|*k*−1**H***<sup>T</sup>* <sup>+</sup> **<sup>Σ</sup>z**)−1(**<sup>z</sup>** <sup>−</sup> **Hx**<sup>ˆ</sup> *<sup>k</sup>*|*k*−1)

Where **K** is the Kalman gain matrix, which provides the optimal weights for combining the predicted estimate of the state with the new measurements. After the update, the current

It can be shown that the updated state covariance matrix achieves the Cramér-Rao Lower Bound, and that **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*<sup>k</sup>* is unbiased, meaning that the estimator is efficient and automatically

*<sup>B</sup>* μ*B*)

**<sup>z</sup> HH**†**z**)

<sup>=</sup> **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*k*−<sup>1</sup> <sup>+</sup> **<sup>K</sup>**(**<sup>z</sup>** <sup>−</sup> **Hx**<sup>ˆ</sup> *<sup>k</sup>*|*k*−1) (144)

*<sup>p</sup>*(**x***k*−1|**Z***k*−1) = <sup>N</sup> (**x***k*−1; **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*−1|*k*−1, **<sup>P</sup>***k*−1|*k*−1) (145)

*<sup>p</sup>*(**x***k*|**Z***k*−1) = <sup>N</sup> (**x***k*; **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*k*−1, **<sup>P</sup>***k*|*k*−1) (146)

*<sup>p</sup>*(**x***k*|**Z***k*) = <sup>N</sup> (**x***k*; **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*k*, **<sup>P</sup>***k*|*k*) (147)

where **<sup>Q</sup>***k*−<sup>1</sup> is the covariance of the process noise **<sup>w</sup>***k*−1. Next, let <sup>μ</sup>*<sup>B</sup>* = <sup>μ</sup>**<sup>x</sup>** as defined in (139)

*<sup>A</sup>* <sup>=</sup> **<sup>P</sup>**−<sup>1</sup>

*<sup>k</sup>*,*k*−<sup>1</sup> <sup>+</sup> **<sup>Q</sup>***k*−<sup>1</sup> (142)

**<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*k*−<sup>1</sup> <sup>=</sup> **<sup>Φ</sup>***k*,*k*−1**x**<sup>ˆ</sup> *<sup>k</sup>*−1|*k*−<sup>1</sup> (141)

<sup>=</sup> **<sup>P</sup>***k*|*k*−<sup>1</sup> <sup>−</sup> **<sup>P</sup>***k*|*k*−1**H***T*(**HP***k*|*k*−1**H***<sup>T</sup>* <sup>+</sup> **<sup>Σ</sup>z**)−1**HP***k*|*k*−<sup>1</sup> (143)

*<sup>k</sup>*|*k*−<sup>1</sup> or the inverse of

(MLE), thought the converse is generally not true. An efficient estimator therefore has the properties that its estimates **x**ˆ *<sup>k</sup>* and those of any other estimator **x**˜ *<sup>k</sup>* satisfy

$$E\{\left(\hat{\mathbf{x}}\_{k} - \mathbf{x}\_{k}\right)^{T}\left(\hat{\mathbf{x}}\_{k} - \mathbf{x}\_{k}\right)\} \le E\{\left(\tilde{\mathbf{x}}\_{k} - \mathbf{x}\_{k}\right)^{T}\left(\tilde{\mathbf{x}}\_{k} - \mathbf{x}\_{k}\right)\}\tag{131}$$

where (in the case that both are unbiased),

$$E\{\mathbf{\hat{x}}\_{k}\} = E\{\mathbf{\tilde{x}}\_{k}\} = E\{\mathbf{x}\_{k}\} \tag{132}$$

The Gaussian likelihood function has the form

$$\ell(\mu, \Sigma; \mathbf{x}) = \exp(-\frac{1}{2}(\mathbf{x} - \mu)^T \Sigma^{-1} (\mathbf{x} - \mu)) \tag{133}$$

where the free parameters are the mean μ and the covariance **Σ** and the data **x** is given. A maximum likelihood estimator is one that provides μ and **Σ** that maximize � given a particular sample **x**. In other words, it provides the parameters of the Gaussian pdf that would most likely lead to having observed **x** (Papoulis, 1977). For two independent events, the joint probability is *P*(*a*, *b*) = *P*(*a*)*P*(*b*). Likelihood functions obey a similar rule

$$\ell\_{A,B}(\mu\_{A,B}, \Sigma\_{A,B}; \mathbf{x}) = \ell\_A(\mu\_A, \Sigma\_A; \mathbf{x}) \ell\_B(\mu\_B, \Sigma\_B; \mathbf{x}) \tag{134}$$

which, for the log-likelihood reduces to the equality

$$\log(\ell\_{A,B}(\mu\_{A,B}, \Sigma\_{A,B}|\mathbf{x}, \mathbf{y})) = \log(\ell\_A(\mu\_{A\prime}\Sigma\_A|\mathbf{x})) + \log(\ell\_B(\mu\_{B\prime}\Sigma\_B|\mathbf{x})) $$

or

$$\begin{aligned} &-\frac{1}{2}(\mathbf{x}-\boldsymbol{\mu}\_{A,B})^T \boldsymbol{\Sigma}\_{A,B}^{-1} (\mathbf{x}-\boldsymbol{\mu}\_{A,B}) = \\ &\log(c) - \frac{1}{2}(\mathbf{x}-\boldsymbol{\mu}\_{A})^T \boldsymbol{\Sigma}\_{A}^{-1} (\mathbf{x}-\boldsymbol{\mu}\_{A}) - \frac{1}{2}(\mathbf{x}-\boldsymbol{\mu}\_{B})^T \boldsymbol{\Sigma}\_{B}^{-1} (\mathbf{x}-\boldsymbol{\mu}\_{B}) \end{aligned} \tag{135}$$

where *c* > 0 is an arbitrary constant. As shown in (Grewal et al., 2001), after taking the first and second derivatives with respect to **x**, this further reduces to

$$
\Sigma\_{A,B}^{-1} = \Sigma\_A^{-1} + \Sigma\_B^{-1} \tag{136}
$$

Furthermore, the joint MLE is given by

$$
\Sigma\_{A,B}^{-1}\mu\_{A,B} = \Sigma\_A^{-1}\mu\_A + \Sigma\_B^{-1}\mu\_B \tag{137}
$$

$$
\Delta \Rightarrow \mu\_{A,B} = (\Sigma\_A^{-1} + \Sigma\_B^{-1})^\dagger (\Sigma\_A^{-1} \mu\_A + \Sigma\_B^{-1} \mu\_B) \tag{138}
$$

where † is the generalized inverse of the matrix.

For any measurement vector **<sup>z</sup>**, its ML estimate ˆμ**<sup>z</sup>** <sup>=</sup> **<sup>z</sup>** and its covariance **<sup>Σ</sup><sup>z</sup>** <sup>=</sup> *<sup>E</sup>*{**vv***T*}. We now transform the likelihood based on the measurements into likelihood of the state vector by

$$
\mu\_{\mathbf{x}} = \mathbf{H}^{\dagger} \mathbf{z} \tag{139}
$$

$$
\boldsymbol{\Sigma}\_{\mathbf{x}}^{-1} = \mathbf{H}^T \boldsymbol{\Sigma}\_{\mathbf{z}}^{-1} \mathbf{H} \tag{140}
$$

Let <sup>μ</sup>*<sup>A</sup>* <sup>=</sup> **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*k*−1, that is the MLE prior to update, and **<sup>Σ</sup>**−<sup>1</sup> *<sup>A</sup>* <sup>=</sup> **<sup>P</sup>**−<sup>1</sup> *<sup>k</sup>*|*k*−<sup>1</sup> or the inverse of covariance matrix of the MLE prior to update, whose propagation in time is given by

$$
\hat{\mathbf{x}}\_{k|k-1} = \Phi\_{k,k-1} \hat{\mathbf{x}}\_{k-1|k-1} \tag{141}
$$

and

30 Will-be-set-by-IN-TECH

(MLE), thought the converse is generally not true. An efficient estimator therefore has the

2

where the free parameters are the mean μ and the covariance **Σ** and the data **x** is given. A maximum likelihood estimator is one that provides μ and **Σ** that maximize � given a particular sample **x**. In other words, it provides the parameters of the Gaussian pdf that would most likely lead to having observed **x** (Papoulis, 1977). For two independent events, the joint

log(�*A*,*B*(μ*A*,*B*, **Σ***A*,*B*|**x**, **y**)) = log(�*A*(μ*A*, **Σ***A*|**x**)) + log(�*B*(μ*B*, **Σ***B*|**x**))

2

*<sup>A</sup>* <sup>+</sup> **<sup>Σ</sup>**−<sup>1</sup>

*<sup>B</sup>* )†(**Σ**−<sup>1</sup>

where *c* > 0 is an arbitrary constant. As shown in (Grewal et al., 2001), after taking the first

*<sup>A</sup>* <sup>μ</sup>*<sup>A</sup>* <sup>+</sup> **<sup>Σ</sup>**−<sup>1</sup>

*<sup>A</sup>* <sup>+</sup> **<sup>Σ</sup>**−<sup>1</sup>

**Σ**−<sup>1</sup>

For any measurement vector **<sup>z</sup>**, its ML estimate ˆμ**<sup>z</sup>** <sup>=</sup> **<sup>z</sup>** and its covariance **<sup>Σ</sup><sup>z</sup>** <sup>=</sup> *<sup>E</sup>*{**vv***T*}. We now transform the likelihood based on the measurements into likelihood of the state vector

**<sup>x</sup>** = **<sup>H</sup>***T***Σ**−<sup>1</sup>

*<sup>E</sup>*{(**x**<sup>ˆ</sup> *<sup>k</sup>* <sup>−</sup> **<sup>x</sup>***k*)*T*(**x**<sup>ˆ</sup> *<sup>k</sup>* <sup>−</sup> **<sup>x</sup>***k*)} ≤ *<sup>E</sup>*{(**x**˜ *<sup>k</sup>* <sup>−</sup> **<sup>x</sup>***k*)*T*(**x**˜ *<sup>k</sup>* <sup>−</sup> **<sup>x</sup>***k*)} (131)

�*A*,*B*(μ*A*,*B*, **Σ***A*,*B*; **x**) = �*A*(μ*A*, **Σ***A*; **x**)�*B*(μ*B*, **Σ***B*; **x**) (134)

(**<sup>x</sup>** <sup>−</sup> <sup>μ</sup>*B*)*T***Σ**−<sup>1</sup>

*<sup>A</sup>* <sup>μ</sup>*<sup>A</sup>* <sup>+</sup> **<sup>Σ</sup>**−<sup>1</sup>

*E*{**x**ˆ *<sup>k</sup>*} = *E*{**x**˜ *<sup>k</sup>*} = *E*{**x***k*} (132)

(**<sup>x</sup>** <sup>−</sup> <sup>μ</sup>)*T***Σ**−1(**x**−μ)) (133)

*<sup>B</sup>* (**x**−μ*B*) (135)

*<sup>B</sup>* μ*B*) (138)

*<sup>B</sup>* (136)

*<sup>B</sup>* μ*<sup>B</sup>* (137)

μ**<sup>x</sup>** = **H**†**z** (139)

**<sup>z</sup> H** (140)

properties that its estimates **x**ˆ *<sup>k</sup>* and those of any other estimator **x**˜ *<sup>k</sup>* satisfy

�(μ, **<sup>Σ</sup>**; **<sup>x</sup>**) = exp(−<sup>1</sup>

probability is *P*(*a*, *b*) = *P*(*a*)*P*(*b*). Likelihood functions obey a similar rule

*<sup>A</sup>*,*B*(**x**−μ*A*,*B*) =

**Σ**−<sup>1</sup> *<sup>A</sup>*,*<sup>B</sup>* <sup>=</sup> **<sup>Σ</sup>**−<sup>1</sup>

*<sup>A</sup>* (**x**−μ*A*) <sup>−</sup> <sup>1</sup>

where (in the case that both are unbiased),

The Gaussian likelihood function has the form

which, for the log-likelihood reduces to the equality

(**<sup>x</sup>** <sup>−</sup> <sup>μ</sup>*A*,*B*)*T***Σ**−<sup>1</sup>

**Σ**−<sup>1</sup>

where † is the generalized inverse of the matrix.

(**<sup>x</sup>** <sup>−</sup> <sup>μ</sup>*A*)*T***Σ**−<sup>1</sup>

and second derivatives with respect to **x**, this further reduces to

*<sup>A</sup>*,*B*μ*A*,*<sup>B</sup>* <sup>=</sup> **<sup>Σ</sup>**−<sup>1</sup>

<sup>⇒</sup> <sup>μ</sup>*A*,*<sup>B</sup>* = (**Σ**−<sup>1</sup>

2

or

by

− 1 2

log(*c*) <sup>−</sup> <sup>1</sup>

Furthermore, the joint MLE is given by

$$\mathbf{P}\_{k|k-1} = \boldsymbol{\Phi}\_{k,k-1} \mathbf{P}\_{k-1|k-1} \boldsymbol{\Phi}\_{k,k-1}^T + \mathbf{Q}\_{k-1} \tag{142}$$

where **<sup>Q</sup>***k*−<sup>1</sup> is the covariance of the process noise **<sup>w</sup>***k*−1. Next, let <sup>μ</sup>*<sup>B</sup>* = <sup>μ</sup>**<sup>x</sup>** as defined in (139) and **Σ**−<sup>1</sup> *<sup>B</sup>* <sup>=</sup> **<sup>Σ</sup>**−<sup>1</sup> **<sup>x</sup>** as defined in (140). Now, the covariance after the update **<sup>P</sup>***k*|*<sup>k</sup>* is

$$\begin{aligned} \mathbf{P}\_{k|k} &= \boldsymbol{\Sigma}\_{A,B} \\ &= (\boldsymbol{\Sigma}\_{A}^{-1} + \mathbf{H}^{T} \boldsymbol{\Sigma}\_{\mathbf{z}}^{-1} \mathbf{H})^{-1} \\ &= \boldsymbol{\Sigma}\_{A} - \boldsymbol{\Sigma}\_{A} \mathbf{H}^{T} (\mathbf{H} \boldsymbol{\Sigma}\_{A} \mathbf{H}^{T} + \boldsymbol{\Sigma}\_{\mathbf{z}})^{-1} \mathbf{H} \boldsymbol{\Sigma}\_{A} \\ &= \mathbf{P}\_{k|k-1} - \mathbf{P}\_{k|k-1} \mathbf{H}^{T} (\mathbf{H} \mathbf{P}\_{k|k-1} \mathbf{H}^{T} + \boldsymbol{\Sigma}\_{\mathbf{z}})^{-1} \mathbf{H} \mathbf{P}\_{k|k-1} \end{aligned} \tag{143}$$

The estimate after the update is **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*<sup>k</sup>* <sup>=</sup> <sup>μ</sup>*A*,*B*, which by (138), is

$$\hat{\mathbf{x}}\_{k|k} = (\boldsymbol{\Sigma}\_A^{-1} + \boldsymbol{\Sigma}\_B^{-1})^\dagger (\boldsymbol{\Sigma}\_A^{-1} \boldsymbol{\mu}\_A + \boldsymbol{\Sigma}\_B^{-1} \boldsymbol{\mu}\_B)$$

$$= \mathbf{P}\_{k|k} (\mathbf{P}\_{k|k-1} \hat{\mathbf{x}}\_{k|k-1} + \mathbf{H}^T \boldsymbol{\Sigma}\_\mathbf{z}^{-1} \mathbf{H} \mathbf{H}^\dagger \mathbf{z})$$

which, after some simplification, becomes

$$
\hat{\mathbf{x}}\_{k|k} = \hat{\mathbf{x}}\_{k|k-1} + \mathbf{P}\_{k|k-1} \mathbf{H}^T (\mathbf{H} \mathbf{P}\_{k|k-1} \mathbf{H}^T + \Sigma\_\mathbf{z})^{-1} (\mathbf{z} - \mathbf{H} \hat{\mathbf{x}}\_{k|k-1})
$$

$$
= \hat{\mathbf{x}}\_{k|k-1} + \mathbf{K} (\mathbf{z} - \mathbf{H} \hat{\mathbf{x}}\_{k|k-1}) \tag{144}
$$

Where **K** is the Kalman gain matrix, which provides the optimal weights for combining the predicted estimate of the state with the new measurements. After the update, the current estimate and its covariance is propagated in time using (141) and (142)

It can be shown that the updated state covariance matrix achieves the Cramér-Rao Lower Bound, and that **<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*<sup>k</sup>* is unbiased, meaning that the estimator is efficient and automatically both the MLE and the MVUE. Finally, in terms of pdfs,

$$p(\mathbf{x}\_{k-1}|\mathbf{Z}\_{k-1}) = \mathcal{N}(\mathbf{x}\_{k-1}; \mathbf{\hat{x}}\_{k-1|k-1}, \mathbf{P}\_{k-1|k-1}) \tag{145}$$

$$p(\mathbf{x}\_k|\mathbf{Z}\_{k-1}) = \mathcal{N}(\mathbf{x}\_k; \mathbf{\hat{x}}\_{k|k-1}, \mathbf{P}\_{k|k-1}) \tag{146}$$

$$p(\mathbf{x}\_k|\mathbf{Z}\_k) = \mathcal{N}(\mathbf{x}\_k; \mathbf{\hat{x}}\_{k|k'} \mathbf{P}\_{k|k}) \tag{147}$$

which provides a Bayesian interpretation of the Kalman filter.

Under the assumption of a fast sampling rate, the state transition matrix **<sup>Φ</sup>***k*,*k*−<sup>1</sup> is given by **<sup>Φ</sup>***k*,*k*−<sup>1</sup> = *<sup>e</sup>*

Fundamentals of GNSS-Aided Inertial Navigation 35

Where **F** is the error dynamics matrix defined in 3.6. Using (141),(142), (143) and (144), it becomes evident that although alignment errors and sensor biases are not directly observable, the Kalman gain matrix **K** contains information about their contribution to the navigation states. Since the error dynamics are linearized about the current state estimates, the filter

We finish with the remark that, in general, the assumptions of linearity, Gaussianity and uncorrelated noise sources are not strictly justified in INS applications. Beyond employing an extended Kalman filter (EKF) as described above, the further development and application of nonlinear filters such as the unscented Kalman filter (UKF) and the particle filter have been undertaken in attempts at solving the more general Bayesian formulation of the problem.

Aided inertial navigation remains an active area of research, especially with the introduction of smaller and cheaper (but noisier) inertial sensors. Among the challenges presented by these devices is heading initialization (Titterton & Weston, 2004), which necessitates the use of other aiding systems, and proper stochastic modeling of their error charactertics. In addition, the nonlinearity of the state equations has prompted much research in applied optimal estimation. Despite this, the underlying concepts remain the same and the development presented here should give the reader enough background to understand the issues involved, enabling him

Angeles, J.(2003). *Fundamentals of robotic mechanical systems: theory, methods, and algorithms*,

Grewal, M.S.; Weill, L.R. & Andrews, A.P.(2001). *Globabl Positioning Systems, Inertial Navigation,*

Hofmann-Wellenhof; B. Lichtenegger, H. & Collins, J. (2001). *Global Positioning System Theory*

Hofmann-Wellenhof, B. & Moritz, H. (2005).*Physical Geodesy, Second Edition*, Springer-Verlag,

Kalman, R.E. (1960). A New Approach to Linear Filtering and Prediction Problems.

Kim, J.W.; Hwang, D. & Lee, S.J. (2006). A Deeply Coupled GPS/INS Integrated Kalman

Kreye, C.; Eissfeller, B. & Winkler, J.O. (2000). Improvement of GNSS Receiver Performance

Filter Design Using a Linearized Correlator Output. *Position, Location, And Navigation*

Using Deeply Coupled INS Measurement. *Proceedings of ION GPS*, pp. 844-854, The

*Transactions of the ASME Journal of Basic Engineering*, 82, 34-45, 1960.

*Symposium, 2006 IEEE/ION* , pp. 300-305, April 2006, IEEE, San Diego. Kohler, W.E. & Johnson, L. (2006). *Elementary Differential Equations with Boundary Value*

Duda, R.; Hart, P. & Stork, D.(2001). *Pattern Classification*, John Wiley & Sons, New York. Featherstone, W.E. & Dentith, M.C. (1997). A Geodetic Approach to Gravity Data Reduction

or her to pursue more detailed aspects of INS and aided INS design as necessary.

for Geophysics. *Computers and Geosciences*, 23, 10, 1063-1070

*and Integration*, John Wiley & Sons, New York.

*Problems*, Pearson/Addison Wesley, Boston.

Institute of Navigation, Salt Lake City.

*and Practice*, Springer-Verlag, Wien and New York.

presented here is an example of extended Kalman filtering.

**6. Conclusion**

**7. References**

Springer-Verlag, New York.

Wien and New York.

*<sup>δ</sup>t***<sup>F</sup>** (150)

## **5.4 GNSS aiding**

Because of unbounded position errors associated with misalignment and gyro drift, along with the undesirability of having even bounded oscillations in the position due to accelerometer and velocity errors, it is necessary for most applications using medium-grade and commodity-grade IMUs to employ an aiding method. That is, using an external (and independent) estimate of navigation states to limit the accumulation of errors in the INS. For the last two decades, the preferred method has been to use measurements obtained from global navigation satellite systems such as GPS to update the INS error estimates and improve the navigation solution. The simplest way to achieve this, of course, is to simply use the calculated positions and velocities from the GNSS directly in place of the results from the mechanization as they become available. This is the so-called reset "filter", although from the standpoint of optimal filtering, it has many undesirable effects such as introducing sudden jumps in the navigation states. Moreover, the complementary filter places all the weight on the GNSS-derived values, which themselves are subject to error.

Alternatively, Kalman filtering is used to optimally estimate the error states of the INS, with updates coming from GNSS in one of several architectures:


In the loosely-coupled scheme, at each update we let

$$\mathbf{z} = \begin{pmatrix} \phi\_{INS} - \phi\_{GNSS} \\ \lambda\_{INS} - \lambda\_{GNSS} \\ h\_{INS} - h\_{GNSS} \\ v\_{\mathcal{E}\_{INS}} - v\_{\mathcal{E}\_{GNSS}} \\ v\_{\mathcal{U}\_{INS}} - v\_{\mathcal{U}\_{GNSS}} \\ v\_{\mathcal{U}\_{INS}} - v\_{\mathcal{U}\_{GNSS}} \end{pmatrix} = \begin{pmatrix} \delta\phi \\ \delta\lambda \\ \delta\hbar \\ \delta v\_{\mathcal{E}} \\ \delta v\_{\mathcal{U}} \\ \delta v\_{\mathcal{U}} \end{pmatrix} \tag{148}$$

The measurement equation is then

$$\mathbf{H}\hat{\mathbf{x}}\_{k|k-1} = \begin{pmatrix} 1 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \\ 0 \ 1 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \\ 0 \ 0 \ 1 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \\ 0 \ 0 \ 0 \ 1 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \\ 0 \ 0 \ 0 \ 0 \ 1 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \end{pmatrix} \hat{\mathbf{x}}\_{k|k-1} \tag{149}$$

Under the assumption of a fast sampling rate, the state transition matrix **<sup>Φ</sup>***k*,*k*−<sup>1</sup> is given by

$$
\Phi\_{k,k-1} = \mathcal{e}^{\delta t \mathbf{F}} \tag{150}
$$

Where **F** is the error dynamics matrix defined in 3.6. Using (141),(142), (143) and (144), it becomes evident that although alignment errors and sensor biases are not directly observable, the Kalman gain matrix **K** contains information about their contribution to the navigation states. Since the error dynamics are linearized about the current state estimates, the filter presented here is an example of extended Kalman filtering.

We finish with the remark that, in general, the assumptions of linearity, Gaussianity and uncorrelated noise sources are not strictly justified in INS applications. Beyond employing an extended Kalman filter (EKF) as described above, the further development and application of nonlinear filters such as the unscented Kalman filter (UKF) and the particle filter have been undertaken in attempts at solving the more general Bayesian formulation of the problem.

## **6. Conclusion**

32 Will-be-set-by-IN-TECH

Because of unbounded position errors associated with misalignment and gyro drift, along with the undesirability of having even bounded oscillations in the position due to accelerometer and velocity errors, it is necessary for most applications using medium-grade and commodity-grade IMUs to employ an aiding method. That is, using an external (and independent) estimate of navigation states to limit the accumulation of errors in the INS. For the last two decades, the preferred method has been to use measurements obtained from global navigation satellite systems such as GPS to update the INS error estimates and improve the navigation solution. The simplest way to achieve this, of course, is to simply use the calculated positions and velocities from the GNSS directly in place of the results from the mechanization as they become available. This is the so-called reset "filter", although from the standpoint of optimal filtering, it has many undesirable effects such as introducing sudden jumps in the navigation states. Moreover, the complementary filter places all the weight on

Alternatively, Kalman filtering is used to optimally estimate the error states of the INS, with

• Loosely-coupled integration. Here, the GNSS system acts to provide a full position and velocity estimate independently of the INS mechanization. The measurements of the error states arises from subtracting the GNSS states from the position and velocity arising from the mechanization. These are then transferred through the design matrix **H** of the measurement equations and used to update the error estimates, which in turn are

• Tightly-coupled integration. In this scheme, the GNSS measurements and error states are directly incorporated into the Kalman filter, the primary benefit being that the navigation state can be improved over the mechanization alone with fewer than four GNSS satellites being tracked at any given time. A detailed treatment can be found in (Grewal et al., 2001). • Deeply-coupled integration. This is a hardware-level implementation which further incorporates the states associated with the GNSS receiver signal tracking loop. This allows for better tracking stability under high dynamics and rapid reacquisition of GNSS signals

the GNSS-derived values, which themselves are subject to error.

under intermittent visibility (Kim et al., 2006; Kreye et al., 2000).

⎛

*φINS* − *φGNSS λINS* − *λGNSS hINS* − *hGNSS veINS* − *veGNSS vnINS* − *vnGNSS vuINS* − *vuGNSS*

⎞

⎛

*δφ δλ δh δve δvn δvu* ⎞

⎟⎟⎟⎟⎟⎟⎠

⎞

⎟⎟⎟⎟⎟⎟⎠

**<sup>x</sup>**<sup>ˆ</sup> *<sup>k</sup>*|*k*−<sup>1</sup> (149)

(148)

⎜⎜⎜⎜⎜⎜⎝

⎟⎟⎟⎟⎟⎟⎠ =

100000000000000 010000000000000 001000000000000 000100000000000 000010000000000 000001000000000

⎜⎜⎜⎜⎜⎜⎝

⎛

⎜⎜⎜⎜⎜⎜⎝

**z** =

In the loosely-coupled scheme, at each update we let

**Hx**<sup>ˆ</sup> *<sup>k</sup>*|*k*−<sup>1</sup> <sup>=</sup>

The measurement equation is then

updates coming from GNSS in one of several architectures:

subtracted from the navigation states.

**5.4 GNSS aiding**

Aided inertial navigation remains an active area of research, especially with the introduction of smaller and cheaper (but noisier) inertial sensors. Among the challenges presented by these devices is heading initialization (Titterton & Weston, 2004), which necessitates the use of other aiding systems, and proper stochastic modeling of their error charactertics. In addition, the nonlinearity of the state equations has prompted much research in applied optimal estimation. Despite this, the underlying concepts remain the same and the development presented here should give the reader enough background to understand the issues involved, enabling him or her to pursue more detailed aspects of INS and aided INS design as necessary.

## **7. References**


**2** 

*China* 

**Quantitative Feedback Theory and** 

Xiaojun Xing and Dongli Yuan

{ } *P* of possible plants which

*Northwestern Polytechnical University, Xi'an,* 

**Its Application in UAV's Flight Control** 

Quantitative feedback theory (hereafter referred as QFT), developed by Isaac Horowitz (Horowitz, 1963; Horowitz and Sidi, 1972), is a frequency domain technique utilizing the Nichols chart in order to achieve a desired robust design over a specified region of plant uncertainty. Desired time-domain responses are transformed into frequency domain tolerances, which lead to bounds (or constraints) on the loop transmission function. The design process is highly transparent, allowing a designer to see what trade-offs are

QFT is also a unified theory that emphasizes the use of feedback for achieving the desired system performance tolerances despite plant uncertainty and plant disturbances. QFT quantitatively formulates these two factors in the form of (a) the set { } *R R T* of acceptable command or tracking input-output relationships and the set { } *D D T* of acceptable

include the uncertainties. The objective is to guarantee that the control ratio *T YR <sup>R</sup>* / is a member of *R* and *T YD <sup>D</sup>* / is a member of *<sup>D</sup>* , for all plants *P* which are contained in

 . QFT has been developed for control systems which are both linear and nonlinear, timeinvariant and time-varying, continuous and sampled-data, uncertain multiple-input singleoutput (MISO) and multiple-input multiple-output (MIMO) plants, and for both output and

The QFT synthesis technique for highly uncertain linear time-invariant MIMO plants has the

1. The MIMO synthesis problem is converted into a number of single-loop feedback problems in which parameter uncertainty, external disturbances, and performance tolerances are derived from the original MIMO problem. The solutions to these single-

2. The design is tuned to the extent of the uncertainty and the performance tolerances.

**1. Introduction** 

internal variable feedback.

2. SISO nonlinear systems. 3. MIMO LTI systems. 4. MIMO nonlinear systems.

following features:

necessary to achieve a desired performance level.

disturbance input-output relationships, and (b) a set

loop problems represent a solution to the MIMO plant.

This design technique is applicable to the following problem classes:

1. Single-input single-output (SISO) linear-time-invariant (LTI) systems

