An Unconventional Multiple Low-Cost IMU and GPS-Integrated Kinematic Positioning and Navigation Method Based on Singer Model

Abstract

To release the strong dependence of the conventional inertial navigation mechanization on the a priori low-cost inertial measurement unit (IMU) error model, this research applies an unconventional multi-sensor integration strategy to integrate multiple low-cost IMUs and a global positioning system (GPS) for mass-market automotive applications. The unconventional integration strategy utilizes a basic three-dimensional (3D) kinematic trajectory model as the system model to directly estimate navigational parameters, and it allows the measurements from all of the sensors independently participating in measurement updates. However, the less complex kinematic model cannot realize smooth transitions between different motion statuses for the road vehicle with acceleration maneuvers. In this manuscript, we establish a more practical 3D kinematic trajectory model based on a “current” statistical Singer acceleration model to realize smooth transitions for the maneuvering vehicle. In addition, taking advantage of the unconventional strategy, we individually model the systematic errors of each IMU and the measurements of all sensors, in contrast to most existing approaches that adopt the common-mode errors for different sensors of the same design. A real dataset involving a GPS and multiple IMUs is processed to validate the success of the proposed algorithm model under the unconventional integration strategy.

1. Introduction

With the popularity of intelligent vehicles, positioning and navigation technology is particularly important, because the foremost requirement for all of these safety and assistance systems is an accurate knowledge of the vehicular states (including vehicle position, vehicle velocity, vehicle acceleration, vehicle attitude, etc.) at all times [1]. High-precision sensors can certainly obtain very accurate vehicular states, but their high prices prevent them from being used in general-priced vehicles. Developing affordable sensors is the main research trend for driving applications [2]. On the other hand, the sensor should offer continuous and higher-update-rate observations under all circumstances for driving applications. Thus, considering cost-efficiency and high sampling frequency, a low-cost inertial measurement unit (IMU) is a good option and could autonomously provide navigation information. Furthermore, IMUs mostly contain a gyroscope and accelerometer for all three axes which can offer sufficient navigation information about a sudden acceleration or angular and heading change [3]. However, the accuracy of sensors decreases with the price reduction, and the performance is seriously affected by accumulated bias, drift, and sensor noises [4]. While an automotive-grade gyroscope typically gives drift performance of 1°/h, a microelectromechanical system (MEMS) gyroscope has a typical performance of 70°/h [1]. Plainly, low precision is one of the most critical obstacles in the development of a low-cost IMU, which limits its applications such as navigation and guidance [5]. An approach to design sensors for systems requiring better performance than the one with a single low-accuracy IMU may offer is to fuse the measurements of multiple low-accuracy IMUs to achieve the advantage of complementarity [6]. This “wisdom of the crowd” design also has, in addition to the improved measurement performance, the benefit of making it possible for sensor fault detection and diagnosis, thereby increasing the integrity and reliability of the system [6]. Many related studies confirmed the feasibility of this design, including Leszczynski (2014), Martin (2013), Skog (2014), Wahlström (2018), etc. [6,7,8,9]. However, these studies for improving the accuracy of MEMS IMUs only focused on the device itself.

The global positioning system (GPS) is by far the most widely used high-precision localization method. However, it is further constrained by the non-availability of location during signal outage in dense urban areas [10,11]. Additionally, for determining sudden acceleration and braking, the GPS shows very limited success due to the insufficient update rate. Although both the low-cost IMU and GPS have their merits and demerits, they are complementary [4]. All things considered, one preferable approach is to integrate multiple low-cost inertial measurement units (IMUs) and one GPS receiver for kinematic positioning and navigation in mass-market automotive applications.

The optimal filter design is the key to combining multiple sensors for accuracy improvement, and the adoption of the Kalman filter (KF) is widespread. Conventionally, the Kalman filter based on integration mechanization usually adopts the inertial navigation mechanization to estimate the error states and sensor systematic errors through error measurements on the basis of the aiding sensors [12,13,14]. Its strong dependence on the a priori inertial error model may limit the use of a low-cost IMU, because the time-variant noise model could be highly sensitive to the dynamic excitations and the temperature [15]. At the same time, the direct Kalman filter, which can accurately reflect the evolution of the real state, was also occasionally discussed and applied [16,17,18]. However, these system models used in Kalman filters are all based on the a priori inertial error model. In other words, in all cases, there is no essential difference, as the IMU measurements are only applied in free inertial navigation calculation and, thus, no measurement updates are performed in Kalman filtering between adjacent measurement epochs [19]. The severe drift of low-cost IMU systematic errors occurring during GPS outages can easily result in an intolerable free inertial navigation solution between two aiding measurement updates [20,21]. To address the realization of a better utilization of measurements from IMUs in Kalman filtering, especially with low-cost IMUs, several studies were carried out. An unconventional Kalman filter was already developed by Wang and Qian [19], specifically (1) utilizing a three-dimensional (3D) kinematic trajectory model as the central system model, so that the influences of the time-variant errors of low-cost IMUs on the inertial navigation solution are essentially alleviated; (2) allowing the measurements of all sensors, IMU included, directly and independently participating in measurement updates of Kalman filtering, so that the heavy dependence of the inertial navigation mechanization on the IMU measurements in the conventional integration strategy is released [15,19]. The unconventionality of this integration strategy mainly embodies in (1) implementation of the direct estimation of the whole-value states (navigational parameters) via sensor measurements instead of the error states via error measurements; (2) the lack of the need to make a distinction between the aiding sensors and the core sensors, since the raw observables (specific forces and angular rates) of IMUs directly participate in measurement updates as the raw outputs from sensors such as a GPS does, thereby implementing true multi-sensor integration.

This research applies the unconventional integration strategy for multiple low-cost IMUs and a GPS-integrated system. There are some characteristics and some corresponding defects from previous 3D kinematic trajectory models. Inevitably, a road vehicle is certain to experience severe or light irregular changes of velocity from time to time, and the changing velocity indicates the presence of acceleration. However, the jerk vector in the kinematic trajectory model is usually treated as process noise. In most cases, the a priori knowledge of vehicle maneuvers is very little. Because the maneuvering process is controlled by human forces, it is difficult to be accurately described with mathematical formulas and can only be artificially approximated under various assumptions. Therefore, a less complex kinematic model cannot realize smooth transitions between different motion statuses for the road vehicle, which leads to an intolerable positioning and navigation solution. One should avoid employing the most complex kinematic model as the core system model throughout [18]. An appropriate model will not only be able to properly model the vehicle maneuvers, but also exhibit mathematical tractability.

We found that the key problem is how to model the unknown target accelerations. Some mature maneuvering target models were proposed on this issue. So far, the most influential maneuvering target model is the Singer model proposed by Singer in 1970, which attempts to model large-scale maneuvers by assuming a time-correlated input process and incorporating the statistics into the subsequent filter design [22]. As a global statistical model, the Singer model considers all maneuvering possibilities of targets and can be applied to many types of maneuvering. However, the Singer model is essentially an a priori model, which has some limitations in effectively describing random maneuvers of the target. Based on the Singer model, a series of modifications were developed, including the “current” statistical (CS) model, adaptive current input statistical model (ACISM), jerk model, coordinate turn model, etc. [23,24]. The common feature of these models is that the maneuver of the target is regarded as the result of a time-correlated colored noise sequence, rather than a statistically independent white noise sequence. Among them, the ACISM considers the disturbance of maneuvering acceleration caused by environmental noises and other factors, the jerk model is more suitable for describing jerky maneuvering behaviors of highly maneuvering targets (such as aircraft), and the coordinate turn model is mainly applied to turning motions. There is no comparatively great environmental noise (such as atmospheric turbulence) or strong maneuvers in the application of this research. Therefore, the “current” statistical model, which can fulfil the needs of this study, is used to model the system acceleration. As one enhanced part of this manuscript, a more practical 3D kinematic trajectory model based on the “current” statistical Singer acceleration model (CSSAM) is established as the main part of the system equation so that the smooth transitions for the maneuvering vehicle can be realized.

In addition, the significant approach to how the low-cost IMU arrays are fused in this research involves enabling the direct use of measurements for individual IMUs and separately modeling their systematic errors in Kalman filtering, in contrast to most existing approaches working under the supposition of “the common-mode errors of different sensors of the same design” [7], which means modeling their systematic errors as a group of the shared states in KF from the view of algorithm design. Usually, one created a high-performance artificial or virtual IMU to be equivalent to this IMU array [8,25]. In general, those IMU systematic errors are different from run to run and in-run even with the same IMU sensor, which was recognized and largely admitted by the community of IMU manufacturers and users. The centralized filter fusion in Bancroft and Lachapelle (2011) could process the relative position, velocity, and attitude between the IMUs in order not to repeatedly use the GPS measurements once a GPS aiding epoch became available [25]. However, none of the aforementioned outcomes allowed applying the measurements from each IMU and GPS receiver independently in KF measurement updates. As another enhanced part of this manuscript, measurements and systematic errors of these IMU arrays are individually modeled in Kalman filtering, instead of adopting a set of common shared states for all IMUs. Therefore, the effect of the noises of low-cost IMU raw outputs could be markedly reduced because both the raw data and systematic errors participate in Kalman filtering updates. Moreover, we have the choice of multiple low-cost IMUs and need not stick to the ones of the same design or from the same vendor.

As a matter of fact, given the integration strategy and the system model, various nonlinear filtering candidates (e.g., extended Kalman filter (EKF), unscented Kalman filter (UKF), particle filter (PF), etc.) are available. Nevertheless, this manuscript uses the EKF instead of other nonlinear filtering algorithms because (1) the research does not focus on the filtering techniques, (2) the system model does not present strong nonlinearity, and (3) the EKF algorithm is simple and easy to use in practical engineering.

A more practical system motion model is described in Section 2. The formulation of the unconventional multi-sensor Kalman filter is presented in Section 3. The implementation of the Kalman filter is discussed in Section 4. The data processing results from road tests demonstrate the feasibility by adopting the proposed algorithm model under the unconventional integration strategy. The corresponding numerical results along with analysis are provided in Section 5. Conclusions and remarks are given in Section 6.

2. The System Motion Model

The general system motion model contains three parts: (1) the 3D kinematic trajectory [26], (2) the attitudes, and (3) the angular rate.

2.1. Three-Dimensional Kinematic Trajectory Model

