A Novel Approach for Kalman Filter Tuning for Direct and Indirect Inertial Navigation System/Global Navigation Satellite System Integration

1. Introduction

The integration of an inertial navigation system (INS) and a Global Navigation Satellite System (GNSS) is an essential technique for delivering accurate position, velocity, and attitude estimates in both civil and military navigation applications. Effective state estimators are crucial to ensuring the accuracy of these estimates, with the Extended Kalman Filter (EKF) being one of the most widely adopted methods. The EKF can be used for nonlinear systems, where the model equations are linearized locally around the current state estimate during each iteration. The EKF assumes that both process noise and measurement noise are Gaussian, with zero mean [1]. Furthermore, it assumes that the covariance matrices of the process noise

Q

and the measurement noise

R

are known [2]. However, in real-world systems, these conditions are not always met, especially in INS/GNSS integration scenarios, leading to potential degradation in estimation accuracy.

The tuning of an EKF involves optimizing matrices

Q

and

R

to accurately estimate the states of a nonlinear system. This process starts with defining the initial state and covariance estimates, often based on system knowledge and initial measurements. The process noise covariance matrix

Q

and measurement noise covariance matrix

R

are crucial for the EKF performance and require careful tuning. These matrices are usually derived from empirical data or through iterative experimentation. Fine tuning involves adjusting

Q

and

R

to balance responsiveness to new measurements with stability against noisy data. Extensive simulation and field testing are typically required to ensure robustness across various operating conditions [3,4,5,6].

In INS/GNSS integration, the tuning process for an EKF is iterative and time-consuming. It usually begins with a static error estimate obtained through Allan variance analysis [7,8]. This initial estimate is often optimistic and requires extensive static data collection over many hours or even days, serving as a starting point for further adjustments. Subsequent tuning involves discrete parameter adjustments and evaluating the position drift during GNSS outages [7].

To establish the order of parameter tuning, the priority can be set by evaluating the drift caused by them during GNSS signal interruptions. These parameters need to be tested in various scenarios with variable dynamics, which is challenging without extensive field test data. Additionally, the size of the discrete steps chosen in their adjustments influences their final optimization. Consequently, the ideal parameters are rarely known, and those selected are often non-ideal [7,9,10].

Since the 1970s, various methods for estimating the covariance matrices

Q

and

R

have been proposed. Mehra [11,12] was a pioneer in this field, followed by Carew et al. [13] and Belanger [14]. Despite these initial advances, progress in tuning the covariance matrices stagnated for almost three decades. In 2006, Odelson et al. introduced the ALS (Autocovariance Least Squares) method for the estimation of

Q

and

R

, with subsequent improvements presented by Rajamani et al. [15] and Akesson et al. [16]. However, these methods presented limitations in Multiple-Input Multiple-Output (MIMO) systems, as demonstrated by Matisko [17]. In recent years, traditional methods have been refined, and new methods have been developed to improve the tuning of the Kalman filter. Allan variance, for example, has been used to characterize and model noise in inertial sensors, allowing for a better estimation of the

Q

matrix [18,19,20]. Additionally, neural networks have been employed to dynamically adjust the tuning parameters of the Kalman filter, adapting to changes in system characteristics and noise in real time [21,22,23]. Genetic algorithms have also been applied to optimize the tuning of the Kalman filter, exploring an ample space of possible values for

Q

and

R

and selecting the best parameters based on performance criteria [3,24,25].

This paper presents a novel approach for tuning the EKF, utilizing Monte Carlo simulations and taking advantage of the Central Limit Theorem (CLT). The Monte Carlo method enables the simulation and statistical analysis of filter tuning parameters across a diverse range of scenarios [26,27], providing a robust way to estimate uncertainties and complex system behaviors. The CLT ensures that, as the number of samples increases, the distribution of parameter estimates converges to a normal distribution [28,29], offering a theoretical basis for tuning the process noise covariance matrix

Q

of the EKF.

This work proposes using Monte Carlo simulations to calculate the covariance matrix $Q$ , utilizing ideal data from inertial sensors along a trajectory in conjunction with their error models applied to navigation. This method allows for extracting the variances of the $Q$ matrix for both direct and indirect filter implementations. This approach facilitates quicker and more precise filter tuning, resulting in more reliable position and velocity estimates, especially in scenarios where the GNSS signal is degraded. The results are compared with the traditional EKF tuning method and those obtained from the inertial platforms of Castro Leite Consultoria, PINA-F, and PINA-M.

The remainder of this paper is organized as follows: Section 2 explains the concepts of INS/GNSS integration. Section 3 details the novel approach for Kalman filter tuning methodology. Section 4 describes the testing procedure. Section 5 presents the simulations and experimental tests. Section 6 discusses the results obtained. Finally, Section 7 provides the concluding remarks.

2. INS/GNSS Concepts

This section will discuss the main concepts of INS/GNSS integration.

2.1. Inertial Navigation System (INS)

Inertial navigation is an autonomous process of calculating position by the double integration of acceleration at a point whose position needs to be determined [30]. This involves finding position, attitude, and velocity relative to the Earth or the inertial space.

An inertial navigation system (INS) typically consists of three accelerometers and three gyroscopes to measure accelerations in three dimensions and rotation rates around three axes. The development of Micro-Electro-Mechanical System (MEMS) technology has significantly expanded the application area of INS. Today, an Inertial Measurement Unit (IMU) generally includes a three-degree-of-freedom (3-DoF) gyroscope and a three-degree-of-freedom accelerometer, allowing for the precise measurement of motion and orientation in compact and cost-effective packages [31].

Nowadays, strapdown systems are more commonly used, where the gyroscopes and accelerometers are mounted directly to the structure of the vehicle or strapped onto the body segment. Strapdown mechanization (or INS mechanization) is the process of determining the navigation states (position, velocity, and attitude) from raw inertial measurements by solving the differential equations that describe the system’s motion. For this purpose, it is essential to establish a consistent reference frame.

