INS/GNSS Loosely-coupled Kalman Filter (local-navigation frame)

Table of Contents

• Local-Level Frame Error State Equations
  • Short form
  • Detailed form
• Augmented State with Noise
  • Random Walk
  • Gauss-Markov 1. Order
  • Noise scale matrix W
  • Groves' process noise definition
• Kalman-Filter Matrices
  • State transition matrix 𝚽 & Process noise covariance matrix Q
    • State transition matrix 𝚽
      • Exponential Matrix
      • Taylor series
    • Process noise covariance matrix Q
    • Van Loan method
  • Error covariance matrix P
  • Measurement innovation 𝛿z
  • Measurement sensitivity Matrix H
  • Measurement noise covariance matrix R
• Corrections
  • Position, Velocity, Attitude
  • Biases
• Unit discussion
  • System matrix F
  • Gauss-Markov constant 𝛽 & Noise scale matrix G
  • Process noise covariance matrix Q
  • Error covariance matrix P
  • Measurement innovation 𝛿z
  • Measurement sensitivity Matrix H
  • Measurement noise covariance matrix R
• Appendix
  • Derivation
    • Position Equations
    • Attitude Equations
    • Velocity Equations
  • Error Equations comparison
    • Titterton
    • Gleason
    • Noureldin

Local-Level Frame Error State Equations

Short form

Detailed form

This form corresponds to

• [17] Groves, ch. 14.2.4, eq. 14.63, p. 587ff

The following sources can be taken as reference, but have at least one deviation from the form presented here

• [47] Titterton, ch. 12.3.1.3, eq. 12.28, p. 345
  • Different sign in equations , , ,
• [35] Noureldin, ch. 6.1.5, eq. 6.76, p. 219
  • ENU-System
  • Yaw defined counterclockwise
  • Different sign for the DCM term

However in section Error Equations comparison it is shown, that they can be transformed into each other.

Augmented State with Noise

The continuous linear dynamic system can be expressed as

Process noise is introduced into the and matrices in the accelerometer and gyroscope bias terms. Therefore the error equations can be extended as follows

Random Walk

The random noise on the accelerometer and gyro measurements can be estimated as a Random Walk in the angle and velocity errors.

In general, the noise can have an arbitrary characteristic. In a Kalman filter, however, it must be normally distributed and is therefore limited to a Wiener Process. The terms in the noise input matrix are not scaled and therefore need to be compensated with a noise scale matrix depending on the amplitude of the noise process. The amplitude is defined by the standard deviation . This parameter can be chosen by the user for the accel and gyro measurements:

where

• is the standard deviation of the noise on the accelerometer specific-force measurements
• is the standard deviation of the noise on the gyro angular-rate measurements

The bias states are also modeled as a random process. In case of random walk, their amplitudes are defined analogously to the above and .

Gauss-Markov 1. Order

Another option to model the bias states is given by the Gauss-Markov 1. Order process:

This introduces a deterministic part into the equation, namely the correlation coefficient . This parameter defines, how much a current state is correlated with past states. The elements of the matrix are then

where

• is the Gauss-Markov constant for the accelerometer 𝛽 = 1 / 𝜏 (𝜏 correlation length)
• is the Gauss-Markov constant for the gyroscope 𝛽 = 1 / 𝜏 (𝜏 correlation length)

Like before, the scaling is done inside the noise scale matrix. Now, the bias noise's amplitude is given by

with a given standard deviation of for the accelerometer's bias noise and for the gyro's bias noise. The correlation lengths and define how much a current state is correlated to values in the past. This process is generally limited to normal distributions. A detailed example of this type of system can be found in Brown & Hwang [10] (example 9.6).

Noise scale matrix W

are diagonal matrices where entries are filled with the PSDs from and or .

Groves' process noise definition

In Groves [17], process noise is defined without the need of integration. Instead, noise amplitudes are given directly integrated over one time step . Therefore, the PSDs of the accelerometer and gyro noise are:

where

• is the standard deviation of the noise on the accelerometer specific-force measurements
• is the standard deviation of the noise on the gyro angular-rate measurements
• is the interval between the input of successive accelerometer and gyro outputs to the inertial navigation equations

The bias states are modeled by Groves with the noise PSD's:

where

• and are the standard deviations of the accelerometer and gyro dynamic biases
• and are the correlation times of the dynamic accelerometer and gyro biases

see [17] Groves, ch. 14.2.6, eq. 14.84, p. 592

Although correlation lengths and are considered, this process is rather a random walk than a Gauss-Markov 1. order process, because of the missing deterministic part in .

In INSTINCT, Groves' method is not altered from its reference [17] to avoid confusion.

Kalman-Filter Matrices

State transition matrix 𝚽 & Process noise covariance matrix Q

State transition matrix 𝚽

Exponential Matrix

The state transition matrix is defined as

see [17] Groves, ch. 3.2.3, eq. 3.33, p. 97

Taylor series

The exponential matrix cannot be calculated directly and numerical methods are computationally intensive. Therefore the transition matrix can be computed as a power-series expansion

see [17] Groves, ch. 3.2.3, eq. 3.34, p. 98

Process noise covariance matrix Q

The process noise covariance matrix is defined as

In Groves [17], this matrix is approximated as (see ch. 3.2.3, eq. 3.43/3.45, p. 99):

Taking the noise input and scale matrices from and and putting it into the exact form , it becomes

The exponential matrix shall be approximated by first order. This leads to:

The final equation is

where

• is the propagation interval (as we trigger the prediction every time we get IMU measurements this corresponds to , the time between measurements)

see [17] Groves, ch. 14.2.6, eq. 14.80/14.81, p. 591