The instantaneous motion of a rigid body can be described using kinematics without considering the forces that cause different types of motion. For a mechanical system, suitable coordinate systems are necessary to give a mathematical expression of the position, velocity, and acceleration. Two reference frames move relative to each other, as shown in Figure 1. One is the space-fixed system $\mathit{o}\mathit{x}\mathit{y}\mathit{z}(\mathit{S})$, and the other is the moving system ${\mathit{o}}_{\mathit{b}}{\mathit{x}}_{\mathit{b}}{\mathit{y}}_{\mathit{b}}{\mathit{z}}_{\mathit{b}}({\mathit{S}}_{\mathit{b}})$.

In Figure 1, the time-dependent position vector $\overrightarrow{\mathit{r}}$ of a point $\mathit{P}$ is uniquely expressed using three unit vectors $\overrightarrow{\mathit{i}},\overrightarrow{\mathit{j}},\overrightarrow{\mathit{k}}$ along three axes $\mathit{x},\mathit{y},\mathit{z}$ in frame $\mathit{S}$.

$$\overrightarrow{\mathit{r}}=\mathit{x}\overrightarrow{\mathit{i}}+\mathit{y}\overrightarrow{\mathit{j}}+\mathit{z}\overrightarrow{\mathit{k}}.$$

(1)

In components, the motion of the moving point at a time instant $\mathit{t}$ with respect to a start time ${\mathit{t}}_0$ can be given as shown below.

$$\mathit{x}(\mathit{t})=\mathit{x}({\mathit{t}}_0)+\dot{\mathit{x}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0)+\frac{1}{2}\ddot{\mathit{x}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0{)}^2+\frac{1}{6}\stackrel{⃛}{\mathit{x}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0{)}^3+\cdots ,$$

(2)

$$\mathit{y}(\mathit{t})=\mathit{y}({\mathit{t}}_0)+\dot{\mathit{y}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0)+\frac{1}{2}\ddot{\mathit{y}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0{)}^2+\frac{1}{6}\stackrel{⃛}{\mathit{y}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0{)}^3+\cdots ,$$

(3)

$$\mathit{z}(\mathit{t})=\mathit{z}({\mathit{t}}_0)+\dot{\mathit{z}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0)+\frac{1}{2}\ddot{\mathit{z}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0{)}^2+\frac{1}{6}\stackrel{⃛}{\mathit{z}}({\mathit{t}}_0)(\mathit{t}-{\mathit{t}}_0{)}^3+\cdots ,$$

(4)

where $\dot{\mathit{x}},\ddot{\mathit{x}},\stackrel{⃛}{\mathit{x}},$ $\dot{\mathit{y}},\ddot{\mathit{y}},\stackrel{⃛}{\mathit{y}},$ $\dot{\mathit{z}},\ddot{\mathit{z}},\stackrel{⃛}{\mathit{z}},\dots$ are the first, second, and third derivatives of $\mathit{x},\mathit{y},\mathit{z}$ with respect to $\mathit{t}$. It is noteworthy that the time interval $(\mathit{t}-{\mathit{t}}_0)$ must be short enough in order to require as few terms as possible. Let ${\overrightarrow{\mathit{r}}}_0$ be the position vector of the origin ${\mathit{O}}_{\mathit{b}}$, and let $\overrightarrow{\mathbf{\rho }}$ and ${\overrightarrow{\mathbf{\rho }}}_{\mathit{b}}$ be the relative position vector of the point $\mathit{P}$ from ${\mathit{O}}_{\mathit{b}}$ and the same vector in ${\mathit{S}}_{\mathit{b}}$; then, we can get the following relationship:

$$\overrightarrow{\mathit{r}}(\mathit{t})={\overrightarrow{\mathit{r}}}_0(\mathit{t})+\overrightarrow{\mathbf{\rho }}(\mathit{t})={\overrightarrow{\mathit{r}}}_0(\mathit{t})+{\mathit{D}}^{\mathit{T}}(\mathit{t}){\overrightarrow{\mathbf{\rho }}}_{\mathit{b}}(\mathit{t}),$$

(5)

where $\mathit{D}(\mathit{t})$ is the instantaneous rotation matrix from $\mathit{S}$ to ${\mathit{S}}_{\mathit{b}}$. According to Equation (5), we can successively derive the velocity vector, acceleration vector, and jerk vector. Moreover, $\mathit{D}(\mathit{t})$ is coupled with the angular motion.

With respect to a rigid body in motion, the basic trajectory parameters are considered in the local navigation frame (${\mathit{S}}_{\mathit{n}}$), i.e., the position ${\mathit{r}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$ of the IMU center, the velocity ${\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$, the acceleration ${\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$, and the jerk ${\mathit{j}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$. In essence, ${\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$ is the derivative of ${\mathit{r}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$, and it is also transformed from its opposite number ${\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ in the body frame (${\mathit{S}}_{\mathit{b}}$). Likewise, ${\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$ and ${\mathit{j}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$ are the derivatives of ${\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$ and ${\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{n}}$, respectively. According to the vector dynamics, the trajectory parameters are directly derived as shown below.

$${\dot{\mathit{r}}}_{\mathit{n}\mathit{b}}^{\mathit{n}}={\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{n}}={\mathit{C}}_{\mathit{b}}^{\mathit{n}}{\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{b}},$$

(6)

$${\dot{\mathit{v}}}_{\mathit{n}\mathit{b}}^{\mathit{n}}={\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{n}}={\mathit{C}}_{\mathit{b}}^{\mathit{n}}[{\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}\times ]{\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{b}}+{\mathit{C}}_{\mathit{b}}^{\mathit{n}}{(\begin{array}{ccc}{\dot{\mathit{v}}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& {\dot{\mathit{v}}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}& {\dot{\mathit{v}}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array})}^{\mathit{T}}={\mathit{C}}_{\mathit{b}}^{\mathit{n}}{\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}},$$

(7)

$${\dot{\mathit{a}}}_{\mathit{n}\mathit{b}}^{\mathit{n}}={\mathit{j}}_{\mathit{n}\mathit{b}}^{\mathit{n}}={\mathit{C}}_{\mathit{b}}^{\mathit{n}}[{\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}\times ]{\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}+{\mathit{C}}_{\mathit{b}}^{\mathit{n}}{(\begin{array}{ccc}{\dot{\mathit{a}}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& {\dot{\mathit{a}}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}& {\dot{\mathit{a}}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array})}^{\mathit{T}}={\mathit{C}}_{\mathit{b}}^{\mathit{n}}{\mathit{j}}_{\mathit{n}\mathit{b}}^{\mathit{b}},$$

(8)

where ${\mathit{r}}_{\mathit{n}\mathit{b}}^{\mathit{n}},{\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{n}},{\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{n}},{\mathit{j}}_{\mathit{n}\mathit{b}}^{\mathit{n}},{\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{n}}$ are the position, velocity, acceleration, jerk, and angular rate vectors in ${\mathit{S}}_{\mathit{n}}$, respectively. ${\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{b}},{\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}},{\mathit{j}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ are the velocity, acceleration, and jerk vectors in ${\mathit{S}}_{\mathit{b}}$. ${\mathit{C}}_{\mathit{b}}^{\mathit{n}}$ is the direction cosine matrix (DCM) from ${\mathit{S}}_{\mathit{b}}$ to ${\mathit{S}}_{\mathit{n}}$. The derivatives of the body velocity components and the body acceleration components are given below.

$$\begin{array}{c}\left[\begin{array}{c}{\dot{\mathit{v}}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ {\dot{\mathit{v}}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\dot{\mathit{v}}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array}\right]=\left[\begin{array}{c}{\mathit{a}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ {\mathit{a}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\mathit{a}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array}\right]-\left[\begin{array}{ccc}0& -{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}& 0& -{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ -{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& 0\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array}\right],\\ \left[\begin{array}{c}{\dot{\mathit{a}}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ {\dot{\mathit{a}}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\dot{\mathit{a}}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array}\right]=\left[\begin{array}{c}{\mathit{j}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ {\mathit{j}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\mathit{j}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array}\right]-\left[\begin{array}{ccc}0& -{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}& 0& -{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ -{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& 0\end{array}\right]\left[\begin{array}{c}{\mathit{a}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ {\mathit{a}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\mathit{a}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array}\right],\end{array}$$

where ${\mathit{j}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}},{\mathit{j}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}},{\mathit{j}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}$ are the body jerk components, ${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}},{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}},{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}$ are the body angular rate components, ${\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}},{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}},{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}$ are the body velocity components, and ${\mathit{a}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}},{\mathit{a}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}},{\mathit{a}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}$ are the body acceleration components.

2.2. “Current” Statistical Singer Acceleration Model

The Singer model assumes that the maneuvering acceleration $\mathit{a}(\mathit{t})$ obeys a stationary first-order time-correlated process with zero-mean. The correlation function is expressed in the form of exponential decay as

$${\mathit{R}}_{\mathit{a}}(\mathbf{\tau })=\mathit{E}[\mathit{a}(\mathit{t})\mathit{a}(\mathit{t}+\mathbf{\tau })]={\mathbf{\sigma }}_{\mathit{a}}^2{\mathit{e}}^{-\mathbf{\alpha }\left|\mathbf{\tau }\right|},\mathbf{\alpha }\ge 0,$$

(9)

where ${\mathbf{\sigma }}_{\mathit{a}}^2$ is the variance of the target acceleration, and $\mathbf{\alpha }$ is the reciprocal of the maneuver (acceleration) time constant, which is named “maneuvering frequency” and taken generally as an empirical value. Assuming that the probability density function of the acceleration approximately obeys uniform distribution, ${\mathbf{\sigma }}_{\mathit{a}}^2=\frac{{\mathit{a}}_{\max }^2}{3}(1+4{\mathit{P}}_{\max }-{\mathit{P}}_0)$ with the maximum maneuvering acceleration ${\mathit{a}}_{\max }$ and its probability ${\mathit{P}}_{\max }$, and the non-maneuvering probability ${\mathit{P}}_0$.

Utilizing the correlation function ${\mathit{R}}_{\mathit{a}}(\mathbf{\tau })$, the acceleration $\mathit{a}(\mathit{t})$ may be expressed in terms of white noise by the Wiener–Kolmogorov whitening procedure [16], as shown below.

$$\dot{\mathit{a}}(\mathit{t})=-\mathbf{\alpha }\mathit{a}(\mathit{t})+{\mathit{w}}_{\mathit{a}}(\mathit{t}),$$

(10)

where ${\mathit{w}}_{\mathit{a}}(\mathit{t})$ is the white Gaussian noise with the mean of 0 and the variance of $2\mathbf{\alpha }{\mathbf{\sigma }}_{\mathit{a}}^2$.

The “current” statistical (CS) model adopts a non-zero mean and modified Rayleigh distribution to characterize the maneuvering acceleration. Specifically, the modified Rayleigh distribution is used to describe the “current” probability density of maneuvering acceleration, and the mean value is the “current” acceleration prediction. The random acceleration still conforms to the first-order time-correlated process. According to the Singer model and the CS model, the body acceleration can be given as

$$\begin{array}{l}{\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t})=\overline{\mathit{a}}(\mathit{t})+\mathit{a}(\mathit{t})\\ \dot{\mathit{a}}(\mathit{t})=-\mathbf{\alpha }\mathit{a}(\mathit{t})+{\mathit{w}}_{\mathit{a}}(\mathit{t})\end{array},$$

(11)

where $\overline{\mathit{a}}(\mathit{t})$ is the “current” mean of maneuvering acceleration, and it is constant during each sampling period.

The differential equation of body acceleration is acquired by consolidating Equation (11), as shown below.

$${\dot{\mathit{a}}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t})=-\mathbf{\alpha }{\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t})+\mathbf{\alpha }\overline{\mathit{a}}(\mathit{t})+{\mathit{w}}_{\mathit{a}}(\mathit{t}).$$

(12)

Then, the full-state differential equation in a continuous-time system is as follows:

$$\left[\begin{array}{c}{\displaystyle \int {\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t}){\mathit{d}}_{\mathit{t}}}\\ {\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t})\\ {\dot{\mathit{a}}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t})\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -\mathbf{\alpha }\end{array}\right]\left[\begin{array}{c}{\displaystyle \iint {\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t})}{\mathit{d}}_{\mathit{t}}{\mathit{d}}_{\mathit{t}}\\ {\displaystyle \int {\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t}){\mathit{d}}_{\mathit{t}}}\\ {\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}(\mathit{t})\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ \mathbf{\alpha }\end{array}\right]\overline{\mathit{a}}(\mathit{t})+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]{\mathit{w}}_{\mathit{a}}(\mathit{t}).$$

(13)

2.3. Attitude Angle Model

Typically, Euler angles (i.e., pitch, roll, and heading) of ${\mathit{S}}_{\mathit{b}}$ with respect to ${\mathit{S}}_{\mathit{n}}$ are selected to demonstrate the rotating properties of a 3D object. The differential equations of Euler angles are given in matrix form as follows:

$$\left[\begin{array}{c}\dot{\mathit{P}}\\ \dot{\mathbf{\gamma }}\\ \dot{\mathbf{\psi }}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathbf{\gamma }& 0& \sin \mathbf{\gamma }\\ \sin \mathbf{\gamma }\tan \mathit{P}& 1& -\cos \mathbf{\gamma }\tan \mathit{P}\\ \sin \mathbf{\gamma }\sec \mathit{P}& 0& -\cos \mathbf{\gamma }\sec \mathit{P}\end{array}\right]\left[\begin{array}{c}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array}\right]={\mathit{C}}_{3\times 3}{\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}},$$