The mechanization differential equations are formulated in the navigation frame, also known as the local-level frame or NED (North, East, Down) frame. This frame is fixed to the Earth’s surface, with its axes aligned to point towards geographic north, east, and downward (towards the center of the Earth). The ECEF (Earth-Centered Earth-Fixed) frame, in contrast, is a Cartesian coordinate system that rotates with the Earth, making it suitable for representing global positions in a geocentric reference system. The body frame is fixed to the vehicle itself, with its axes aligned along the forward, right, and downward directions of the vehicle. Finally, the inertial frame, specifically the Earth-Centered Inertial (ECI) frame, is centered at the Earth’s center and does not rotate with the Earth [32]. This work uses the WGS-84 model for the ECEF frame and the J2000 frame for the inertial reference.

The dynamic equations used for mechanization in the navigation frame are presented below and are detailed in [32]. In this context, the superscript indicates the frame in which the variable is expressed, and the subscript denotes the reference frame for the transformation: n refers to the navigation frame, e refers to the ECEF frame, i refers to the ECI frame, and b refers to the body frame.

Position Update:
where $p^{n} = {[ϕ, λ, h]}^{T}$ , with $ϕ$ representing the latitude, $λ$ the longitude, and h the height relative to the Earth ellipsoid. The matrix $D^{- 1}$ is a $3 \times 3$ matrix that transforms the velocity vector from navigation coordinates to geodetic coordinates in the ECEF frame. The vector $v^{n} = {[v_{n}, v_{e}, v_{d}]}^{T}$ represents the velocity in the NED frame, with components along the north ( $v_{n}$ ), east ( $v_{e}$ ), and down ( $v_{d}$ ) directions.
Velocity Update:
${\dot{v}}^{n} = R_{b}^{n} f^{b} + g^{n} - (Ω_{e n}^{n} + 2 Ω_{i e}^{n}) v^{n}$
(2)
where $R_{b}^{n}$ is the direction cosine matrix from the body frame to the navigation frame, $f^{b}$ is the specific force measured by accelerometers in the body frame, $g^{n}$ is the gravity vector, $Ω_{e n}^{n}$ is the skew-symmetric matrix representing the transport rate with respect to the ECEF frame expressed in the navigation frame, and $Ω_{i e}^{n}$ is the skew-symmetric matrix representing the Earth’s rotation rate with respect to the inertial frame expressed in the navigation frame.
Attitude Update:
${\dot{R}}_{b}^{n} = R_{b}^{n} (Ω_{i b}^{b} - Ω_{i n}^{b})$
(3)
where $Ω_{i b}^{b}$ is the skew-symmetric matrix representing the angular rate of the body frame with respect to the inertial frame expressed in the body frame, and $Ω_{i n}^{b}$ is the skew-symmetric matrix representing the angular rate of the navigation frame with respect to the inertial frame expressed in the body frame.

These equations allow the continuous update of navigation states based on the inertial sensor measurements. Strapdown systems offer advantages in terms of simplicity and reliability, as they avoid the complexities and potential failure points associated with gimbaled systems, which use mechanical gyroscopes and rotating platforms to stabilize and orient sensors.

For a comprehensive treatment of strapdown inertial navigation systems and the associated mechanization equations, refer to Savage [30], Farrell [10], and Groves [1].

2.2. Global Navigation Satellite System (GNSS)

The Global Navigation Satellite System (GNSS) is a technology that allows for the determination of position, velocity, and time anywhere on Earth using satellite signals. GNSS includes various satellite constellations, such as the United States’ GPS, Russia’s GLONASS, the European Union’s Galileo, and China’s BeiDou [33,34]. These systems work by transmitting radio signals from satellites in orbit around the Earth. GNSS receivers on the Earth’s surface capture these signals and use the differences in arrival times to calculate the receiver’s position. GNSS is widely used in applications such as terrestrial, aerial, and maritime navigation, precision agriculture, surveying, and environmental monitoring [35]. Modern GNSS receivers can utilize multiple constellations simultaneously, enhancing the accuracy and robustness of measurements.

Despite these advancements, GNSS is still subject to several types of errors that can affect measurement accuracy. The main errors include satellite ephemeris errors (inaccuracies in the transmitted orbital information), satellite clock errors, ionospheric delays (caused by the dispersion of GNSS signals in the ionosphere), tropospheric delays (due to the refraction of signals in the troposphere), multipath effects (signals reflected off surfaces before reaching the receiver), and receiver measurement errors [34].

2.3. INS/GNSS Integration

In INS/GNSS integration, the nominal trajectory is often unknown in advance. Therefore, the current best estimate of the actual trajectory is used as the nominal trajectory. When Kalman filtering is applied to a system that has been linearized around this estimate, it is known as the EKF [10,32,36]. INS/GNSS integration can be performed in two modes: direct (state model) and indirect (error state model). In direct integration, the Kalman filter directly estimates states such as position, velocity, and attitude. In indirect integration, the Kalman filter is used to estimate the errors of the state vector of the inertial navigation algorithm, and these errors are subsequently corrected. Additionally, two types of error feedback mechanisms are employed: open-loop and closed-loop. In the open-loop configuration, corrections to position, velocity, and attitude are applied externally to the INS, with the estimated errors subtracted from the INS solution at each iteration. In contrast, the closed-loop configuration feeds the EKF error estimates back into the system, continuously correcting the INS solution. This feedback keeps the INS errors small, ensuring that the linearity assumption required for the EKF technique is maintained throughout the process [32].

Furthermore, there are different levels of coupling in INS/GNSS integration. Loosely coupled integration processes the GNSS and INS data separately before combining them. This method is simple and efficient, but less robust in environments with degraded GNSS signals. Tightly coupled integration directly incorporates GNSS measurements into the INS navigation equations, providing greater accuracy and robustness when dealing with weak or intermittent GNSS signals [37]. Ultra-tightly coupled integration goes further, directly integrating raw GNSS signal measurements into the navigation filter, resulting in greater resilience to degraded GNSS environments and improved performance in high-dynamic scenarios [38]. The integration of INS and GNSS in this work is based on a loosely coupled approach with a closed-loop configuration, as the trajectory is not known in advance.

2.4. Extended Kalman Filter (EKF)

The Extended Kalman Filter (EKF) is a widely used algorithm for state estimation in nonlinear systems. It extends the traditional Kalman filter (KF), which is designed for linear systems, by linearizing around the current estimate to handle the nonlinearity. The following subsections presents the EKF equations for both direct and indirect navigation modes.