This method resembles a Taylor expansion up to 1. order. Since Groves [17] models the IMU measurements and their biases as random walk processes (see section Groves' process noise definition), only the van-Loan-method can be configured to model biases as Gauss-Markov 1. order processes.

Van Loan method

The state transition matrix and the system/process noise covariance matrix can be calculated with the van Loan method [48] . In short

1. Form a matrix called ( is the dimension of and is the power spectral density of the noise )

   

2. Calculate the exponential of

   

3. Calculate the state transition matrix as

   

4. Calculate the process noise covariance matrix as

   

Error covariance matrix P

The error covariance matrix is initialized as a diagonal matrix with the variance of the initial state as diagonal elements.

Measurement innovation 𝛿z

where

• denotes GNSS indicated measurements
• is the lever arm from the INS to the GNSS antenna in [m]
• is the Cartesian-to-curvilinear position change transformation matrix ([17] Groves, ch. 14.3.1, eq. 14.104, p. 598ff)

This form corresponds to

• [17] Groves, ch. 14.3.1, eq. 14.103, p. 598

Measurement sensitivity Matrix H

see

• [17] Groves, ch. 3.4.1, eq. 3.90, p. 119

where

See

• [17] Groves, ch. 14.3.1, eq. 14.113/14.114, p. 600

Measurement noise covariance matrix R

The measurement noise covariance matrix is a diagonal matrix with the variances of the measurements as diagonal elements.

This variances can be set static or ideally come from the GNSS receiver.

Corrections

This section describes how the errors are applied to the total state.

Position, Velocity, Attitude

The position and velocity errors get applied by simply subtracting them from the previous total state (see [17] Groves, ch. 14.1.1, eq. 14.8, 14.10, p. 564)

The attitude correction can be derived from equation (see [17] Groves, ch. 14.1.1, eq. 14.7, p. 564)

After applying the errors to the total state, the errors have to be reset to zero (closed-loop).

Biases

To correct the accelerometer and gyroscope measurements, we need to accumulate the error states. This has to be done, because contrary to the normal state, the expected value of the biases is not zero.

Afterwards this accumulated value can be applied to the measurements by subtracting it

Afterwards the bias in the error state gets reset to zero, as it is accounted for in the accumulated value.

Unit discussion

System matrix F

The units of the state vector and the matrix are as follows

When scaling elements in the relevant parts of the matrix have to be scaled as well.

The unit is [pseudometre] and is latitude or longitude converted from radian into metre by multiplying it with the Earth radius. However the radius is assumed as a constant for the conversion.

Gauss-Markov constant 𝛽 & Noise scale matrix G

If a bias state is modeled by a Gauss-Markov first order process instead of random walk, the deterministic part of the equation is extended.

When scaling the biases in the factors stay unscaled. However the matrix has to be adapted.

If the noise input matrix is scaled, then the noise scale matrix does not have to be scaled.

Process noise covariance matrix Q

If the alternative process noise covariance matrix is used, it also has to be scaled.

Error covariance matrix P

The error covariance matrix is initialized as a diagonal matrix with the variance of the initial state as diagonal elements. Therefore the units are the squared units of the state vector.

When scaling elements in the relevant parts of the matrix have to be adapted. This means the scale factors have to be multiplied with the correct rows and columns. As is a diagonal matrix, this means only the diagonal terms have to be scaled.

Measurement innovation 𝛿z

When scaling elements in the relevant parts of the measurement innovation have to be adapted

Measurement sensitivity Matrix H

When scaling elements in the relevant parts of the measurement innovation have to be adapted. The matrix stays unscaled.

Measurement noise covariance matrix R

When scaling elements in the measurement innovation is scaled and therefore the relevant parts of the matrix have to be adapted.

Appendix

Derivation

Position Equations

The time derivative of curvilinear position as a function of the Earth-referenced velocity in local navigation frame axes (see [17] Groves, ch. 2.4.2, eq. 2.111, p. 61 or [47] Titterton, ch. 3.7, eq. 3.81,3.85,3.86, p. 48ff):

Differential of left and right side neglecting errors in and ( and )

Attitude Equations

The equations here mainly follow [47] Titterton, ch. 12.3.1.1, p. 342f

The estimated attitude can be written in terms of true direction cosine matrix

where is the transformation from true reference axes to estimated reference axes and it can be approximated for small angle misalignments as

with being a skew symmetric matrix and being the attitude errors (e.g. roll, pitch, yaw for small euler angles).

Substituting into gives

and solving for

Differentiating this equation yields:

Using the following equations

• Time derivative DCM local-navigation-frame  [17] Groves, ch. 5.4.1, eq. 5.39, p. 176 or [47] Titterton, ch. 3.5.3, eq. 3.28, p. 32
  • Time derivative DCM  [17] Groves, ch. 2.3.1, eq. 2.52, p. 45
  • DCM with small angle approximation  [17] Groves, ch. 2.2.2, eq. 2.26, p. 39
• Angular rate splitting  [17] Groves, ch. 2.3.1, eq. 2.48, p. 45
• Skew-symmetric matrix transformation  [17] Groves, ch. 2.3.1, eq. 2.51, p. 45

leads to the time derivative of the coordinate transformation matrix (see [17] Groves, ch. 5.4.1, eq. 5.40, p. 177 or [47] Titterton, ch. 12.3.1.1, eq. 12.12, p. 342)

where

• is the skew-symmetric matrix of the absolute body rate,
• is the skew-symmetric matrix of the navigation frame rate,
• is the skew-symmetric matrix of the earth rotation in navigation frame coordinates ([17] Groves, ch. 5.4.1, eq. 5.41, p. 177), and
• is the skew-symmetric matrix of the transport rate (rotation of the local-navigation-frame with respect to the Earth) ([17] Groves, ch. 5.4.1, eq. 5.44, p. 177)

Similarly, the time time differential of the estimated matrix can be calculated:

where

• is the skew-symmetric matrix of the measured body rate,
• is the skew-symmetric matrix of the estimated turn rate of the navigation reference frame,

Substituting and into

Substituting into gives:

Now we make the following substitutions

• 
• 

and neglect error product terms. This leads to (same as [47] Titterton, ch. 12.3.1.1, eq. 12.17, p. 343)

Finally by performing an element by element comparison with

the equation can be expressed in vector form as:

The transport rate is defined as (see [17] Groves, ch. 5.4.1, eq. 5.44, p. 177 or [14] Gleason, ch. 6.2.3.2, eq. 6.15, p. 155)

The differential of left and right side neglecting errors in and ( and )

The Earth rotation in local-navigation-frame coordinates is given

The differential of left and right side is

Substituting and into gives:

Velocity Equations

The equations here mainly follow [47] Titterton, ch. 12.3.1.2, p. 343f

The time derivative of the velocity in local-navigation-frame coordinates is (see [47] Titterton, ch. 3.7.1, eq. 3.69, p. 47 or [24] Jekeli, ch. 4.3.4, eq. 4.88, p. 127)

where

• is the velocity with respect to the Earth in local-navigation frame coordinates,
• is the specific force vector as measured by a triad of accelerometers and resolved into local-navigation frame coordinates
• is the turn rate of the Earth expressed in local-navigation frame coordinates
• is the turn rate of the local frame with respect to the Earth-fixed frame, called the transport rate, expressed in local-navigation frame coordinates
• is the local gravity vector which is a combination of (see [47] Titterton, ch. 3.7.1, eq. 3.75, p. 48)
  • the local gravitation vector (caused by effects of mass attraction)
  • the centrifugal acceleration caused by the Earth's rotation

The estimated velocity follows the same propagation

Differencing equation and we get

• Substituting equation :
• and writing
  • 
  • 
  • 
  • 
  • 

we get

Now neglecting error product terms and

This is equal to [47] Titterton, ch. 12.3.1.2, eq. 12.21, p. 344, except the sign of the gravity error term.

Substituting equations , , and leads to

The gravity error can be expressed as (see [17] Groves, ch. 14.2.4, eq. 14.59, p. 586)

where

• is the local navigation frame down unit vector
• is assumed
• centrifugal term and latitude-error dependence have been neglected

Reordering the terms leads to the final form

Error Equations comparison

Titterton

[47] Titterton, ch. 12.3.1.3, eq. 12.28, p. 345 has different sign in equations , , ,

As all signs related to the attitude error have a different sign, it can be explained by the attitude error being defined in the opposite direction. To check this, we can look at the definition of the correction of the state with the attitude errors ([17] Groves, ch. 14.1.1, eq. 14.7, p. 564)

while Titterton rearranges equation and defines the correction as ([47] Titterton, ch. 13.6.2.3, eq. 13.15, p. 407)

Therefore the attitude error is defined differently in Groves and Titterton, which explains the sign change in the error equations.

Gleason

[14] Gleason, ch. 6.2.5, eq. 6.21-6.23, p. 157 define the same error equations as Titterton, but has a bracket mistake inside the velocity equation (bracket should include the '2')

Noureldin

Extracting the equations from [35] Noureldin, ch. 6.1.5, eq. 6.76, p. 219 gives

Reordering roll/pitch and east/north gives

In [35] ch. 2.2.6.1, p. 32 Noureldin defines the yaw angle as measured counterclockwise from north which is the opposite measurement direction of the NED convention. Therefore all yaw terms have to flip the sign.

Next we replace the up component with a down component

As in [17] Groves, all signs concerning the attitude errors are now switched comparing to [47] Titterton. This leads to the same assumption, that the attitude errors are defined differently. And this can easily be shown by looking at [35] Noureldin, ch. 6.1.3, eq. 6.44, p. 212

Comparing this with equation we can see that the attitude error is defined in the opposite direction.