(14)

wherein $\mathit{P},\mathbf{\gamma },\mathbf{\psi }$ are the pitch, roll, and heading, respectively, ${\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ is the angular rate in ${\mathit{S}}_{\mathit{b}}$, and ${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}},{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}},{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}$ are the components of ${\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ in three axes.

2.4. Angular Rate Model

For a normally running vehicle with a smooth steering within a small time interval, three components of ${\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ are reasonably treated as independent. In a general way, ${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}$ and ${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathrm{y}}^{\mathit{b}}$ are both modeled as zero-mean processes by the first-order Markov model, and ${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}$ is modeled as a non-zero-mean random process with random disturbance. More concretely, the zero-mean Singer motion model is adopted to describe the dynamic variation of ${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}$ and ${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathrm{y}}^{\mathit{b}}$ in the system model, and the modified Singer model is used to express the dynamics of ${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}$ [22,27,28].

3. The Formulation of the Unconventional Kalman Filter

In a general way, the state and measurement vector ought to be arranged for the system Kalman filter. In this research, the whole value state which describes the kinetic characteristic of the vehicle is one part of the state vector. The systematic errors of each IMU, e.g., the biases, scale factor errors, and so forth, are also modeled individually in the Kalman filter, instead of assuming “the common-mode errors of different sensors of the same design”. Therefore, the systematic error is the other part. As for the measurement vector, the raw observables from the GPS and multiple IMUs are considered. Figure 2 illustrates the unconventional integration mechanism in this research. To demonstrate the benefits of the individual modeling for systematic errors and measurements, choosing different low-cost IMUs from different vendors is the best choice. Considering the cost of the experiment and the amount of calculation, it is advisable to integrate three low-cost IMUs and one GPS receiver in the kinematic positioning and navigation system.

3.1. The State Vector Including the Systematic Errors in Kalman Filter

In this manuscript, the state vector of the constructed multi-sensor integration Kalman filter contains 51 components as follows:

$${\mathit{X}}_{51\times 1}={\left[\begin{array}{ccccccccccccccccc}{\mathit{r}}^{\mathit{T}}& {({\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{b}})}^{\mathit{T}}& {({\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}})}^{\mathit{T}}& {\mathbf{\theta }}^{\mathit{T}}& {({\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}})}^{\mathit{T}}& {\mathit{b}}_{\mathit{g}1}^{\mathit{T}}& {\mathit{b}}_{\mathit{g}2}^{\mathit{T}}& {\mathit{b}}_{\mathit{g}3}^{\mathit{T}}& {\mathit{b}}_{\mathit{a}1}^{\mathit{T}}& {\mathit{b}}_{\mathit{a}2}^{\mathit{T}}& {\mathit{b}}_{\mathit{a}3}^{\mathit{T}}& {\mathit{S}}_{\mathit{g}1}^{\mathit{T}}& {\mathit{S}}_{\mathit{g}2}^{\mathit{T}}& {\mathit{S}}_{\mathit{g}3}^{\mathit{T}}& {\mathit{S}}_{\mathit{a}1}^{\mathit{T}}& {\mathit{S}}_{\mathit{a}2}^{\mathit{T}}& {\mathit{S}}_{\mathit{a}3}^{\mathit{T}}\end{array}\right]}^{\mathit{T}},$$