2.4.1. Direct Mode (State Model)

In the Direct EKF (DEKF), also known as the state model, the states are estimated based on the mathematical model of the system rather than the error dynamics of the states [39]. Thus, the DEKF directly estimates the navigation states, which in this work are represented by the following state vector:

$x = {[p^{T}, v^{T}, ϵ^{T}]}^{T},$

(4)

where $ϵ$ represents the attitude in Euler angles, specifically roll ( $ϕ$ ), pitch ( $θ$ ), and yaw ( $ψ$ ).

The general system of differential equations that governs the state-space model used by the EKF can be expressed as follows:

$\dot{x} (t) = f (x (t), u (t), t) + w (t),$

(5)

$z (t) = h (x (t), t) + v (t),$

(6)

where $x (t)$ is the state vector as a function of the time t, $u (t)$ is the deterministic forcing function, $w (t)$ is the model noise, $z (t)$ is the measurement vector, $v (t)$ is the measurement noise, and $f$ and $h$ are nonlinear known functions [40].

The discrete state of process and measurements of the DEKF are defined as follows:

$x_{k} = Φ_{k - 1} x_{k - 1} + w_{k - 1},$

(7)

$z_{k} = H_{k} x_{k} + v_{k},$

(8)

where $x$ represents the state vector as defined in Vector (4), $Φ$ is the transition matrix, $H$ is the observation matrix, and k denotes the discrete time step.

The transition matrix

Φ_{k - 1}

can be found based on the first-order Taylor series approximation [41,42]:

$Φ_{k - 1} \approx I_{9} + Δ t [\begin{matrix} 0_{3 \times 3} & I_{3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & - F_{23} \\ 0_{3 \times 3} & 0_{3 \times 3} & F_{33} \end{matrix}],$

(9)

$F_{23} = R_{b}^{n} \cdot [\begin{matrix} 0 & - f_{D} & f_{E} \\ f_{D} & 0 & - f_{N} \\ - f_{E} & f_{N} & 0 \end{matrix}],$

$F_{33} = [\begin{matrix} 0 & - ω_{D} & ω_{E} \\ ω_{D} & 0 & - ω_{N} \\ - ω_{E} & ω_{N} & 0 \end{matrix}],$

where

$Δ t$ is the time step interval;
$I_{9}$ is the 9 × 9 identity matrix;
$I_{3}$ is the 3 × 3 identity matrix;
$0_{3 \times 3}$ is a 3 × 3 zero matrix;
$R_{b}^{n}$ is the rotation matrix from body frame to navigation frame, which follows the 3-2-1 (yaw–pitch–roll) rotation sequence;
$f_{N}$ , $f_{E}$ , and $f_{D}$ are components of specific force in the north, east, and down directions, respectively;
$ω_{N}$ , $ω_{E}$ , and $ω_{D}$ are the angular velocities in the north, east, and down directions, respectively.

In the next equations, $I_{m}$ represents an identity matrix of order m, and $0_{m \times n}$ represents a zero matrix of dimension $m \times n$ .

The observations of the algorithm consist of GNSS positions and velocities; therefore,

$H = [\begin{matrix} I_{3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3} & 0_{3 \times 3} \end{matrix}] .$

(10)

Also, define the following:

$\begin{matrix} Q_{k - 1} & = E [w_{k - 1} w_{k - 1}^{T}], \\ R_{k} & = E [v_{k} v_{k}^{T}], \\ P_{k - 1}^{+} & = E [({\hat{x}}_{k - 1}^{+} - x_{k - 1}) {({\hat{x}}_{k - 1}^{+} - x_{k - 1})}^{T}], \\ P_{k}^{-} & = E [({\hat{x}}_{k}^{-} - x_{k}) {({\hat{x}}_{k}^{-} - x_{k})}^{T}], \\ P_{k}^{+} & = E [({\hat{x}}_{k}^{+} - x_{k}) {({\hat{x}}_{k}^{+} - x_{k})}^{T}], \end{matrix}$

(11)

where $E [\cdot]$ is the expected value operator, and $Q$ and $R$ represent the covariance matrices of the process noise and measurement noise, respectively, $P$ is error covariance matrix, the superscript (−) denotes the a priori estimate, and superscript (+) denotes the a posteriori estimate.

Once the process and measurement models are defined by the previous equations, the DEFK procedure can be summarized as follows:

Prediction—Estimates the states for the next time step using the accelerations and current angular velocity combined with the states produced by the DEKF for the previous time step:
${\hat{x}}_{k}^{-} = Φ_{k - 1} {\hat{x}}_{k - 1}^{+},$
(12)
$P_{k}^{-} = Φ_{k - 1} P_{k - 1}^{+} Φ_{k - 1}^{T} + Q_{k - 1} .$
(13)
Kalman Gain—Finds the Kalman gain $K$ based on the estimation produced in the Prediction step:
$K_{k} = P_{k}^{-} H_{k}^{T} {(H_{k} P_{k}^{-} H_{k}^{T} + R_{k})}^{- 1} .$
(14)
Update—Uses measurements at time step k and the Kalman gain to correct the estimation produced in the Prediction step:
${\hat{x}}_{k}^{+} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - H_{k} {\hat{x}}_{k}^{-}),$
(15)
$P_{k}^{+} = (I - K_{k} H_{k}) P_{k}^{-} .$
(16)

2.4.2. Indirect Mode (Error State Model)

In the Indirect EKF (IEKF), also known as the error state model, the EKF estimates the errors in the navigation states rather than the states themselves. This approach involves maintaining a reference trajectory and estimating deviations from this trajectory. Therefore, in this work, the navigation error states are represented by the error state vector:

$δ x_{k} = {[δ p^{T}, δ v^{T}, δ ϵ^{T}]}^{T} .$

(17)

The continuous-time system dynamic model and measurement model of the IEKF are defined as follows:

$δ \dot{x} (t) = f (δ x (t), t) + G w (t),$

(18)

$z (t) = h (δ x (t), t) + v (t),$

(19)