where the position vector is expressed by triaxial coordinates in the earth-fixed coordinate system $\mathit{r}={(\begin{array}{ccc}\mathit{X}& \mathit{Y}& \mathit{Z}\end{array})}^{\mathit{T}}$, the body velocity vector ${\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{b}}={(\begin{array}{ccc}{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}& {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array})}^{\mathit{T}}$, the body acceleration vector ${\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}={(\begin{array}{ccc}{\mathit{a}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& {\mathit{a}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}& {\mathit{a}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array})}^{\mathit{T}}$, the attitude $\mathbf{\theta }={(\begin{array}{ccc}\mathit{P}& \mathbf{\gamma }& \mathbf{\psi }\end{array})}^{\mathit{T}}$, the body angular rate vector ${\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}={(\begin{array}{ccc}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\end{array})}^{\mathit{T}}$, the gyroscope bias vector ${\mathit{b}}_{\mathit{g}1}={(\begin{array}{ccc}{\mathit{b}}_{\mathit{g}1\mathit{x}}& {\mathit{b}}_{\mathit{g}1\mathit{y}}& {\mathit{b}}_{\mathit{g}1\mathit{z}}\end{array})}^{\mathit{T}}$, ${\mathit{b}}_{\mathit{g}2}={(\begin{array}{ccc}{\mathit{b}}_{\mathit{g}2\mathit{x}}& {\mathit{b}}_{\mathit{g}2\mathit{y}}& {\mathit{b}}_{\mathit{g}2\mathit{z}}\end{array})}^{\mathit{T}}$, ${\mathit{b}}_{\mathit{g}3}={(\begin{array}{ccc}{\mathit{b}}_{\mathit{g}3\mathit{x}}& {\mathit{b}}_{\mathit{g}3\mathit{y}}& {\mathit{b}}_{\mathit{g}3\mathit{z}}\end{array})}^{\mathit{T}}$, the accelerometer bias vector ${\mathit{b}}_{\mathit{a}1}={(\begin{array}{ccc}{\mathit{b}}_{\mathit{a}1\mathit{x}}& {\mathit{b}}_{\mathit{a}1\mathit{y}}& {\mathit{b}}_{\mathit{a}1\mathit{z}}\end{array})}^{\mathit{T}}$, ${\mathit{b}}_{\mathit{a}2}={(\begin{array}{ccc}{\mathit{b}}_{\mathit{a}2\mathit{x}}& {\mathit{b}}_{\mathit{a}2\mathit{y}}& {\mathit{b}}_{\mathit{a}2\mathit{z}}\end{array})}^{\mathit{T}}$, ${\mathit{b}}_{\mathit{a}3}={(\begin{array}{ccc}{\mathit{b}}_{\mathit{a}3\mathit{x}}& {\mathit{b}}_{\mathit{a}3\mathit{y}}& {\mathit{b}}_{\mathit{a}3\mathit{z}}\end{array})}^{\mathit{T}}$, the gyroscope scale factor error ${\mathit{s}}_{\mathit{g}1}={(\begin{array}{ccc}{\mathit{s}}_{\mathit{g}1\mathit{x}}& {\mathit{s}}_{\mathit{g}1\mathit{y}}& {\mathit{s}}_{\mathit{g}1\mathit{z}}\end{array})}^{\mathit{T}}$, ${\mathit{s}}_{\mathit{g}2}={(\begin{array}{ccc}{\mathit{s}}_{\mathit{g}2\mathit{x}}& {\mathit{s}}_{\mathit{g}2\mathit{y}}& {\mathit{s}}_{\mathit{g}2\mathit{z}}\end{array})}^{\mathit{T}}$, ${\mathit{s}}_{\mathit{g}3}={(\begin{array}{ccc}{\mathit{s}}_{\mathit{g}3\mathit{x}}& {\mathit{s}}_{\mathit{g}3\mathit{y}}& {\mathit{s}}_{\mathit{g}3\mathit{z}}\end{array})}^{\mathit{T}}$, and the accelerometer scale factor error ${\mathit{s}}_{\mathit{a}1}={(\begin{array}{ccc}{\mathit{s}}_{\mathit{a}1\mathit{x}}& {\mathit{s}}_{\mathit{a}1\mathit{y}}& {\mathit{s}}_{\mathit{a}1\mathit{z}}\end{array})}^{\mathit{T}}$, ${\mathit{s}}_{\mathit{a}2}={(\begin{array}{ccc}{\mathit{s}}_{\mathit{a}2\mathit{x}}& {\mathit{s}}_{\mathit{a}2\mathit{y}}& {\mathit{s}}_{\mathit{a}2\mathit{z}}\end{array})}^{\mathit{T}}$, ${\mathit{s}}_{\mathit{a}3}={(\begin{array}{ccc}{\mathit{s}}_{\mathit{a}3\mathit{x}}& {\mathit{s}}_{\mathit{a}3\mathit{y}}& {\mathit{s}}_{\mathit{a}3\mathit{z}}\end{array})}^{\mathit{T}}$. Herein, subscripts 1, 2, and 3 are the sequence numbers of the IMUs.

3.2. The Discretization of the System Model

On the basis of the differential model in Section 2, the discrete system model for KF can be summarized after omitting the higher-order terms in Taylor series expansion. It is mentionable that the position vector $\mathit{r}$ of a vehicle should be in the earth-fixed coordinate frame (${\mathit{S}}_{\mathit{e}}$); thus, the position vector $\mathit{r}$ at epoch $\mathit{k}+1$ in ${\mathit{S}}_{\mathit{e}}$ is calculated concretely as follows:

• Firstly, the local coordinate increment $\Delta {\mathit{r}}_{\mathit{n}}$ from epoch $\mathit{k}$ to $\mathit{k}+1$ in ${\mathit{S}}_{\mathit{n}}$ is as follows:

  $$\Delta {\mathit{r}}_{\mathit{n}}=\Delta \mathit{t}{\mathit{C}}_{\mathit{b}(\mathit{k})}^{\mathit{n}}{\mathit{v}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+\frac{\Delta {\mathit{t}}^2}{2}{\mathit{C}}_{\mathit{b}(\mathit{k})}^{\mathit{n}}{\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+\frac{\Delta {\mathit{t}}^3}{6}{\mathit{C}}_{\mathit{b}(\mathit{k})}^{\mathit{n}}{\mathit{j}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}.$$

  (15)
• Next, $\Delta {\mathit{r}}_{\mathit{n}}$ is transformed from ${\mathit{S}}_{\mathit{n}}$ to ${\mathit{S}}_{\mathit{e}}$ as follows:

  $$\Delta {\mathit{r}}_{\mathit{e}}={\mathit{C}}_{\mathit{n}(\mathit{k})}^{\mathit{e}}\Delta {\mathit{r}}_{\mathit{n}}.$$

  (16)
• Finally, we obtain the position vector $\mathit{r}$ at epoch $\mathit{k}+1$ as follows:

  $$\mathit{r}(\mathit{k}+1)=\mathit{r}(\mathit{k})+\Delta {\mathit{r}}_{\mathit{e}}=\mathit{r}(\mathit{k})+{\mathit{C}}_{\mathit{n}(\mathit{k})}^{\mathit{e}}\Delta {\mathit{r}}_{\mathit{n}}.$$

  (17)

Furthermore, there is a particular discussion for the discretization of the body acceleration model. The discrete state of Equation (13) can be derived as

$$\mathit{Y}(\mathit{k}+1)=\tilde{\mathit{A}}(\mathit{k})\mathit{Y}(\mathit{k})+\tilde{\mathit{B}}(\mathit{k})\overline{\mathit{a}}(\mathit{k})+{\mathit{w}}_{\mathit{a}}(\mathit{k}),$$

(18)

with $\tilde{\mathit{A}}(\mathit{k})=\left[\begin{array}{ccc}1& \Delta \mathit{t}& {(\mathbf{\alpha }\Delta \mathit{t}-1+{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/\mathbf{\alpha }}^2\\ 0& 1& (1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/\mathbf{\alpha }\\ 0& 0& {\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}}\end{array}\right],\tilde{\mathit{B}}(\mathit{k})=\left(\left[\begin{array}{c}\Delta {\mathit{t}}^2/2\\ \Delta \mathit{t}\\ 1\end{array}\right]-\left[\begin{array}{c}{(\mathbf{\alpha }\Delta \mathit{t}-1+{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/\mathbf{\alpha }}^2\\ (1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/\mathbf{\alpha }\\ {\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}}\end{array}\right]\right)$.

The discrete acceleration is obtained from Equation (18) as follows:

$${\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k}+1)}^{\mathit{b}}={\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}}{\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+(1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})\overline{\mathit{a}}(\mathrm{k})+{\mathit{w}}_{\mathit{a}}(\mathrm{k}),$$

(19)

where $\overline{\mathit{a}}$ is the “current” mean of maneuvering acceleration, which is acquired by calculating the mean value of both sides of Equation (19), as shown below.

$$\overline{\mathit{a}}(\mathit{k}+1)=\mathit{E}\left\{{\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k}+1)}^{\mathit{b}}\right|{\mathit{z}}^{\mathit{k}}\}={\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}}\mathit{E}\{{\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\left|{\mathit{z}}^{\mathit{k}}\right\}+(1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})\overline{\mathit{a}}(\mathit{k})={\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}}{\widehat{\mathit{a}}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+(1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})\overline{\mathit{a}}(\mathit{k}),$$

(20)

where ${\mathit{z}}^{\mathit{k}}$ is the sequence of all measurements up to the current time, and ${\widehat{\mathit{a}}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}$ is the acceleration estimate of the previous moment. Equation (20) shows that $\overline{\mathit{a}}(\mathit{k}+1)$ is not only related to the current information ${\widehat{\mathit{a}}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}$, but also to the past information $\overline{\mathit{a}}(\mathit{k})$.

Since the CS model assumes that the maneuvering acceleration at an arbitrary time obeys the modified Rayleigh distribution, the variance of body acceleration can be obtained as follows:

$${\mathbf{\sigma }}_{\mathit{a}}^2=\{\begin{array}{c}\frac{4-\mathbf{\pi }}{\mathbf{\pi }}{[{\mathit{a}}_{\max }-\widehat{\mathit{a}}(\mathit{k}-1)]}^2\begin{array}{c}\end{array}\begin{array}{c}0\end{array}<\widehat{\mathit{a}}(\mathit{k}-1)<{\mathit{a}}_{\max }\\ \frac{4-\mathbf{\pi }}{\mathbf{\pi }}{[{\mathit{a}}_{\max }+\widehat{\mathit{a}}(\mathit{k}-1)]}^2\begin{array}{c}\end{array}\begin{array}{c}{\mathit{a}}_{-\max }<\widehat{\mathit{a}}(\mathit{k}-1)<0\end{array}\end{array}.$$

(21)

As can be seen from Equations (13) and (18), the position vector $\mathit{r}$ and the body velocity vector ${\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ are affected by the CS Singer acceleration model. Consequently, combining the 3D kinematic trajectory model in Section 2.1 and the CS Singer acceleration model in Section 2.2, the discrete system equations of kinematic trajectory can be derived as follows:

$$\begin{array}{c}\mathit{r}(\mathit{k}+1)=\mathit{r}(\mathit{k})+{\mathit{C}}_{\mathit{n}(\mathit{k})}^{\mathit{e}}\{\Delta \mathit{t}{\mathit{C}}_{\mathit{b}(\mathit{k})}^{\mathit{n}}{\mathit{v}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+\left[{(\mathbf{\alpha }\Delta \mathit{t}-1+{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/\mathbf{\alpha }}^2\right]{\mathit{C}}_{\mathit{b}(\mathit{k})}^{\mathit{n}}{\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\\ +\left[\frac{\Delta {\mathit{t}}^2}{2}-(\mathbf{\alpha }\Delta \mathit{t}-1+{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/{\mathbf{\alpha }}^2\right]{\mathit{C}}_{\mathit{b}(\mathit{k})}^{\mathit{n}}\overline{\mathit{a}}(\mathit{k})+{\mathit{C}}_{\mathit{b}(\mathit{k})}^{\mathit{n}}{\mathit{w}}_{\mathit{a}}(\mathit{k})+\frac{\Delta {\mathit{t}}^3}{6}{\mathit{C}}_{\mathit{b}(\mathit{k})}^{\mathit{n}}{\mathit{j}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\}\end{array},$$

(22)

$$\begin{array}{c}{\mathit{v}}_{\mathit{n}\mathit{b}(\mathit{k}+1)}^{\mathit{b}}=\left[{\mathit{I}}_{3\times 3}-\Delta \mathit{t}\left[{\mathbf{\omega }}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\times \right]+\frac{\Delta {\mathit{t}}^2}{2}{\left[{\mathbf{\omega }}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\times \right]}^2\right]{\mathit{v}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+\left[(1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/\mathbf{\alpha }\right]{\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\\ +\left[\Delta \mathit{t}-(1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/\mathbf{\alpha }\right]\overline{\mathit{a}}(\mathit{k})+{\mathit{w}}_{\mathit{a}}(\mathit{k})+\frac{\Delta {\mathit{t}}^2}{2}\left[{\mathit{v}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\times \right]{\dot{\mathbf{\omega }}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+\frac{\Delta {\mathit{t}}^2}{2}{\mathit{j}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\end{array},$$

(23)

$${\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k}+1)}^{\mathit{b}}={\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}}{\mathit{a}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+(1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})\overline{\mathit{a}}(\mathit{k})+{\mathit{w}}_{\mathit{a}}(\mathit{k}).$$

(24)

The discrete system equations of the attitude and body angular rate are directly given as follows:

$$\mathbf{\theta }(\mathit{k}+1)=\mathbf{\theta }(\mathit{k})+\Delta \mathit{t}{\mathit{C}}_{3\times 3}{\mathbf{\omega }}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}+\frac{\Delta {\mathit{t}}^2}{2}{\mathit{C}}_{3\times 3}{\dot{\mathbf{\omega }}}_{\mathit{n}\mathit{b}}^{\mathit{b}},$$

(25)

$${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}(\mathit{k}+1)}^{\mathit{b}}={\mathit{e}}^{-\Delta \mathit{t}/{\mathit{T}}_{\mathit{x}}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}(\mathit{k})}^{\mathit{b}}+{\mathit{w}}_{\mathbf{\omega }\mathit{x}},$$

(26)

$${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}(\mathit{k}+1)}^{\mathit{b}}={\mathit{e}}^{-\Delta \mathit{t}/{\mathit{T}}_{\mathit{y}}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}(\mathit{k})}^{\mathit{b}}+{\mathit{w}}_{\mathbf{\omega }\mathit{y}},$$

(27)

$${\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}(\mathit{k}+1)}^{\mathit{b}}={\mathit{e}}^{-\Delta \mathit{t}/{\mathit{T}}_{\mathit{z}}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}(\mathit{k})}^{\mathit{b}}+(1-{\mathit{e}}^{-\Delta \mathit{t}/{\mathit{T}}_{\mathit{z}}}){\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}(\mathit{k}-1)}^{\mathit{b}}+{\mathit{w}}_{\mathbf{\omega }\mathit{z}}.$$

(28)

The discrete system equations of the systematic errors of three different low-cost IMUs are individually modeled as follows:

$${\mathit{b}}_{\mathit{g}\mathrm{i}}(\mathit{k}+1)={\mathit{b}}_{\mathit{g}\mathrm{i}}(\mathit{k})+{\mathit{w}}_{{\mathit{b}}_{\mathit{g}\mathrm{i}}},$$

(29)

$${\mathit{b}}_{\mathit{a}\mathrm{i}}(\mathit{k}+1)={\mathit{b}}_{\mathit{a}\mathrm{i}}(\mathit{k})+{\mathit{w}}_{{\mathit{b}}_{\mathit{a}\mathrm{i}}},$$

(30)

$${\mathit{s}}_{\mathit{g}\mathrm{i}}(\mathit{k}+1)={\mathit{s}}_{\mathit{g}\mathrm{i}}(\mathit{k})+{\mathit{w}}_{{\mathit{s}}_{\mathit{g}\mathrm{i}}},$$

(31)

$${\mathit{s}}_{\mathit{a}\mathrm{i}}(\mathit{k}+1)={\mathit{s}}_{\mathit{a}\mathrm{i}}(\mathit{k})+{\mathit{w}}_{{\mathit{s}}_{\mathit{a}\mathrm{i}}},$$

(32)

where $\mathrm{i}$ represents the sequence number of the IMU with $\mathrm{i}=1,2,3$, $\Delta \mathit{t}={\mathit{t}}_{\mathit{k}+1}-{\mathit{t}}_{\mathit{k}}$ is the time interval, ${\mathit{T}}_{\mathit{x}},{\mathit{T}}_{\mathit{y}},{\mathit{T}}_{\mathit{z}}$ are the time correlation coefficients of the first-order Markov model, ${\mathit{w}}_{\mathbf{\omega }\mathit{x}},{\mathit{w}}_{\mathbf{\omega }\mathit{y}},{\mathit{w}}_{\mathbf{\omega }\mathit{z}}$ are the independent white noises for triaxial angular rates, And ${\mathit{w}}_{\mathit{b}\mathit{g}},{\mathit{w}}_{\mathit{s}\mathit{g}},{\mathit{w}}_{\mathit{b}\mathit{a}}$, and ${\mathit{w}}_{\mathit{s}\mathit{a}}$ are the white-noise vectors for the biases and scale factor errors of gyroscopes and accelerometers, while subscripts 1, 2, and 3 are sequence numbers of three different IMUs. ${\mathit{j}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ is treated as process noise for the position vector, velocity vector, and acceleration vector, ${\dot{\mathbf{\omega }}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ is the angular acceleration as process noise for the velocity vector, acceleration vector, and attitude vector, and ${\mathit{C}}_{\mathit{b}}^{\mathit{n}}$ is the DCM. ${\mathit{C}}_{3\times 3}$ is the coefficient matrix as in Equation (14). ${\mathit{C}}_{\mathit{n}}^{\mathit{e}}$, the position cosine matrix, can be obtained through the position information as follows:

$${\mathit{C}}_{\mathit{n}}^{\mathit{e}}=\left[\begin{array}{ccc}-\sin \mathbf{\lambda }& -\sin \mathbf{\phi }\cos \mathbf{\lambda }& \cos \mathbf{\phi }\cos \mathbf{\lambda }\\ \cos \mathbf{\lambda }& -\sin \mathbf{\phi }\sin \mathbf{\lambda }& \cos \mathbf{\phi }\sin \mathbf{\lambda }\\ 0& \cos \mathbf{\phi }& \sin \mathbf{\phi }\end{array}\right],$$

wherein $\mathbf{\phi }$ and $\mathbf{\lambda }$ are the latitude and longitude, respectively, calculated by the coordinate components of the position vector $\mathit{r}$ in ${\mathit{S}}_{\mathit{e}}$.

3.3. The Measurement Model of IMU

Generally, the IMU raw outputs include specific forces from three orthogonal accelerometers and angular rates from three orthogonal gyroscopes. We should derive three groups of measurement equations because there are three IMUs in the integration system. Figure 3 shows the structure diagram of IMUs, where Figure 3a is the general view of the vehicle, and Figure 3b represents the partial enlarged details.

The measurement equations for low-cost IMUs could be simplified depending on specific needs [15,19]. Considering that IMUs cannot be located at the same point on the body, the measurements from different IMUs must be transformed to the same reference frame so as to perform the fusion algorithm [29]. Here, the central IMU is selected as the reference. Three groups of measurement equations for angular rate and specific force are respectively derived as shown below based on the particular structure.

$${\mathbf{\omega }}_{\mathit{i}\mathit{b}-\mathit{i}\mathit{m}\mathit{u}1}^{\mathit{b}}=(\mathit{I}+{\mathit{S}}_{\mathit{g}1}){\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}+{\mathit{b}}_{\mathit{g}1}+{\Delta }_{\mathit{g}1},$$

(33)

$${\mathbf{\omega }}_{\mathit{i}\mathit{b}-\mathit{i}\mathit{m}\mathit{u}2}^{\mathit{b}}=(\mathit{I}+{\mathit{S}}_{\mathit{g}2}){\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}+{\mathit{b}}_{\mathit{g}2}+{\Delta }_{\mathit{g}2},$$

(34)

$${\mathbf{\omega }}_{\mathit{i}\mathit{b}-\mathit{i}\mathit{m}\mathit{u}3}^{\mathit{b}}=(\mathit{I}+{\mathit{S}}_{\mathit{g}3}){\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}+{\mathit{b}}_{\mathit{g}3}+{\Delta }_{\mathit{g}3},$$

(35)

$${\mathit{f}}_{\mathit{i}\mathit{b}-\mathit{i}\mathit{m}\mathit{u}1}^{\mathit{b}}=(\mathit{I}+{\mathit{S}}_{\mathit{a}1})({\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}-{\mathit{C}}_{\mathit{n}}^{\mathit{b}}{\mathit{g}}^{\mathit{n}})+{\mathit{b}}_{\mathit{a}1}+{\Delta }_{\mathit{a}1},$$

(36)

$${\mathit{f}}_{\mathit{i}\mathit{b}-\mathit{i}\mathit{m}\mathit{u}2}^{\mathit{b}}=(\mathit{I}+{\mathit{S}}_{\mathit{a}2})({\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}-{\mathit{C}}_{\mathit{n}}^{\mathit{b}}{\mathit{g}}^{\mathit{n}})+{\mathbf{\omega }}_{\mathit{i}\mathit{b}}^{\mathit{b}}\times ({\mathbf{\omega }}_{\mathit{i}\mathit{b}}^{\mathit{b}}\times {\mathit{r}}_2)+{\mathit{b}}_{\mathit{a}2}+{\Delta }_{\mathit{a}2},$$

(37)

$${\mathit{f}}_{\mathit{i}\mathit{b}-\mathit{i}\mathit{m}\mathit{u}3}^{\mathit{b}}=(\mathit{I}+{\mathit{S}}_{\mathit{a}3})({\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}-{\mathit{C}}_{\mathit{n}}^{\mathit{b}}{\mathit{g}}^{\mathit{n}})+{\mathbf{\omega }}_{\mathit{i}\mathit{b}}^{\mathit{b}}\times ({\mathbf{\omega }}_{\mathit{i}\mathit{b}}^{\mathit{b}}\times {\mathit{r}}_3)+{\mathit{b}}_{\mathit{a}3}+{\Delta }_{\mathit{a}3},$$

(38)

where ${\mathit{g}}^{\mathit{n}}$ is the local gravity vector in ${\mathit{S}}_{\mathit{n}}$, ${\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}},{\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ are the rotation rate and acceleration vectors of ${\mathit{S}}_{\mathit{b}}$ with respect to ${\mathit{S}}_{\mathit{n}}$, ${\mathit{S}}_{\mathit{g}},{\mathit{S}}_{\mathit{a}}$ are $3\times 3$ scale factor error matrices for gyros and accelerometers, ${\mathit{b}}_{\mathit{g}},{\mathit{b}}_{\mathit{a}}$ have the same meanings as mentioned in Section 3.1, and ${\Delta }_{\mathit{g},}{\Delta }_{\mathit{a}}$ are Gaussian white noises for the angular rate vector and specific force vector. The lever arm parameters of the remaining IMUs relative to the central one are ${\mathit{r}}_2={[-0.5,0,0]}^{\mathit{T}}$ and ${\mathit{r}}_3={[0.5,0,0]}^{\mathit{T}}$.

3.4. The Measurement Model of the GPS

In this study, not only do the raw observables of IMUs participate in measurement updates, but so do those of the GPS. The GPS, as another sensor distinct from the IMU but with equal status in the system, offers two types of observables (carrier phase and pseudo-range). However, only the pseudo-range is adopted to complete the specific navigation task. The pseudo-range observation equation is generally given as

$$\mathit{P}{\mathit{R}}_{\mathit{A}}^{\mathit{j}}={\mathbf{\rho }}_{\mathit{A}}^{\mathit{j}}+\mathit{C}(\mathbf{\delta }{\mathit{t}}_{\mathit{A}}-\mathbf{\delta }{\mathit{t}}^{\mathit{j}})+{\mathit{d}}_{\mathit{A}-\mathit{t}\mathit{r}\mathit{o}\mathit{p}}^{\mathit{j}}+{\mathit{d}}_{\mathit{A}-\mathit{i}\mathit{o}\mathit{n}}^{\mathit{j}}+{\mathbf{\epsilon }}_{\mathit{P}{\mathit{R}}_{\mathit{A}}^{\mathit{j}}},$$

(39)

where $\mathit{j}=1,2,\cdots ,\mathit{n}$ denotes the $\mathit{j}-\mathit{t}\mathit{h}$ satellite, and ${\mathbf{\rho }}_{\mathit{A}}^{\mathit{j}}=\sqrt{{({\mathit{X}}^{\mathit{j}}-\mathit{X})}^2+{({\mathit{Y}}^{\mathit{j}}-\mathit{Y})}^2+{({\mathit{Z}}^{\mathit{j}}-\mathit{Z})}^2}$ is the distance between satellite $\mathit{j}$ and receiver $\mathit{A}$, where $({\mathit{X}}^{\mathit{j}},{\mathit{Y}}^{\mathit{j}},{\mathit{Z}}^{\mathit{j}})$ is the geocentric coordinate of satellite $\mathit{j}$, calculated by the relevant parameters provided in the satellite navigation message, $\mathit{c}$ is the light speed, $\mathbf{\delta }{\mathit{t}}_{\mathit{A}},\mathbf{\delta }{\mathit{t}}^{\mathit{j}}$ are the clock errors of receiver and satellite, respectively, ${\mathit{d}}_{\mathit{A}-\mathit{t}\mathit{r}\mathit{o}\mathit{p}}^{\mathit{j}},{\mathit{d}}_{\mathit{A}-\mathit{i}\mathit{o}\mathit{n}}^{\mathit{j}}$ are the tropospheric and ionospheric delays, and ${\mathbf{\epsilon }}_{\mathit{P}{\mathit{R}}_{\mathit{A}}^{\mathit{j}}}$ is the random noise.

4. The Implementation of the Kalman Filter

On the basis of the system model proposed above, the KF is structured straightforwardly. The state Equations (22)–(32) and measurement Equations (33)–(39) are separately generalized by the discrete nonlinear system and measurement models as shown below.

$${\mathit{X}}_{\mathit{k}+1}=\mathit{f}({\mathit{X}}_{\mathit{k}})+{\mathit{B}}_{\mathit{k}}{\mathit{u}}_{\mathit{k}}+{\mathbf{\Gamma }}_{\mathit{k}}{\mathit{W}}_{\mathit{k}},$$

(40)

$${\mathit{Z}}_{\mathit{k}}=\mathit{h}({\mathit{X}}_{\mathit{k}})+{\Delta }_{\mathit{k}},$$

(41)

where ${\mathit{X}}_{\mathit{k}}$ is the state vector as determined in Section 3.1, ${\mathit{Z}}_{\mathit{k}}$ is the measurement vector as described in Section 3.3 and Section 3.4, $\mathit{f}()$ and $\mathit{h}()$ are nonlinear mathematical functions, and they are constructed from Equations (22)–(32) and Equations (33)–(39), respectively, ${\mathit{B}}_{\mathit{k}}$ and ${\mathbf{\Gamma }}_{\mathit{k}}$ are coefficient matrices, ${\mathit{u}}_{\mathit{k}}$ is the system input, ${\mathit{u}}_{\mathit{k}}=\overline{\mathit{a}}(\mathit{k})$, ${\Delta }_{\mathit{k}}$ is the measurement noise vector, and ${\mathit{W}}_{\mathit{k}}$ is the process noise vector including the jerk vector, the derivative of the angular rate vector, and so forth. Specifically, ${\mathit{B}}_{\mathit{k}}$, ${\mathbf{\Gamma }}_{\mathit{k}}$, and ${\mathit{W}}_{\mathit{k}}$ are given as

$$\begin{array}{c}{\mathit{B}}_{\mathit{k}}={\left[\begin{array}{c}\left[\frac{\Delta {\mathit{t}}^2}{2}-(\mathbf{\alpha }\Delta \mathit{t}-1+{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/{\mathbf{\alpha }}^2\right]\cdot {\mathit{C}}_{\mathit{n}}^{\mathit{e}}\cdot {\mathit{C}}_{\mathit{b}}^{\mathit{n}}\\ \left[\Delta \mathit{t}-(1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})/\mathbf{\alpha }\right]\cdot {\mathit{I}}_{3\times 3}\\ (1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})\cdot {\mathit{I}}_{3\times 3}\\ 0_{42\times 3}\end{array}\right]}_{51\times 3},\\ {\mathbf{\Gamma }}_{\mathit{k}(51\times 48)}=\left[\begin{array}{cc}\begin{array}{ccc}{\mathit{C}}_{\mathit{n}}^{\mathit{e}}\cdot {\mathit{C}}_{\mathit{b}}^{\mathit{n}}& \frac{\Delta {\mathit{t}}^3}{6}{\mathit{C}}_{\mathit{n}}^{\mathit{e}}\cdot {\mathit{C}}_{\mathit{b}}^{\mathit{n}}& 0_{3\times 3}\\ {\mathit{I}}_{3\times 3}& \frac{\Delta {\mathit{t}}^2}{2}{\mathit{I}}_{3\times 3}& \frac{\Delta {\mathit{t}}^2\cdot [{\mathit{v}}_{\mathit{n}\mathit{b}}^{\mathit{b}}\times ]}{2}\\ {\mathit{I}}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& \frac{\Delta {\mathit{t}}^2}{2}{\mathit{C}}_{3\times 3}\end{array}& \boxed{\begin{array}{c}\\ \\ 0_{12\times 39}\\ \\ \end{array}}\\ \boxed{\begin{array}{ccccccccc}& & & & 0_{39\times 9}& & & & \end{array}}& {\mathit{I}}_{39\times 39}\end{array}\right],\\ \begin{array}{c}{\mathit{W}}_{\mathit{k}}=[{({\mathit{w}}_{\mathit{a}})}^{\mathit{T}},{({\mathit{j}}_{\mathit{n}\mathit{b}}^{\mathit{b}})}^{\mathit{T}},{({\dot{\mathbf{\omega }}}_{\mathit{n}\mathit{b}}^{\mathit{b}})}^{\mathit{T}},{\mathit{w}}_{\mathbf{\omega }\mathit{x}},{\mathit{w}}_{\mathbf{\omega }\mathit{y}},{\mathit{w}}_{\mathbf{\omega }\mathit{z}},{({\mathit{w}}_{\mathit{b}\mathit{g}1})}^{\mathit{T}},{({\mathit{w}}_{\mathit{b}\mathit{g}2})}^{\mathit{T}},{({\mathit{w}}_{\mathit{b}\mathit{g}3})}^{\mathit{T}},\\ {({\mathit{w}}_{\mathit{b}\mathit{a}1})}^{\mathit{T}},{({\mathit{w}}_{\mathit{b}\mathit{a}2})}^{\mathit{T}},{({\mathit{w}}_{\mathit{b}\mathit{a}3})}^{\mathit{T}},{({\mathit{w}}_{\mathit{s}\mathit{g}1})}^{\mathit{T}},{({\mathit{w}}_{\mathit{s}\mathit{g}2})}^{\mathit{T}},{({\mathit{w}}_{\mathit{s}\mathit{g}3})}^{\mathit{T}},{({\mathit{w}}_{\mathit{s}\mathit{a}1})}^{\mathit{T}},{({\mathit{w}}_{\mathit{s}\mathit{a}2})}^{\mathit{T}},{({\mathit{w}}_{\mathit{s}\mathit{a}3})}^{\mathit{T}}{]}^{\mathit{T}}\end{array}\end{array}$$

As is well known, the Kalman filter performs the prediction of the state vector through two information update processes: time update and measurement update. Specifically, the one-step prediction and the variance propagation of the state vector proceed from epoch $\mathit{k}$ to $\mathit{k}+1$ during the time update.

$${\widehat{\mathit{X}}}_{\mathit{k}+1/\mathit{k}}=\mathit{f}({\widehat{\mathit{X}}}_{\mathit{k}})+{\mathit{B}}_{\mathit{k}}{\mathit{u}}_{\mathit{k}},$$

(42)

$${\mathit{P}}_{\mathit{k}+1/\mathit{k}}={\mathsf{\Phi }}_{\mathit{k}+1,\mathit{k}}{\mathit{P}}_{\mathit{k}}{\mathsf{\Phi }}_{\mathit{k}+1,\mathit{k}}^{\mathit{T}}+{\mathbf{\Gamma }}_{\mathit{k}}{\mathit{Q}}_{\mathit{k}}{\mathbf{\Gamma }}_{\mathit{k}}^{\mathit{T}},$$

(43)

where ${\mathit{P}}_{\mathit{k}}$ is the mean square error matrix of the state vector, and ${\mathit{Q}}_{\mathit{k}}$ is the a priori variance–covariance matrix of the process noise vector. ${\mathsf{\Phi }}_{\mathit{k}+1,\mathit{k}}$ is the Jacobian matrix of the nonlinear system model in Equation (40), with ${\mathsf{\Phi }}_{[\mathit{i},\mathit{j}]}=\frac{\partial {\mathit{f}}_{[\mathit{i}]}}{\partial {\mathit{X}}_{[\mathit{j}]}}$ ($\mathit{i},\mathit{j}$ represent the serial numbers; ${\mathit{f}}_{[\mathit{i}]}$ represents the i-th system equation; ${\mathit{X}}_{[\mathit{j}]}$ represents the j-th component of state vector $\mathit{X}$), as follows:

$${\mathsf{\Phi }}_{\mathit{k}+1,\mathit{k}}=\left[\begin{array}{cc}\begin{array}{ccccc}{\mathit{I}}_{3\times 3}& \Delta \mathit{t}\cdot {\mathit{C}}_{\mathit{n}}^{\mathit{e}}\cdot {\mathit{C}}_{\mathit{b}}^{\mathit{n}}& \raisebox{1ex}{$\partial {\mathit{r}}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathit{a}}_{\mathit{k}}$}\right.& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& \raisebox{1ex}{$\partial {\mathit{v}}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathit{v}}_{\mathit{k}}$}\right.& \raisebox{1ex}{$\partial {\mathit{v}}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathit{a}}_{\mathit{k}}$}\right.& 0_{3\times 3}& \raisebox{1ex}{$\partial {\mathit{v}}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathbf{\omega }}_{\mathit{k}}$}\right.\\ 0_{3\times 3}& 0_{3\times 3}& {\mathrm{e}}^{-\mathbf{\alpha }\Delta \mathit{t}}\cdot {\mathit{I}}_{3\times 3}& 0_{3\times 3}& 0\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& \raisebox{1ex}{$\partial {\mathbf{\theta }}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathbf{\theta }}_{\mathit{k}}$}\right.& \Delta \mathit{t}\cdot {\mathit{C}}_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& \raisebox{1ex}{$\partial {\mathbf{\omega }}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathbf{\omega }}_{\mathit{k}}$}\right.\end{array}& \boxed{\begin{array}{c}\\ \\ \\ 0_{15\times 36}\\ \\ \\ \end{array}}\\ \boxed{\begin{array}{ccccc}& & \begin{array}{ccccc}& & \begin{array}{cccc}& \begin{array}{cc}& 0_{36\times 15}\end{array}& & \end{array}& & \end{array}& & \end{array}}& \boxed{{\mathit{I}}_{36\times 36}}\end{array}\right],$$

with

$$\raisebox{1ex}{$\partial {\mathit{r}}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathit{a}}_{\mathit{k}}$}\right.={(\mathbf{\alpha }\Delta \mathit{t}-1+{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})\cdot {\mathit{C}}_{\mathit{n}}^{\mathit{e}}\cdot {\mathit{C}}_{\mathit{b}}^{\mathit{n}}/\mathbf{\alpha }}^2,\raisebox{1ex}{$\partial {\mathit{v}}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathit{v}}_{\mathit{k}}$}\right.={\mathit{I}}_{3\times 3}-\Delta \mathit{t}\left[{\mathbf{\omega }}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\times \right]+\frac{\Delta {\mathit{t}}^2}{2}{\left[{\mathbf{\omega }}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\times \right]}^2,$$