where $f$ and $h$ are the nonlinear functions of the error state vector, $G$ is the process noise gain matrix, and $w$ and $v$ represent process and measurement noise, respectively.

The discrete state of process and measurement models of the IEKF are defined as follows:

$δ x_{k} = Φ_{k - 1} δ x_{k - 1} + G_{k - 1} w_{k - 1},$

(20)

$z_{k} = H_{k} δ x_{k} + v_{k},$

(21)

where $δ x$ represents the error state vector as defined in Vector (17), $z$ is the measurement vector, $Φ$ is the transition matrix, $H$ is the observation matrix, and and k denotes the discrete time step.

The transition matrix

Φ_{k - 1}

can be found based on the first-order Taylor series approximation [10,42]:

$Φ_{k - 1} \approx I_{9} + Δ t [\begin{matrix} 0 & 0 & \frac{ρ_{E}}{R_{e}} & \frac{1}{R_{e}} & 0 & 0 & 0 & 0 & 0 \\ \frac{- ρ_{D}}{cos (ϕ)} & 0 & \frac{- ρ_{N}}{R_{e} cos (ϕ)} & 0 & \frac{1}{R_{e} cos (ϕ)} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & - 1 & 0 & 0 & 0 \\ F_{41} & 0 & F_{43} & k_{D} & 2 ω_{D} & - ρ_{E} & 0 & f_{D} & - f_{E} \\ F_{51} & 0 & F_{53} & F_{54} & F_{55} & F_{56} & - f_{D} & 0 & f_{N} \\ - 2 v_{E} Ω_{D} & 0 & F_{63} & 2 ρ_{E} & - 2 ω_{N} & 0 & f_{E} & - f_{N} & 0 \\ - Ω_{D} & 0 & \frac{ρ_{N}}{R_{e}} & 0 & - \frac{1}{R_{e}} & 0 & 0 & ω_{D} & - ω_{E} \\ 0 & 0 & \frac{ρ_{E}}{R_{e}} & \frac{1}{R_{e}} & 0 & 0 & - ω_{D} & 0 & ω_{N} \\ F_{91} & 0 & \frac{ρ_{D}}{R_{e}} & 0 & \frac{tan (ϕ)}{R_{e}} & 0 & ω_{E} & - ω_{N} & 0 \end{matrix}],$

(22)

$\begin{matrix} Ω_{N} & = ω_{i e} cos (ϕ) & F_{41} & = - 2 Ω_{N} v_{e} - \frac{ρ_{N} v_{e}}{{cos}^{2} (ϕ)} \\ Ω_{D} & = - ω_{i e} sin (ϕ) & F_{43} & = ρ_{E} k_{D} - ρ_{N} ρ_{D} \\ ρ_{N} & = \frac{v_{e}}{R_{e}} & F_{51} & = 2 (Ω_{N} v_{n} + Ω_{D} v_{d}) + \frac{ρ_{N} v_{n}}{{cos}^{2} (ϕ)} \\ ρ_{E} & = - \frac{v_{n}}{R_{e}} & F_{53} & = - ρ_{E} ρ_{D} - k_{D} ρ_{N} \\ ρ_{D} & = - \frac{v_{e} tan (ϕ)}{R_{e}} & F_{54} & = - (ω_{D} + Ω_{D}) \\ ω_{N} & = Ω_{N} + ρ_{N} & F_{55} & = k_{D} - ρ_{E} tan (ϕ) \\ ω_{E} & = ρ_{E} & F_{56} & = ω_{N} + Ω_{N} \\ ω_{D} & = Ω_{D} + ρ_{D} & F_{63} & = ρ_{N}^{2} + ρ_{E}^{2} - \frac{2 g}{R_{e}} \\ k_{D} & = \frac{v_{d}}{R_{e}} & F_{91} & = Ω_{N} + \frac{ρ_{N}}{cos {(ϕ)}^{2}} \end{matrix}$

where

$R_{e}$ is the Earth’s mean radius;
$ω_{i e}$ is the Earth’s rotation rate;
$Ω_{N}$ and $Ω_{D}$ are components of the angular velocity due to Earth’s rotation;
$ρ_{N}$ , $ρ_{E}$ , and $ρ_{D}$ are components of the relative velocity with respect to the Earth’s surface;
$v_{e}$ , $v_{n}$ , and $v_{d}$ are the eastward, northward, and downward components of the velocity, respectively;
$ϕ$ is the latitude;
$ω_{N}$ , $ω_{E}$ , and $ω_{D}$ are the angular velocities incorporating both the Earth’s rotation and the relative velocities;
$f_{N}$ , $f_{E}$ , and $f_{D}$ are components of specific force;
$k_{D}$ is a component of the velocity.

The matrix

G

projects the process noise

w

into the state space, accounting for uncertainty in the state estimation [10], thus:

$G = [\begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} \\ - R_{b}^{n} & 0_{3 \times 3} \\ 0_{3 \times 3} & R_{b}^{n} \end{matrix}],$

(23)

The observations of the algorithm consist of GNSS positions and velocities, using the same matrix as in Equation (10).

Also, define the following:

$\begin{matrix} Q_{k - 1} & = E [w_{k - 1} w_{k - 1}^{T}], \\ R_{k} & = E [v_{k} v_{k}^{T}], \\ P_{k - 1}^{+} & = E [(δ {\hat{x}}_{k - 1}^{+} - δ x_{k - 1}) {(δ {\hat{x}}_{k - 1}^{+} - δ x_{k - 1})}^{T}], \\ P_{k}^{-} & = E [(δ {\hat{x}}_{k}^{-} - δ x_{k}) {(δ {\hat{x}}_{k}^{-} - δ x_{k})}^{T}], \\ P_{k}^{+} & = E [(δ {\hat{x}}_{k}^{+} - δ x_{k}) {(δ {\hat{x}}_{k}^{+} - δ x_{k})}^{T}], \end{matrix}$

(24)

Once the process and measurement models are defined by the previous equations, the IEFK procedure can be summarized as follows:

Prediction—Estimates the states for the next time step using the accelerations and current angular velocity combined with the states produced by the IEKF for the previous time step:
$δ {\hat{x}}_{k}^{-} = Φ_{k - 1} δ {\hat{x}}_{k - 1},$
(25)
$P_{k}^{-} = Φ_{k - 1} P_{k - 1}^{+} Φ_{k - 1}^{T} + G_{k - 1} Q_{k - 1} G_{k - 1}^{T} .$
(26)
Kalman Gain—Finds the Kalman gain $K$ based on the estimation produced in the Prediction step:
$K_{k} = P_{k}^{-} H_{k}^{T} {(H_{k} P_{k}^{-} H_{k}^{T} + R_{k})}^{- 1} .$
(27)
Update—Uses measurements at time step k and the Kalman gain to correct the estimation produced in the Prediction step:
$δ {\hat{x}}_{k}^{+} = δ {\hat{x}}_{k}^{-} + K_{k} (δ z_{k} - H_{k} δ {\hat{x}}_{k}^{-}),$
(28)
$P_{k}^{+} = (I - K_{k} H_{k}) P_{k}^{-} .$
(29)

where $δ z_{k}$ represents the difference between the positions and velocities estimated by the INS and those measured by the GNSS.

2.5. Standard Tuning of INS/GNSS EKF

In this subsection, the discussion is based on the work presented by Groves [1].

The primary sources of system noise in the INS are the random walk of velocity error caused by noise in the accelerometer’s specific-force measurements and the random walk of attitude error due to noise in the gyroscope’s angular-rate measurements. Additionally, if separate dynamic bias states for the accelerometer and gyroscope are not estimated, the in-run variation in the accelerometer and gyroscope biases can be approximated as white noise.

For small propagation intervals (≤0.2 s), the system noise covariance matrix can be approximated using Equation (30) for the DEKF and Equation (31) for the IEKF, where

S_{r g}

and

S_{r a}

are the variances of the gyroscope random noise and accelerometer random noise, respectively.