$$\raisebox{1ex}{$\partial {\mathit{v}}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathit{a}}_{\mathit{k}}$}\right.=(1-{\mathit{e}}^{-\mathbf{\alpha }\Delta \mathit{t}})\cdot {\mathit{I}}_{3\times 3}/\mathbf{\alpha },\mathrm{and}\raisebox{1ex}{$\partial {\mathit{v}}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathbf{\omega }}_{\mathit{k}}$}\right.=\Delta \mathit{t}\left[{\mathit{v}}_{\mathit{n}\mathit{b}(\mathit{k})}^{\mathit{b}}\times \right]+\frac{\Delta {\mathit{t}}^2}{2}{\mathit{M}}_{\mathit{k}},\mathrm{where}$$

$${\mathit{M}}_{\mathit{k}}=\left[\begin{array}{ccc}{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}+{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}& -2{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}+{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& -2{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}+{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\\ {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}-2{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}+{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}& -2{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}+{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\\ {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}-2{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}& {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}-2{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}& {\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}+{\mathit{v}}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}\end{array}\right],$$

$$\begin{gathered} \raisebox{1ex}{$\partial {\mathbf{\theta }}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathbf{\theta }}_{\mathit{k}}$}\right.=\left[\begin{array}{ccc}1& -\sin \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}\Delta \mathit{t}+\cos \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}\Delta \mathit{t}& 0\\ (\sin \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}-\cos \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}){(\sec \mathit{P})}^2\Delta \mathit{t}& 1+(\cos \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}+\sin \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}})(\tan \mathit{P})\Delta \mathit{t}& 0\\ (\sin \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}-\cos \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}})(\sec \mathit{P})(\tan \mathit{P})\Delta \mathit{t}& (\cos \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}+\sin \mathbf{\gamma }{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}})(\sec \mathit{P})\Delta \mathit{t}& 1\end{array}\right], \\ \raisebox{1ex}{$\partial {\mathbf{\omega }}_{\mathit{k}+1}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathbf{\omega }}_{\mathit{k}}$}\right.=\left[\begin{array}{ccc}{\mathit{e}}^{-\Delta \mathit{t}/{\mathit{T}}_{\mathit{x}}}& 0& 0\\ 0& {\mathit{e}}^{-\Delta \mathit{t}/{\mathit{T}}_{\mathit{y}}}& 0\\ 0& 0& {\mathit{e}}^{-\Delta \mathit{t}/{\mathit{T}}_{\mathit{z}}}\end{array}\right]. \end{gathered}$$

Subsequently, the measurement update is performed as follows when the measurements at epoch $\mathit{k}+1$ are available, thus accomplishing the state estimation:

$${\widehat{\mathit{X}}}_{\mathit{k}+1}={\widehat{\mathit{X}}}_{\mathit{k}+1/\mathit{k}}+{\mathit{K}}_{\mathit{k}+1}({\mathit{Z}}_{\mathit{k}+1}-\mathit{h}({\widehat{\mathit{X}}}_{\mathit{k}+1/\mathit{k}})),$$

(44)

$${\mathit{K}}_{\mathit{k}+1}={\mathit{P}}_{\mathit{k}+1/\mathit{k}}{\mathit{H}}_{\mathit{k}+1}^{\mathit{T}}{({\mathit{H}}_{\mathit{k}+1}{\mathit{P}}_{\mathit{k}+1/\mathit{k}}{\mathit{H}}_{\mathit{k}+1}^{\mathit{T}}+{\mathit{R}}_{\mathit{k}+1})}^{-1},$$

(45)

$${\mathit{P}}_{\mathit{k}+1}=(\mathit{I}-{\mathit{K}}_{\mathit{k}+1}{\mathit{H}}_{\mathit{k}+1}){\mathit{P}}_{\mathit{k}+1/\mathit{k}},$$

(46)

where ${\mathit{K}}_{\mathit{k}}$ is the Kalman filter gain matrix, And ${\mathit{R}}_{\mathit{k}}$ is the a priori variance–covariance matrix of the measurement noise vector. ${\mathit{H}}_{\mathit{k}}$ is the Jacobian matrix of the nonlinear measurement model in Equation (41), with ${\mathit{H}}_{[\mathit{i},\mathit{j}]}=\frac{\partial {\mathit{h}}_{[\mathit{i}]}}{\partial {\mathit{X}}_{[\mathit{j}]}}$ ($i,j$ represent the serial numbers; ${\mathit{h}}_{[\mathit{i}]}$ represents the i-th measurement equation; ${\mathit{X}}_{[\mathit{j}]}$ represents the j-th component of state vector $\mathit{X}$), as follows:

$${\mathit{H}}_{\mathit{k}}=\left[\begin{array}{c}\boxed{\begin{array}{cccccc}\boxed{0_{9\times 12}}& \boxed{\begin{array}{c}\frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}1(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}\\ \frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}\\ \frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}\end{array}}& \boxed{{\mathit{I}}_{9\times 9}}& \boxed{0_{9\times 9}}& \boxed{\begin{array}{ccc}\frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}1(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{g}1(\mathit{k})}}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& \frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{g}2(\mathit{k})}}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& \frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{g}3(\mathit{k})}}\end{array}}& \boxed{0_{9\times 9}}\end{array}}\\ \boxed{\begin{array}{cccccccc}\boxed{0_{9\times 6}}& \boxed{\begin{array}{c}\frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}1(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{a}}_{\mathit{k}}}\\ \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{a}}_{\mathit{k}}}\\ \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{a}}_{\mathit{k}}}\end{array}}& \boxed{0_{9\times 3}}& \boxed{\begin{array}{c}{0}_{3\times 3}\\ \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}\\ \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}\end{array}}& \boxed{0_{9\times 9}}& \boxed{{\mathit{I}}_{9\times 9}}& \boxed{0_{9\times 9}}& \boxed{\begin{array}{ccc}\frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}1(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{a}1(\mathit{k})}}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{a}2(\mathit{k})}}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{a}3(\mathit{k})}}\end{array}}\end{array}}\\ \boxed{\begin{array}{c}\begin{array}{cc}\boxed{\raisebox{1ex}{$\partial \mathit{P}{\mathit{R}}_{\mathit{k}}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathit{r}}_{\mathit{k}}$}\right.}& \boxed{0_{1\times 48}}\end{array}\\ ⋮\end{array}}\end{array}\right],$$

with

$$\frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}1(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}={\mathit{I}}_{3\times 3}+{\mathit{S}}_{\mathit{g}1},\frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}={\mathit{I}}_{3\times 3}+{\mathit{S}}_{\mathit{g}2},\frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}={\mathit{I}}_{3\times 3}+{\mathit{S}}_{\mathit{g}3},\frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}1(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{g}1(\mathit{k})}}=\frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{g}2(\mathit{k})}}=\frac{\partial {\mathbf{\omega }}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{g}3(\mathit{k})}}=\mathit{d}\mathit{i}\mathit{a}\mathit{g}({\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}},{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}},{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}),$$

$$\begin{gathered} \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}1(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{a}}_{\mathit{k}}}={\mathit{I}}_{3\times 3}+{\mathit{S}}_{\mathit{a}1},\frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{a}}_{\mathit{k}}}={\mathit{I}}_{3\times 3}+{\mathit{S}}_{\mathit{a}2},\frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{a}}_{\mathit{k}}}={\mathit{I}}_{3\times 3}+{\mathit{S}}_{\mathit{a}3}, \\ \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}=\left[\begin{array}{ccc}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_2(3)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_2(2)& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_2(2)-2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_2(1)& -2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_2(1)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_2(3)\\ -2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_2(2)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_2(1)& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_2(3)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_2(1)& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_2(3)-2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_2(2)\\ {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_2(1)-2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_2(3)& -2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_2(3)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_2(2)& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_2(1)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_2(2)\end{array}\right], \\ \frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathbf{\omega }}_{\mathit{k}}}=\left[\begin{array}{ccc}{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_3(3)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_3(2)& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_3(2)-2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_3(1)& -2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_3(1)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_3(3)\\ -2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_3(2)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_3(1)& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_3(3)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_3(1)& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_3(3)-2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_3(2)\\ {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_3(1)-2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_3(3)& -2{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_3(3)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}{\mathit{r}}_3(2)& {\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}{\mathit{r}}_3(1)+{\mathbf{\omega }}_{\mathit{n}\mathit{b}\mathit{y}}^{\mathit{b}}{\mathit{r}}_3(2)\end{array}\right], \end{gathered}$$

$\frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}1(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{a}1(\mathit{k})}}=\frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}2(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{a}2(\mathit{k})}}=\frac{\partial {\mathit{f}}_{\mathit{i}\mathit{b}3(\mathit{k})}^{\mathit{b}}}{\partial {\mathit{s}}_{\mathit{a}3(\mathit{k})}}=\mathit{d}\mathit{i}\mathit{a}\mathit{g}({\mathit{N}}_{\mathit{x}},{\mathit{N}}_{\mathit{y}},{\mathit{N}}_{\mathit{z}})$, wherein, ${\left[{\mathit{N}}_{\mathit{x}},{\mathit{N}}_{\mathit{y}},{\mathit{N}}_{\mathit{z}}\right]}^{\mathit{T}}=({\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}-{\mathit{C}}_{\mathit{n}}^{\mathit{b}}{\mathit{g}}^{\mathit{n}})$,

$$\raisebox{1ex}{$\partial \mathit{P}{\mathit{R}}_{\mathit{k}}$}\!\left/ \!\raisebox{-1ex}{$\partial {\mathit{r}}_{\mathit{k}}$}\right.=\left[\begin{array}{ccc}\raisebox{1ex}{${(\mathit{X}-{\mathit{X}}^{\mathit{j}})}_{\mathit{k}}$}\!\left/ \!\raisebox{-1ex}{${\mathbf{\rho }}_{\mathit{A}}^{\mathit{j}}{}_{(\mathit{k})}$}\right.& \raisebox{1ex}{${(\mathit{Y}-{\mathit{Y}}^{\mathit{j}})}_{\mathit{k}}$}\!\left/ \!\raisebox{-1ex}{${\mathbf{\rho }}_{\mathit{A}(\mathit{k})}^{\mathit{j}}$}\right.& \raisebox{1ex}{${(\mathit{Z}-{\mathit{Z}}^{\mathit{j}})}_{\mathit{k}}$}\!\left/ \!\raisebox{-1ex}{${\mathbf{\rho }}_{\mathit{A}(\mathit{k})}^{\mathit{j}}$}\right.\end{array}\right].$$