$Q_{I N S_{d e k f}} = [\begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & S_{r a} I_{3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & S_{r g} I_{3} \end{matrix}]$

(30)

$Q_{I N S_{i e k f}} = [\begin{matrix} S_{r a} I_{3} & 0_{3 \times 3} \\ 0_{3 \times 3} & S_{r g} I_{3} \end{matrix}]$

(31)

The power spectral densities (PSDs) of accelerometers and gyroscopes at frequencies below 1 Hz are generally white, meaning the standard deviation of the average specific force and angular rate noise is inversely proportional to the square root of the averaging time. As a result, inertial sensor noise is typically characterized by the root of PSD. The common units used are $μ$ g/ $\sqrt{Hz}$ for accelerometer noise, where 1 $μ$ g/ $\sqrt{Hz} = 9.80665 \times 10^{- 6}$ m s^−1.5, and $° / \sqrt{hr}$ or $° / hr / \sqrt{Hz}$ for gyroscope noise, with $1^{\circ} / hr = 2.909 \times 10^{- 4}$ rad s^−0.5 and $1^{\circ} / \sqrt{Hz} = 4.848 \times 10^{- 6}$ rad s^−0.5. The standard deviations of these random noise samples are obtained by multiplying the corresponding root PSDs by the square root of the sampling rate, or by dividing by the square root of the sampling interval. Since the variance is the square of the standard deviation, this process also allows for the determination of the noise variance over the sampling period. It is important to note that white random noise cannot be calibrated or compensated for, as it lacks correlation between past and future values.

The tuning of the matrix $R$ will not be discussed in this work, as its values will be derived from the GNSS receiver data. Consequently, the novel approach proposed for tuning the INS/GNSS integration will focus on the matrix $Q$ and will be discussed in the next section.

3. Novel Approach EKF Tuning

The proposed EKF tuning method for INS/GNSS integration in both direct and indirect modes is based on the Monte Carlo technique, using ideal data from a predetermined trajectory and applying the inertial sensor error model to it. The method presented in this paper involves generating multiple trajectory measurements from a reference trajectory, with each measurement incorporating different random values of the accelerometer and gyroscope error model coefficients. These simulations are used to pre-calculate the variance matrices $Q$ for both EKF modes.

Figure 1 illustrates the flow of the Monte Carlo simulation process. The process begins with the use of the true specific force and angular rate from a reference trajectory, which are then combined with IMU errors to generate IMU measurements. Next, the INS mechanization is performed to estimate the navigation states. During the INS loop, the navigation variances and IMU variances are calculated and saved. The process is repeated 200 times, and the final variances produced yield the navigation variances for the DEKF and the IMU variances for the IEKF.

The color coding is as follows: orange blocks represent inputs, blue blocks represent the main computational steps of the Monte Carlo simulation, which include the INS loop, and green blocks represent the output variance matrices for both EKF modes.

3.1. Mathematical Modeling

In this subsection, the mathematical equations required for the implementation of the algorithm are discussed. Detailed explanations of the methods and equations used to ensure its proper functioning are provided.

For the input data, ideal inertial sensor data are used, which can be obtained through the trajectory generators proposed by Savage [43] or tools like MATLAB’s Navigation Toolbox. The IMU errors are derived from the manufacturer’s datasheet, and the provided data are implemented accordingly. The next topic will discuss the IMU errors in detail and explain how they are integrated into the system.

3.1.1. Inertial Sensor Error Characteristics

In this subsection, the following equations are based on the work presented by Groves [1].

The accelerometer error model is given by Equation (32), which accounts for bias (

b_{a}

), errors due to the scale factor and cross-coupling (

M_{a}

, as shown in Equation (35)), and random noise (

w_{a}

${\tilde{f}}_{i b}^{b} = b_{a} + (I_{3 \times 3} + M_{a}) f_{i b}^{b} + w_{a},$

(32)

where $f_{i b}^{b}$ is the ideal specific force expressed in the body frame, obtained from the trajectory, and ${\tilde{f}}_{i b}^{b}$ is the computed specific force from the IMU.

The gyroscope error model is given by Equation (33), where the considered stochastic variables are bias (

b_{g}

), errors due to the scale factor and cross-coupling (

M_{g}

, as shown in Equation (35)), g-dependent bias (

G_{g}

), and random noise (

w_{g}

${\tilde{ω}}_{i b}^{b} = b_{g} + (I_{3 \times 3} + M_{g}) ω_{i b}^{b} + G_{g} f_{i b}^{b} + w_{g},$

(33)

where $ω_{i b}^{b}$ is the ideal angular rate expressed in the body frame, also obtained from the trajectory, and ${\tilde{ω}}_{i b}^{b}$ is the computed angular rate vector from the IMU.

When considering biases, it can be divided into static components,

b_{a s}

and

b_{g s}

, and dynamic components,

b_{a d}

and

b_{g d}

, where

$b_{a} = b_{a s} + b_{a d} and b_{g} = b_{g s} + b_{g d} .$

(34)

The static component, also referred to as the fixed bias, turn-on bias, or bias repeatability, encompasses the run-to-run variation in each instrument bias along with the residual fixed bias that remains after sensor calibration. This component remains constant during the IMU’s operation but can differ between runs. The dynamic component, also termed in-run bias variation or bias instability, changes over periods in the order of a minute and includes the residual temperature-dependent bias left after sensor calibration. Typically, the dynamic bias constitutes about 10% of the static bias [1].

The scale factor and cross-coupling errors for a nominally orthogonal set of accelerometers and gyroscopes can be represented by the following matrices:

$M_{a} = (\begin{matrix} s_{a, x} & m_{a, x y} & m_{a, x z} \\ m_{a, y x} & s_{a, y} & m_{a, y z} \\ m_{a, z x} & m_{a, z y} & s_{a, z} \end{matrix}), M_{g} = (\begin{matrix} s_{g, x} & m_{g, x y} & m_{g, x z} \\ m_{g, y x} & s_{g, y} & m_{g, y z} \\ m_{g, z x} & m_{g, z y} & s_{g, z} \end{matrix}),$

(35)

where the accelerometer and gyroscope scale factor errors within the IMU are represented by the vectors $s_{a} = (s_{a, x}, s_{a, y}, s_{a, z})$ and $s_{g} = (s_{g, x}, s_{g, y}, s_{g, z})$ , respectively. Additionally, $m_{a, α β}$ represents the cross-coupling coefficient for the specific force along the $β$ -axis sensed by the $α$ -axis accelerometer, and $m_{g, α β}$ represents the cross-coupling coefficient for the angular rate along the $β$ -axis sensed by the $α$ -axis gyroscope. Scale factor and cross-coupling errors are dimensionless and are typically expressed in parts per million (ppm) or as a percentage.

Once the inertial sensor data are obtained, their variances are calculated incrementally and the data are used in the INS mechanization equations.

3.1.2. INS Mechanization

For INS mechanization using Equations (1)–(3), the attitude is propagated using quaternions. These equations are a simplification of those presented by Savage [30], where they are detailed in both continuous and discrete forms. In this work, the continuous form of the equations was used, with the fourth-order Adams–Bashforth predictor method applied for numerical integration. A detailed explanation of these equations is beyond the scope of this work.

Propagation of attitude using quaternions is just one part of the INS process; understanding and accounting for gravitational effects is equally critical. Since accelerometers cannot distinguish the effects of the gravitational field in their acceleration measurements, accurate knowledge of the gravitational field is necessary for INS calculations. The gravity model used in this work is based on the WGS-84 standard, which accounts for the Earth’s oblateness. This model estimates gravitational components based on latitude, longitude, and height (lat, long, h) relative to the Earth’s center.

The results of the INS mechanization equations yield positions in terms of latitude, longitude, and height, with velocities expressed in the NED coordinate frame. Additionally, the variances of positions, velocities, and attitude are calculated incrementally. A detailed explanation of this incremental calculation will be provided in the next subsection.

3.1.3. Monte Carlo Simulation

The algorithm implements Welford’s method [44] for the incremental estimation of the mean and variance of a dataset, incorporating Bessel’s correction [45] for samples. These techniques are useful when dealing with large sequential datasets, and when efficient and accurate calculations of means and variances are needed.

Welford’s mean is calculated incrementally, allowing the mean to be updated as new elements are added to the dataset. The formula is as follows:

${\bar{x}}_{n} = {\bar{x}}_{n - 1} + \frac{x_{n} - {\bar{x}}_{n - 1}}{n},$

(36)

where n is the number of elements processed so far, $x_{n}$ is the most recent element, and ${\bar{x}}_{n}$ is the mean up to that point.

Welford’s variance is a natural extension, including Bessel’s correction to compensate for bias in the variance estimation when dealing with samples. The formula is as follows:

$s_{n}^{2} = \frac{n - 1}{n} \cdot s_{n - 1}^{2} + \frac{1}{n} \cdot {(x_{n} - {\bar{x}}_{n - 1})}^{2},$

(37)

where n is also the number of elements processed so far, $s_{n}^{2}$ is the variance up to that point, and $s_{n - 1}^{2}$ is the variance from the previous step.

By applying all the above equations, which encompass the inertial sensor error model, the INS mechanization equations, and the calculations for means and variances, the algorithm for the novel approach to Kalman filter tuning is described as presented in Algorithm 1:

Algorithm 1: Monte Carlo Simulation

where j is the index of the Monte Carlo loop, k is the index for the trajectory samples, and c is the index used to calculate the means and variances for each trajectory step according to the chosen interval, which in this case is 1 s. Additionally, $c t$ is an auxiliary index that counts the number of seconds until the specified interval is reached. totalTrajectorySamples represents the total number of samples in the trajectory dataset, insMechanization is a function that calculates the INS states based on the current states, and diag represents a diagonal matrix where the provided elements are placed along the main diagonal, with all off-diagonal elements set to zero.

The sensor error model is derived from the IMU datasheet, where not all error parameters need to be filled out. However, the more variables from the error equations that are defined, the more accurate the variance calculations will be. To introduce variability in the Monte Carlo simulation, the IMU errors are modeled as normally distributed random variables, each with a variance specific to its corresponding error parameter.

At the conclusion of the Monte Carlo simulation, the final variance values associated with the inertial sensors, accelerometers, and gyroscopes form the covariance matrix $Q$ for the IEKF. Similarly, the final variance values related to navigation parameters, such as positions, velocities, and attitudes, form the covariance matrix $Q$ for the DEKF.

Modeling inertial sensor errors is complex due to their non-Gaussian nature. In many applications, the Monte Carlo method generates samples assuming these errors follow a Gaussian distribution. In reality, inertial sensor errors often exhibit time-correlated characteristics, and a more accurate representation would involve using models such as a Markov process. To explain the accuracy and reliability of statistical analysis in Monte Carlo simulations for a non-Gaussian distribution, fundamental statistical principles are applied, one of which is the CLT.

3.2. Central Limit Theorem

The Central Limit Theorem (CLT) supports the proposed method by ensuring that, as the number of samples increases, the distribution of the sample tends to approach a normal distribution. This is particularly beneficial in Monte Carlo simulations, where the noise modeled by numerous random processes aggregates into a Gaussian distribution, simplifying the analysis and enhancing the reliability of statistical measures such as the covariance matrices derived from the simulation. This alignment of the sample distribution with the properties of Gaussian noise helps in making more accurate and robust predictions about the behavior of INS erros.

As discussed by Dobrushin [46] and Trevezas and Limnios [47], the application of the CLT to the Markov process is particularly relevant, as it allows for the modeling of complex stochastic processes where the transition probabilities change over time, ensuring that asymptotic normality can be achieved under certain conditions. This is especially pertinent for inertial sensor errors, which often follow a Markov process. These errors, including biases and random drifts in accelerometers and gyroscopes, can be modeled as first-order Markov processes [1,10,30]. This modeling captures the temporal correlations inherent in the sensor errors, allowing for more accurate tracking and prediction of navigation parameters.

In conclusion, the CLT within Monte Carlo simulations ensures that the distribution of tuning parameters estimates tends toward normality, even in complex stochastic processes like those modeled by first-order Markov processes. This approach simplifies the analysis and ensures that predictions regarding INS errors converge to values closer to the real ones, leading to more reliable INS/GNSS integrations.

4. Testing Procedure

The testing was conducted in a condominium located in São José dos Campos, Brazil, using a car. The test trajectory is illustrated in Figure 2. Two inertial navigation platforms with GPS assistance from Castro Leite Consultoria were used: the PINA-F and the PINA-M, as shown in Figure 3. The PINA-F, which is at Technology Readiness Level (TRL) 9, is equipped with Fiber Optic Gyroscopes (FOGs) and Quartz Pendulous Accelerometers (QPAs), while the PINA-M, at TRL 8, employs MEMS technology for its sensors (accelerometers and gyroscopes). The specifications for each platform can be seen in Table 1 and Table 2.

Four laps in the condominium were conducted for each platform, during which the GPS signal was intentionally suppressed at certain parts of the trajectory. The navigation results of the platforms are compared using algorithms with DEKF and IEKF under standard tuning conditions, as well as with those obtained using the novel tuning approach proposed in this study. These results will be discussed in the next section.

5. Results

This section presents the $Q$ matrix values derived from the proposed novel approach for Kalman filter tuning in both direct and indirect INS/GNSS integration. Additionally, the other parameters used to initialize the EKFs are detailed, along with the results obtained from the filter tunings in both the standard (STD) form and the proposed new approach (NA). These results are then compared with those obtained from the platforms.

To calculate the variances in the

Q

matrix using the NA tuning method, inertial data from a roller coaster trajectory, employed in the SIA project (Inertial Systems for Aerospace Application, Sistemas Inerciais para Aplicação Aeroespacial in Portuguese) at the Brazilian Institute of Aeronautics and Space (IAE, Instituto de Aeronáutica e Espaço in Portuguese), are used as the reference trajectory in the Monte Carlo simulation. This trajectory induces more pronounced accelerations and angular velocities, leading to larger variances. This approach is specifically designed to tune the filter for worst-case scenarios, thereby ensuring robustness under extreme conditions. The variance results, truncated to the fourth decimal place, are presented in Table 3 and Table 4 for the DEKF and IEKF, respectively. For the DEKF variances, it can be observed that the position variance values were high for the vertical channel due to its instability, a phenomenon discussed in detail by Savage et al. [30] in the context of inertial navigation systems.

For the other EKF parameters, all filters used the same calibrated inertial sensor data from the platforms, the same value for the matrix $R$ , and identical initial point values in the INS for each run, with all data originating from the platforms. For the initial values of the covariance matrix $P$ , all filters used the same values, except for those compared with the PINA-F platform. This exception was necessary because the PINA-F platform does not provide standard deviation data in its log.

The following subsections present the results obtained from the filter tunings. The reference trajectory, shown in some graphs, was obtained using Google Earth and is therefore only applicable to latitude and longitude plots. In the graph legends, “STD Q” denotes the standard tuning, while “NA Q” represents the tuning based on the novel approach proposed in this work. To enhance visualization, areas where the GNSS signal experienced significant degradation are enlarged in the graphs.

5.1. DEKF

This subsection will present the results of the tunings for the direct filter.

5.1.1. PINA-F

Figure 4 presents the results of the INS/GNSS integration for the PINA-F in the geodetic reference frame. No significant differences are observed between the STD and NA tuning. However, Figure 5, Figure 6 and Figure 7 reveal variations when the axes are analyzed independently. In Figure 5, there is degradation in altitude with the STD tuning, while the NA tuning maintains closer alignment with the platform results. In Figure 6, the filter with the NA tuning follows the platform results, whereas the STD tuning shows discrepancies across all three axes. Regarding the Euler angles, presented in Figure 7, the NA tuning shows results close to those of the platform, while the STD tuning diverges at the end of the trajectory, particularly in the roll and pitch angles.

5.1.2. PINA-M

Figure 8 presents the results of the INS/GNSS integration for the PINA-M in the geodetic reference frame. The NA tuning maintained the trajectory even during moments of GNSS signal interference, achieving better results in some parts of the trajectory than the platform itself, as seen in the enlarged area between latitudes −23.2428 and −23.2436 and longitudes −45.882 and −45.8815. In Figure 9, there is degradation in altitude with the STD tuning, and in latitude and longitude during periods without GNSS, while the NA tuning maintains closer alignment with the platform results. In Figure 10, the filter with the NA tuning follows the platform results, whereas the STD tuning shows discrepancies across all three axes. For the Euler angles, presented in Figure 11, the tunings show similar results overall, but the STD tuning exhibits greater divergence in the roll angle.

5.2. IEKF

This subsection will present the results of the tunings for the indirect filter.

5.2.1. PINA-F

Figure 12 presents the results of the INS/GNSS integration for the PINA-F in the geodetic reference frame. The NA tuning demonstrates a smaller error compared to the STD tuning, as shown in the enlarged area. In Figure 13, there is degradation in altitude with the STD tuning, while the NA tuning maintains closer alignment with the platform’s results. In Figure 14, the filter with the NA tuning follows the platform results for north and east velocities, whereas the STD tuning shows discrepancies. For down velocity, the filters diverged from each other. Regarding the Euler angles, presented in Figure 15, the filters showed similar results for all three axes.

5.2.2. PINA-M

Figure 16 presents the results of the INS/GNSS integration for the PINA-M in the geodetic reference frame. The NA tuning maintains the trajectory even during periods of GNSS signal interference, achieving better results compared to the platform itself, as highlighted in the enlarged areas. In Figure 17, the STD tuning shows degradation in altitude throughout the trajectory, as well as in latitude and longitude during GNSS outages, whereas the NA tuning remains closely aligned with the platform’s results. In Figure 18, the NA-tuned filter closely follows the platform’s results, while the STD tuning exhibits discrepancies across all three axes. For the Euler angles, presented in Figure 19, the STD tuning diverges from both the platform results and the NA tuning, particularly in the

ϕ

and

θ

axes.

6. Discussion

The results demonstrated that the proposed innovative tuning method achieved satisfactory results compared to commercial platforms, even when different sensor technologies were used. The method aims to quickly determine the appropriate values for the $Q$ matrix based on sensor datasheet values, thus eliminating the need for an experienced designer, as well as any heuristic assumptions or initial guesses.

The datasheet information provided by manufacturers, especially for lower-cost sensors, often does not perfectly correspond to the actual performance of the sensors. This discrepancy necessitates the performance of new Allan variance tests to obtain values closer to real-world performance. These tests are time-consuming, require a controlled environment, and demand expertise from the person conducting them. The proposed method converts the datasheet values into more accurate real-world values, streamlining the process and removing the need for a specialist.

Additionally, the implementation of the method on platforms with different sensor technologies, such as FOGs and MEMS, highlights the robustness and flexibility of the approach. Even in scenarios where the sensors have significantly different noise characteristics, the proposed tuning method successfully maintained trajectory accuracy, particularly during periods of GNSS signal degradation.

Future work will focus on further optimizing the tuning parameters. Specifically, the $Q$ matrix will be made adaptive to temperature variations, as inertial sensors are particularly sensitive to these changes. This adjustment would allow for even more precise tuning under variable operational conditions, potentially further improving the performance of inertial navigation systems.

Moreover, incorporating Real-Time Kinematic (RTK) data in future experiments could provide an even higher level of accuracy and serve as a more precise reference for evaluating the proposed method. A direct comparison of the proposed method with other recent automatic Kalman filter tuning methods, including those that use machine learning or genetic algorithms, would also be beneficial.

7. Conclusions

This work presents an analysis of the tuning methods for Extended Kalman Filters (EKFs) used in INS/GNSS integration. By comparing the standard (STD) tuning approach with a newly proposed approach (NA), the study demonstrates the efficacy of the NA tuning in maintaining trajectory accuracy, especially during periods of GNSS signal degradation. The NA tuning consistently shows better alignment with the platform results across various metrics, including altitude, latitude, longitude, velocities, and Euler angles. The findings indicate that the proposed tuning method not only enhances the performance of inertial navigation systems by providing more reliable and precise navigation solutions but also offers significant improvements in tuning efficiency. The proposed method achieves appropriate values for the $Q$ matrix based on the datasheet specifications of the inertial sensors, streamlining the tuning process and reducing the time required to reach these desired values.

Source link

Adalberto J. A. Tavares Jr. www.mdpi.com

Greenberg News

A Novel Approach for Kalman Filter Tuning for Direct and Indirect Inertial Navigation System/Global Navigation Satellite System Integration

1. Introduction

2. INS/GNSS Concepts

2.1. Inertial Navigation System (INS)

2.2. Global Navigation Satellite System (GNSS)

2.3. INS/GNSS Integration

2.4. Extended Kalman Filter (EKF)

2.4.1. Direct Mode (State Model)

2.4.2. Indirect Mode (Error State Model)

2.5. Standard Tuning of INS/GNSS EKF

3. Novel Approach EKF Tuning

3.1. Mathematical Modeling

3.1.1. Inertial Sensor Error Characteristics

3.1.2. INS Mechanization

3.1.3. Monte Carlo Simulation

3.2. Central Limit Theorem

4. Testing Procedure

5. Results

5.1. DEKF

5.1.1. PINA-F

5.1.2. PINA-M

5.2. IEKF

5.2.1. PINA-F

5.2.2. PINA-M

6. Discussion

7. Conclusions

Greenberg

1. Introduction

2. INS/GNSS Concepts

2.1. Inertial Navigation System (INS)

2.2. Global Navigation Satellite System (GNSS)

2.3. INS/GNSS Integration

2.4. Extended Kalman Filter (EKF)

2.4.1. Direct Mode (State Model)

2.4.2. Indirect Mode (Error State Model)

2.5. Standard Tuning of INS/GNSS EKF

3. Novel Approach EKF Tuning

3.1. Mathematical Modeling

3.1.1. Inertial Sensor Error Characteristics

3.1.2. INS Mechanization

3.1.3. Monte Carlo Simulation

3.2. Central Limit Theorem

4. Testing Procedure

5. Results

5.1. DEKF

5.1.1. PINA-F

5.1.2. PINA-M

5.2. IEKF

5.2.1. PINA-F

5.2.2. PINA-M

6. Discussion

7. Conclusions

Related Posts

Environmental Factor – September 2023: Diet and exposures in pregnancy: grantee tackles research, messaging

A Cerebral Multivoxel Magnetic Resonance Study

Environmental Factor – September 2023: Grantees in Alaska on front lines of climate change and health issues

Greenberg