5. Road Test and Results

The proposed algorithm model under the unconventional integration strategy was adopted to process the navigation data from multiple low-cost IMUs and a GPS-integrated system on a land vehicle. Several road tests were performed by our ground-based vehicle navigation system with a Harxon Mini Survey Antenna GPS500 and three IMUs (FOS/05M, ADIS16405BMLZ, and Crossbow IMU440CA), as shown in Figure 4.

Table 1. Performance parameters of the inertial measurement units (IMUs). USD—United States dollars.

	Specifications of Gyroscope	Specifications of Accelerometer	Cost (USD)
FOS/05M	In-run bias stability ≤7.2°/h Angular random walk ≤5.5°/$\sqrt{\mathrm{hr}}$	In-run bias stability ≤1.0 mg Velocity random walk ≤1.0 m/s/$\sqrt{\mathrm{hr}}$	1500
ADIS16405BMLZ	In-run bias stability ≤25.2°/h Angular random walk ≤2.0°/$\sqrt{\mathrm{hr}}$	In-run bias stability ≤0.2 mg Velocity random walk ≤0.2 m/s/$\sqrt{\mathrm{hr}}$	600
Crossbow IMU440CA	In-run bias stability ≤10.0°/h Angular random walk ≤4.5°/$\sqrt{\mathrm{hr}}$	In-run bias stability ≤1.0 mg Velocity random walk ≤1.0 m/s/$\sqrt{\mathrm{hr}}$	1200

shows the performance parameters of the three IMUs. The experimental results are given in this section from one of our road tests. The used dataset was collected in Harbin, China, from which an 8-min data fragment of the data source was selected for demonstration purposes. Figure 5a reveals the environment where the measurements were made. As can be seen from Figure 5a, the chosen test environment was an urban highway, with the Songhua River and buildings nearby. Figure 5b,c depict the trajectory and velocity profile of the test vehicle.

In order to obtain the ground truth as the reference, we equipped a high-grade Fiber-Optic Gyroscope (FOG) inertial navigation system (INS) (gyroscope: constant drift less than 0.01°/h, random noise less than 0.001°/h; accelerometer: constant bias less than 100 $\mathsf{\mu }\mathrm{g}$, random noise less than 10 $\mathsf{\mu }\mathrm{g}$) developed by our research group on the test vehicle, as shown in Figure 4a. Hence, the benchmarks for attitude, velocity, and position could be provided by fusing the measurements of the high-grade INS and the GPS.

5.1. Further Insights into the Unconventional Integration Mechanism

The distinctions between the conventional integration mechanism (left side) and the unconventional counterpart (right side) are shown in Figure 6: (1) the embedment of the prediction of the kinematic states, (2) the introduction of IMU measurements, and (3) the navigation parameters directly used in the state vector.

The acceleration vector and angular rate vector are only regarded as inputs of the mechanization in the conventional error state-based Kalman filter, while the realistic measurements of the IMU directly participate in the measurement update process in the unconventional Kalman filter. In principle, the Kalman filter is equivalent to a sequential least square with a time-variant state vector and the process noises [30]. That is to say, the system model is composed of a group of virtual measurements for the state vector and reflects the connections between the state vectors from epoch to epoch. These extra virtual measurements mean that the measurement redundancies in the unconventional Kalman filter for the body acceleration vector ${\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ and the body angular rate vector ${\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ are evidently better than those in the conventional Kalman filter. Therefore, the accuracy of ${\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ and ${\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ in the unconventional KF for multiple low-cost IMUs and a GPS-integrated system will be undoubtedly improved because they are not only predicted but also measured. The prediction for ${\mathit{a}}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ and ${\mathbf{\omega }}_{\mathit{n}\mathit{b}}^{\mathit{b}}$ in the system model can be used as a rigorous reference to check on the performance of the IMU without increasing the complexity of the filtering structure. Furthermore, the system equations of the unconventional KF can act as the dynamic constraints for the navigation parameters, for example, assuming that ${\mathit{v}}_{\mathit{n}\mathit{b}\mathit{z}}^{\mathit{b}}=0$ and/or ${\mathit{v}}_{\mathit{n}\mathit{b}\mathit{x}}^{\mathit{b}}=0$.

Generally speaking, the most significant feature of the unconventional integration mechanism lies in the improvement of the overall measurement redundancy of the system through the system model based on the 3D kinematic trajectory model.

As discussed above, it is expected that the accuracy of the navigation solutions shall be improved by using the rigorous trajectory model as the system model in the unconventional KF. As the components of the state vector, the acceleration and angular rate vectors in the body frame (${\mathit{S}}_{\mathit{b}}$) will also profit from the unconventional KF time updates for the same reason. For better visual effects, Figure 7 and Figure 8 show the comparisons between the raw IMU outputs and filtered signals.

As can be seen from Figure 7, the overall variation of the IMU raw angular rates is slightly larger than that of the filtered angular rates. However, it is apparent from Figure 8 that the overall fluctuation of the filtered accelerations is significantly lower than that of the IMU raw specific forces. The improvement is due to the introduction of the novel system model in the unconventional Kalman filter. It is worth mentioning that the raw output of the accelerometer in the vertical direction includes acceleration due to gravity, while the corresponding filtered acceleration does not. That is why there is a large difference (about 10 ${\mathrm{m}/\mathrm{s}}^2$) between the raw and filtered acceleration values along the vertical axis in Figure 8; here, we only compare the fluctuating ranges of the two curves.

The comparison results from Figure 7 and Figure 8 confirm the fact that the influences of the IMU measurement noises on the final navigation solutions are effectively mitigated due to the participation of the IMU outputs in the KF measurement updates.

5.2. Verification of the Proposed Algorithm Model under the Unconventional Integration Strategy

Figure 9, Figure 10, Figure 11 and Figure 12 exhibit the solution accuracies for the kinematic trajectory parameters and attitude using the proposed algorithm model under the unconventional integration strategy.

Figure 9 shows the solution accuracy comparisons for 3D position using the basic 3D kinematic trajectory model or the practical model based on the “current” statistical Singer acceleration model (CSSAM). The overall 3D position accuracy using the proposed algorithm model is under 5 $\mathit{m}$, which is a great improvement compared with the accuracy (10 $\mathit{m}$) using the basic one. Figure 10 shows that the estimation errors for the velocity state vector in the three axial directions of the body frame using the proposed algorithm model are within $-0.05\sim 0.15$ $\mathit{m}/\mathit{s}$, $\pm 0.2$ $\mathit{m}/\mathit{s}$, and $\pm 0.2$ $\mathit{m}/\mathit{s}$, respectively. The velocity error in the direction of travel is obviously reduced compared with that using the basic model. Figure 11 shows that the estimation errors for acceleration state vector in the three axial directions of the body frame are within $\pm 0.5$ $\mathit{m}/{\mathit{s}}^2$, $\pm 1$ $\mathit{m}/{\mathit{s}}^2$, and $\pm 2$ $\mathit{m}/{\mathit{s}}^2$, respectively. As can be seen, the accuracy of the acceleration estimation is apparently improved by using the proposed algorithm model. As shown in Figure 12, the accuracies for attitude (pitch, roll, and heading) are within $\pm 2°$, $-0.2\sim 0.6°$, and $-1\sim 3°$, respectively. It is to be observed that the pitch angle has an apparent fluctuation from 3–4.5 min. The trajectory curve in Figure 5b illustrates that the vehicle experienced ups and downs in the test duration, which could affect the estimation of attitude angles (especially for pitch angle). It also indicates that the attitude estimation model still needs improvement so that it can be applied to more complex motion forms.

Apparently, the navigation parameters during the 8-min vehicle motion were estimated within acceptable ranges by adopting the proposed algorithm model under the unconventional integration strategy. Furthermore, the reason why the 3D position error using the basic trajectory model did not diverge is that the output of the central accelerometer with real-time calibration was utilized to regulate the acceleration estimation error when significant acceleration maneuvers occurred.

5.3. IMU Systematic Error Estimation

Since the systematic errors of these IMU arrays, i.e., biases and scale factor errors of gyroscopes and accelerometers, are individually modeled in Kalman filtering, the systematic error estimations of each IMU can be obtained separately, as illustrated in Figure 13, Figure 14, Figure 15 and Figure 16. As an example, this manuscript only shows the systematic errors of the central IMU due to space limitations. The common existing approach under the conventional inertial navigation mechanization adopts a set of common shared errors for all IMUs and, sometimes, these error parameters are from initial calibration results or technical specifications [7,28]. However, in fact, the a priori error model defined for a static low-cost MEMS inertial sensor needs to be checked and compensated for in the dynamic working environment as the vibration on a low-cost IMU might cause significant changes in its scale factors and noise level compared to those in the static case [31].

As can be seen from Figure 13, Figure 14, Figure 15 and Figure 16, in contrast to the a priori constant systematic error with the common existing approach, the systematic error estimations with the individual modeling method vary with time, which conforms better with the real situation. In other words, the individual modeling method under the unconventional integration strategy can adjust the systematic error estimations of each IMU according to the real-time measurements from each IMU. This technique can firstly verify if those IMUs really share the same systematic errors quantitatively, even if they are the common errors physically; then, they can be modeled either separately or combined, laying the groundwork for future research such as auto calibration and fault detection. While applying the individual modeling method for the IMU array, in spite of its characteristic properties, one must confront the following problem: high computation load caused by high-rate measurement updates in the Kalman filter, e.g., with IMUs whose measurement rate may be at 100 Hz. Although the modern computation capability is considerably improved, one cannot stand such a high measurement update rate, especially one so unreasonable, which renders it useless in real time. How to appropriately reduce the high-rate KF measurement updates without compromising the valuable information embedded in the high-rate measurements will become a topic for further study.

6. Conclusions

This research fused the information from one GPS receiver and three low-cost IMUs by applying an unconventional multi-sensor integration strategy. The enhanced and improved parts involved establishing a more practical 3D kinematic trajectory model based on the “current” statistical Singer acceleration model as the core of the system model, and individually modeling the measurements and systematic errors of these IMU arrays in Kalman filtering. The processing results of the experimental data demonstrated the success of the proposed algorithm model under the unconventional integration strategy with satisfactory solution performance and reliability. Future work will involve developing a more precise fusion algorithm by using the carrier phase information from the GPS.