An Automatic Vehicle Navigation System Based on Filters Integrating Inertial Navigation and Global Positioning Systems

Abstract

The key technologies for advanced autonomous vehicles include those relating to perception, decision making, and execution. Path-tracking control in autonomous vehicles is heavily dependent on their positioning system. Therefore, the development of low-cost and reliable positioning systems is crucial to improving perception and decision-making technologies for autonomous vehicles. Although the accuracy of the global positioning system (GPS) is extremely high, it is vulnerable to interference. Further, despite the low positioning accuracy of inertial navigation systems (INSs), their robustness is notably high. Therefore, an integrated navigation information method based on the Adaptive Particle Filter and the Iterative Kalman Filter (APF-IKF) was developed in this study. Firstly, an integrated navigation system model was established. Then, the IKF was adopted to estimate the speed, latitude and longitude errors of the INS. Thirdly, the newest estimated error results were introduced into the APF to optimize the distribution function, and the particle quality was improved. In this process, the APF can filter non-Gaussian noise, preliminarily estimate the error, optimize the result with the IKF and correct the output information of the INS with the final estimated error. Finally, by using differential GPS positioning as the benchmark, we built a real-vehicle test platform with a low-cost and low-precision GPS and inertial units and carried out a series of real-vehicle tests. The experimental results show that compared with the traditional KF method, APF-IKF can significantly improve the positioning accuracy and robustness of the system.

1. Introduction

Intelligent driving technology has emerged against the backdrop of a new wave of technological revolutions. It integrates modern sensor technology, information and communication technology, automatic control technology and more [1,2,3,4,5,6]. With the critical need for highly reliable and precise positioning and attitude information, navigation systems are increasingly playing a more significant role in intelligent driving systems, which makes them a hotbed of research and development [7,8,9,10].

With the advantages of high positioning accuracy, the GPS is widely used in the fields of aerospace and vehicles [11,12,13,14,15]. However, with weak anti-interference ability, strict requirements on the environment for vehicles and high susceptibility to electromagnetic waves or surrounding buildings, the GPS is not suitable for application in intelligent vehicles on its own. As INSs have a strong anti-interference ability and can independently obtain continuous three-dimensional space location and orientation data under any weather conditions, and in medium environments, they can measure the complete information of the motion state of a mass body with steady output. Therefore, INSs are increasingly applied in vehicles [16,17,18,19,20,21,22,23].

In practical applications, the price of high-precision inertial navigation systems is high. Meanwhile, although the widely used micro-electromechanical systems have a low cost, they have null drift and noise, meaning slow error accumulation occurs over time when they are used alone [24]. In order to make up for the deficiencies of INSs and the GPS, integrated navigation technologies have been widely developed. Alireza Shaghaghian proposed a GPS/INS fusion method based on the improved Kalman filter, which improves the estimation of position and velocity in navigation problems by integrating classic and modern control techniques [25]. Chen Zhimin and Yuan Xinqu researched the GPS/INS loose/tight coupling algorithm based on the EKF and achieved good positioning results [26]. Rongjian Wang proposed a multi-model filtering method for tracking different motion models and filtering to improve fusion positioning accuracy and robustness, enabling high-precision, high-availability positioning in complex environments [27]. To solve the problem of EKF performance degradation when system nonlinearity increases or measurement covariance is inaccurate, Jiang Haitao proposed an extended state observer (ESO) to improve the performance of loosely coupled GPS/INS integrated navigation with accurate measurement modeling [28]. Kyunghyun Ryu compared the effects of the navigation fusion method based on the EKF and UKF. Through the comparison of aircraft attitude estimation effects under a fixed trajectory and variable trajectory, it was verified that the UKF could estimate position and attitude more accurately in nonlinear regions [29].

To reduce the multi-path effect of GNSS in large cities, Guohao Zhang et al. proposed an integrated navigation positioning algorithm based on the adaptive Kalman filter. The positioning accuracy was improved on the basis of the traditional Kalman filter by adjusting the measurement noise covariance at different positioning accuracy values [30]. Yahui Liu proposed an integrated navigation method of the adaptive Kalman filter based on attenuation factors; this algorithm continuously updates the covariance of process and measurement noise and effectively suppresses white Gaussian noise by collecting the estimated and observed values in order to achieve the expected aim [31]. Chuang Zhang et al. studied the GPS/IMU/LOG integrated navigation system based on the AFPF, addressed sample depletion, improved the particle quality and verified the algorithm’s effectiveness with experiments [32]. The above research methods satisfactorily deal with Gaussian noise.

Although existing research methods such as improved Kalman filters, loose/tight coupling algorithms, multi-model filtering, extended state observers and adaptive Kalman filters have made significant achievements in enhancing the performance of INS/GPS integrated navigation systems, they still possess certain limitations. They are often sensitive to model linearization and Gaussian noise assumptions, which may render them ineffective in dealing with nonlinearities and non-Gaussian noise. Moreover, they might require intricate tuning and are quite susceptible to variations in sensor quality and environmental conditions, with high computational demands that complicate the management of multiple models simultaneously. Furthermore, despite the increase in system complexity and cost due to the incorporation of auxiliary sensors, improper fusion process management could introduce new sources of error. Moreover, environmental noise has a non-ignorable effect on GPS/INS integrated navigation, which is influenced not only by Gaussian white noise but also by non-Gaussian noise caused by random environmental noise [33].

In practical industrial applications, the predominant approach employed in a unified navigation system is the Kalman filtering algorithm. The Kalman filtering algorithm is primarily applicable to linear systems and situations involving Gaussian noise. Its performance deteriorates significantly in the case of nonlinear systems. The auxiliary particle filter integrates the concepts of particle filtering and iterative Kalman filtering, enabling it to address nonlinear systems and non-Gaussian noise scenarios. Assisted particle filtering is capable of handling a wide range of complex dynamic systems and measurement models. By sampling from the prior probability density function and iteratively updating and resampling the samples based on the measurement data, assisted particle filtering can effectively approximate the true posterior probability density function, leading to more precise estimation results.

The posterior probability density function of the system may exhibit a multimodal distribution, posing a challenge for the Kalman filtering algorithm. However, the auxiliary particle filter excels in representing the multimodal distribution through its particle set, thereby yielding more precise estimation results. Even if the model exhibits errors or noise in its statistical characteristics that deviate from practical assumptions, the auxiliary particle filter can still yield relatively accurate estimation results. In terms of robustness, the auxiliary particle filter demonstrates resilience to variations in model errors and noise statistical characteristics.

In this study, we thus developed an information fusion method for navigation systems using APF-IKF (Adaptive Particle Filter–Iterated Kalman Filter). We established a combined navigation error model that informed the development of the APF-IKF algorithm. The empirical probability distribution, as well as the system state estimation and variance of the APF, are optimized by incorporating the latest observational data from the IKF. This approach addresses issues such as particle impoverishment and degradation, effectively mitigating the influence of non-Gaussian noise. By integrating the current error value with the estimated and measured values, we can derive an optimal error estimate in real-time, which is then used to refine the INS information, producing the final output. This method effectively counteracts external disturbances in measurement values and model errors, enhancing the precision of the integrated navigation system. To validate the efficacy and superiority of the algorithm presented in this study, we constructed a MATLAB (2017 version)/Simulink error model. A reference trajectory was generated based on real-vehicle testing, utilizing the dual-antenna differential GPS. We then compared the positioning results of a standalone INS, the non-differential GPS and the integrated INS/GPS, analyzing the experimental outcomes.

The main contribution of this study is the proposal of a novel INS/GPS integration method that combines an Adaptive Particle Filter (APF) with the Iterative Kalman Filter (IKF) to achieve more accurate state estimation. Our approach is innovative in the following aspects:

(1)

We developed an improved APF algorithm that adjusts the empirical probability distribution of particles by incorporating a combined navigation error model, thereby effectively addressing non-Gaussian noise and particle degradation issues.

(2)

By integrating the IKF with an APF, our method provides more precise error estimates and corrections without compromising computational efficiency, which is crucial to improving positioning accuracy in dynamic environments.

(3)

We conducted experimental validations that demonstrate the superiority of our method in terms of accuracy and robustness over traditional INS/GPS integration methods, particularly in complex environments.

(4)

We also showed how the effectiveness of the proposed method can be further validated with Hardware-In-the-Loop (HIL) simulations and real-vehicle tests, providing a solid foundation for practical applications.

In summary, the results of this research study not only show enhanced precision and reliability of the INS/GPS integrated navigation system but also open up new possibilities for the application of low-cost MEMS sensors in the field of high-precision navigation. These contributions are significant for the development of intelligent driving technology and may promote the widespread use of autonomous vehicles.

This paper is organized as follows: In Section 2, the GPS/INS integrated navigation system model is presented as our proposal for a combined estimation method. In Section 3, the combined APF-IKF and the iteration method are proposed in detail. In Section 4, we present the verification of the proposed method based on experimental vehicle tests. Finally, the conclusion is described in Section 5.

2. Steering and Braking Cooperative Control Model

The development of an integrated INS and GPS navigation model is presented in this section. The directions of east, north and sky were selected as the reference coordinate system. The system state variable X(t) mainly includes the error angle of the platform, speed and position deviations, and the null drift errors of the single gyroscope and accelerometer, which can be expressed as

$$X(t)=\left[\begin{array}{ccccccccccccccc}{\Sigma }_E& {\Sigma }_N& {\Sigma }_U& \delta V_E& \delta V_N& \delta V_U& \delta L& \delta \lambda & \delta H& {\epsilon }_x& {\epsilon }_y& {\epsilon }_z& {\Delta }_x& {\Delta }_y& {\Delta }_z\end{array}\right]$$

Above, X(t) is a fifteen-dimensional system state vector, and ${\Sigma }_{\mathrm{E}}$, ${\Sigma }_{\mathrm{N}}$ and ${\Sigma }_{\mathrm{U}}$ represent platform misalignment angles in the directions of east, north and sky in the navigation coordinate system, respectively. $\delta V_E$, $\delta V_N$ and $\delta V_U$ represent the speed errors in the directions of east, north and sky in the navigation coordinate system, respectively. $\delta L$, $\delta \lambda$ and $\delta H$ are the longitude, latitude and elevation deviations in the directions of east, north and sky, respectively. ${\epsilon }_x$, ${\epsilon }_y$ and ${\epsilon }_z$ are the gyroscopic drift errors. ${\Delta }_x$, ${\Delta }_y$ and ${\Delta }_z$ are the accelerometer errors. The modeling of the errors of the inertial navigation system is shown as follows.

The errors of the gyroscope mainly include the null drift, high-frequency white noise drift and slow-varying drift of the Markov chain every time the application starts. The reference gyroscope errors can be expressed as

$${\epsilon }_i={\epsilon }_{bi}+{\epsilon }_{ri}+{\omega }_{gi}$$

(1)

Above, ${\epsilon }_b$ represents the null drift of the gyroscope, ${\epsilon }_r$ represents a first-order Markov process, ${\omega }_g$ represents white noise drift and $i$ represents x, y and z.

${\epsilon }_{bi}$ satisfies

$${\dot{\epsilon }}_{bi}=0$$

(2)

After the gyroscope starts each time, the null drift value does not change.

The ${\epsilon }_{ri}$ formula is shown as follows:

$${\dot{\epsilon }}_{ri}=-{\displaystyle \frac{1}{{\tau }_G}}{\epsilon }_{ri}+{\omega }_{ri}$$

(3)

The gyroscope produces a slow fluctuation over time based on the drift of a constant value. This fluctuation is not completely random and has a certain correlation with the value of the previous moment. Therefore, the slow drift can be expressed with the first-order Markov process.

Accelerometer errors mainly include the constant value drift and the white noise error. The error model of the accelerometer is the following formula:

$${\dot{\Delta }}_i={\Delta }_{bi}+{\omega }_{ai}$$

(4)

Above, ${\omega }_a$ is white noise, ${\Delta }_b$ is the constant drift error and $i$ represents x, y and z. ${\Delta }_b$ has the following formula:

$${\dot{\Delta }}_{bi}=0$$

(5)

In this study, we selected ENU as the navigation coordinate system, and the corrected angular velocity of command platform ${\omega }_{cmd}^n$ can be expressed as

$${\omega }_{cmd}^n=\left[\begin{array}{c}-{\displaystyle \frac{V_N}{R_M+h}}\\ {\omega }_{ie}\cos (L)+{\displaystyle \frac{V_E}{R_N+h}}\\ {\omega }_{ie}\sin (L)+{\displaystyle \frac{V_E}{R_N+h}}\tan (L)\end{array}\right]$$

(6)

Above, $R_M$ is the principal radius of the curvature with the carrier located in the meridian, and $R_N$ is the principal radius of the curvature with the carrier located in the prime vertical; $V_E$ and $V_N$ indicate the eastward and northward speed of the carrier motion, respectively, decomposed in navigation coordinates. There are also errors in ${\omega }_{cmd}^n$ due to errors in the INS. Let us suppose that the real platform correction angular speed is ${\omega }_{ip}^p$, ${\omega }_{ip}^p$, which can be expressed as

$${\omega }_{ip}^p={\omega }_{cmd}^n+\delta {\omega }_{en}^n+\delta {\omega }_{ie}^n-{\epsilon }_i^p$$

(7)

Above, ${\epsilon }_i^p$ is the drift error of the INS, $\delta {\omega }_{en}^n$ is the projection of the geographic coordinate error caused by the change in the latitude of the carrier in the navigation coordinates and $\delta {\omega }_{ie}^n$ indicates the projection of the angular speed caused by the rotation of the earth in the navigation coordinates.

In theory, the platform angle of platform coordinate p created by the navigation computer should be consistent with that of navigation coordinate n. However, due to the existence of various deviations of the navigation system, there is a misalignment angle between the two coordinate systems, which is denoted by ${\Phi }^n$ and can be expressed as

$${\Sigma }^n=C_p^n{\omega }_{ip}^p-{\omega }_{cmd}^n$$

(8)

Above, ${\Sigma }^n$ is ${\left[\begin{array}{ccc}{\Sigma }_E& {\Sigma }_N& {\Sigma }_U\end{array}\right]}^{\mathrm{T}}$, and $C_p^n$ is the pose transformation matrix from system p to system n.

Let us suppose that the space transformation matrix from the vehicle coordinate system to the navigation coordinate system is $C_b^n$; thus, the transformation matrix from the vehicle coordinate system to the real coordinate system can be obtained, and the vehicle attitude angle error can be expressed as

$$\begin{array}{l}{\Sigma }^n=C_p^n{\omega }_{in}^n+C_p^n\delta {\omega }_{ie}^p+C_p^n\delta {\omega }_{en}^p-C_p^n{\epsilon }^p-{\omega }_{in}^n\\ \hspace{1em}=C_p^n{\omega }_{in}^n+\delta {\omega }_{ie}^n+{\omega }_{en}^n-{\epsilon }^n-{\omega }_{in}^n\end{array}$$

(9)

In the absence of error, the speed differential equation of navigation system is

$${\dot{V}}_{en}^n=C_b^n{f}^b-\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times V_{en}^n+g^n$$

(10)

$V_{en}^n$ is the eastward, northward and upward speed. There is an error in $C_b^n$ in the above equation. Therefore, the real value of $C_b^n$ without error is ${\widehat{C}}_b^n$ and can be expressed as

$${\widehat{C}}_b^n=\left(I-{\Sigma }^n\right)C_b^n$$

(11)

Above,

$${\Sigma }^n=\left[\begin{array}{ccc}0& -{\Sigma }_U& {\Sigma }_N\\ {\Sigma }_U& 0& {\Sigma }_E\\ -{\Sigma }_N& {\Sigma }_E& 0\end{array}\right]$$

There are also errors in ${\omega }_{ie}^n$ and ${\omega }_{en}^n$, and their real values can be represented by ${\widehat{\omega }}_{ie}$ and ${\widehat{\omega }}_{en}$:

$${\widehat{\omega }}_{ie}={\omega }_{ie}^n+\delta {\omega }_{ie}^n$$

(12)

$${\widehat{\omega }}_{en}={\omega }_{en}^n+\delta {\omega }_{en}^n$$

(13)

Similarly, there is also an error in the accelerometers, and the real value of $f^b$ can be expressed as ${\widehat{f}}^b$:

$${\widehat{f}}^b=f^b+{\Delta }^b$$

(14)

We substitute the real value of information into Equation (10) to obtain the real speed differential equation:

$${\dot{\widehat{V}}}_{en}^n={\widehat{C}}_b^n{\widehat{f}}^b-\left(2{\widehat{\omega }}_{ie}^n+{\widehat{\omega }}_{en}^n\right)\times {\widehat{V}}_{en}^n+g^n$$

(15)

It can be obtained from Equations (10)–(15) that

$$\delta {\dot{\widehat{V}}}_{en}^n={\dot{\widehat{V}}}_{en}^n-{\dot{V}}_{en}^n$$

(16)

We substitute Equations (9)–(14) into Equation (16) to obtain the speed error differential equation, as shown in Equation (17):

$$\begin{array}{l}\delta {\dot{V}}_{en}^n=-{\Sigma }^n{f}^n+C_b^n{\Delta }^b-\left(2{\widehat{\omega }}_{ie}^n+\delta {\omega }_{en}^n\right)\times V_{en}^n-\left(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}{e}^n\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\times V_{en}^n-\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times V_{en}^n+g^n\end{array}$$

(17)

The position error of the inertial navigation system is mainly caused by the speed error, and its equation can be expressed as

$$\delta \dot{L}=\delta V_N{\displaystyle \frac{1}{R_M+h}}-\delta h{\displaystyle \frac{V_N}{{\left(R_M+h\right)}^2}}$$

(18)

$$\delta \dot{\lambda }=\delta V_E{\displaystyle \frac{\sec L}{R_N+h}}+\delta L{\displaystyle \frac{V_E\tan L\sec L}{R_N+h}}-\delta h{\displaystyle \frac{V_E\sec L}{{\left(R_N+h\right)}^2}}$$

(19)

$$\delta \dot{h}=\delta V_U$$

(20)

The state equation of the INS/GPS integrated navigation system can be obtained by using the above formulas. Coefficient matrix $A\left(t\right)$ of the system is

$$A\left(t\right)=\left[\begin{array}{cccc}{F}_{3\times 3}& F_{3\times 3}^{″}& E_{3\times 3}& T_{3\times 6}\\ F_{3\times 3}^{\prime }& F_{3\times 3}^{‴}& E_{3\times 3}^{\prime }& T_{3\times 6}^{\prime }\\ 0_{3\times 3}& F_{3\times 3}^{″″}& E_{3\times 3}^{″}& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}& E_{6\times 6}^{‴}\end{array}\right]$$

Matrix $B\left(t\right)$ is

$$B\left(t\right)=\left[\begin{array}{cc}{C}_b^n& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\\ 0_{9\times 3}& 0_{9\times 3}\end{array}\right]$$

Above, $F_{3\times 3}$, $F_{3\times 3}^{\prime }$, $F_{3\times 3}^{″}$, $F_{3\times 3}^{‴}$, $F_{3\times 3}^{″″}$, $E_{3\times 3}$, $E_{3\times 3}^{\prime }$, $E_{3\times 3}^{″}$, $E_{3\times 3}^{‴}$, $T_{3\times 6}$ and $T_{3\times 6}^{\prime }$ are

$$F_{3\times 3}=\left[\begin{array}{ccc}0& {\omega }_{ie}\sin L+{\displaystyle \frac{V_E\tan L}{R_N+h}}& {\omega }_{ie}\cos L+{\displaystyle \frac{V_E}{R_N+h}}\\ -\left({\omega }_{ie}\sin L+{\displaystyle \frac{V_E\tan L}{R_N+h}}\right)& 0& -{\displaystyle \frac{V_N}{R_M+h}}\\ {\omega }_{ie}\cos L+{\displaystyle \frac{V_E}{R_N+h}}& {\displaystyle \frac{V_N}{R_M+h}}& 0\end{array}\right]$$

$$F_{3\times 3}^{\prime }=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& f_e\\ -f_n& f_e& 0\end{array}\right]$$

$$F_{3\times 3}^{″}=\left[\begin{array}{ccc}0& -{\displaystyle \frac{1}{R_M+h}}& 0\\ {\displaystyle \frac{1}{R_N+h}}& 0& 0\\ {\displaystyle \frac{\tan L}{R_N+h}}& 0& 0\end{array}\right]$$

$$F_{3\times 3}^{‴}=\left[\begin{array}{ccc}{\displaystyle \frac{V_N\tan L}{R_N+h}}-{\displaystyle \frac{V_U}{R_N+h}}& 2{\omega }_{ie}\sin L+{\displaystyle \frac{V_E\tan L}{R_N+h}}& 2{\omega }_{ie}\cos L+{\displaystyle \frac{V_E}{R_N+h}}\\ -2{\omega }_{ie}\sin L+{\displaystyle \frac{V_E\tan L}{R_N+h}}& -{\displaystyle \frac{V_U}{R_M+h}}& -{\displaystyle \frac{V_N}{R_M+h}}\\ 2\left({\omega }_{ie}\cos L+{\displaystyle \frac{V_E}{R_N+h}}\right)& {\displaystyle \frac{2V_N}{R_M+h}}& 0\end{array}\right]$$

$$F_{3\times 3}^{″″}=\left[\begin{array}{ccc}0& {\displaystyle \frac{1}{R_M+h}}& 0\\ {\displaystyle \frac{\sec L}{R_N+h}}& 0& 0\\ 0& 0& 1\end{array}\right]$$

$$E_{3\times 3}=\left[\begin{array}{ccc}0& 0& 0\\ -{\omega }_{ie}\sin L& 0& 0\\ {\omega }_{ie}\cos L+{\displaystyle \frac{V_E}{R_N+h}}{\sec }^{2}L& 0& 0\end{array}\right]$$

$$E_{3\times 3}^{\prime }=\left[\begin{array}{ccc}2{\omega }_{ie}\cos L{V}_N+{\displaystyle \frac{V_E{V}_N}{R_N+h}}{\sec }^{2}L+2{\omega }_{ie}\sin L{V}_U& 0& 0\\ -\left(2{\omega }_{ie}\cos L{V}_E+{\displaystyle \frac{V_E^2}{R_N+h}}{\sec }^{2}L\right)& 0& 0\\ -2{\omega }_{ie}\sin L{V}_E& 0& 0\end{array}\right]$$

$$E_{3\times 3}^{″}=\left[\begin{array}{ccc}0& 0& 0\\ {\displaystyle \frac{V_E\sec L\tan L}{R_N+h}}& 0& 0\\ 0& 0& 0\end{array}\right]$$

$$E_{3\times 3}^{‴}=diag\left[-\alpha ,-\alpha ,-\alpha ,-\beta ,-\beta ,-\beta \right]$$

$$T_{6\times 6}=\left[\begin{array}{cc}{C}_b^n& 0_{3\times 3}\end{array}\right]$$

$$T_{6\times 6}^{\prime }=\left[\begin{array}{cc}{0}_{3\times 3}& C_b^n\end{array}\right]$$

INS/GPS integrated navigation takes the difference between the speed and position received by the GPS and those output by the INS as the measured values. Let us suppose that the real speed and position of the carrier in navigation coordinates are $V_E$, $V_N$, $\lambda$ and $L$. Moreover, $V_{ie}$, $V_{in}$, ${\lambda }_i$ and $L_i$ represent the speed and position information of the INS output resolved in navigation coordinates, while $V_{ge}$, $V_{gn}$, ${\lambda }_g$ and $L_g$ represent that resolved by the GPS. The above can be obtained with the following equations:

$$V_{ie}=V_e+\delta V_{ie}$$

(21)

$$V_{in}=V_n+\delta V_{in}$$

(22)

$$V_{ge}=V_e+\delta V_{ge}$$

(23)

$$V_{gn}=V_n+\delta V_{gn}$$

(24)

$${\lambda }_i=\lambda +\delta {\lambda }_i$$

(25)

$$L_i=L+\delta L_i$$

(26)

$${\lambda }_g=\lambda +\delta {\lambda }_g$$

(27)

$$L_g=L+\delta L_g$$

(28)

The measurement equation of the system is

$$Z\left(t\right)=H\left(t\right)X\left(t\right)+\omega \left(t\right)$$

(29)

Above, $\omega \left(t\right)$ is the measurement noise.

The system error measurement value is

$$Z=\left[\begin{array}{c}{V}_{ie}-V_{ge}\\ V_{in}-V_{gn}\\ {\lambda }_i-{\lambda }_g\\ L_i-L_g\end{array}\right]$$

(30)

Coefficient matrix $H\left(t\right)$ is

$$H\left(t\right)={\left[\begin{array}{cccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& \dots \\ 0& 0& 0& 0& 1& 0& 0& 0& 0& \dots \\ 0& 0& 0& 0& 0& 0& 1& 0& 0& \dots \\ 0& 0& 0& 0& 0& 0& 0& 1& 0& \dots \end{array}\right]}_{4\times 15}$$

(31)

3. Integrated Navigation Algorithm

The integrated navigation system depicted in Figure 1 leverages the APF-IKF approach. Navigation systems often encounter noise patterns that deviate from the Gaussian distribution due to the inherent characteristics of the inertial navigation system (INS). This discrepancy can lead to the suboptimal performance of algorithms such as the Kalman filter (KF) and its extended variant (EKF). To address these challenges posed by nonlinearity and non-Gaussian noise, in this study, we employed the Adaptive Particle Filter (APF) algorithm. The APF enhances the capability of handling the underlying complexities within the data. Subsequently, the Iterated Kalman Filter (IKF) is employed to refine the estimation of errors further. This combination not only improves the system’s adaptability to diverse environmental conditions but also significantly boosts the accuracy and reliability of the navigation outcomes.

The principle of the system illustrated in the provided schematic is as follows.

Initially, the system collects acceleration and angular velocity data with an Inertial Measurement Unit (IMU). These data are fed into the inertial navigation system (INS) to calculate an initial estimate of the vehicle’s position and velocity. The INS computes these estimates by using data from the IMU without external references, leading to the accumulation of error over time. To mitigate this effect, the system employs the Iterative Kalman Filter (IKF), which iteratively refines the INS output to optimize the estimate and reduce prediction errors. However, the IKF has limited capabilities when dealing with non-Gaussian noise and nonlinear systems.

Firstly, the Adaptive Particle Filter (APF), with its robust capability of handling nonlinearity and non-Gaussian noise, assists the system in the localization process. The APF is a state estimation technique based on the Monte Carlo method, particularly suitable for state estimation in nonlinear and non-Gaussian environments. It uses a set of particles to represent the probability distribution and continuously adjusts the particles’ positions and weights as new observations are made. Resampling is performed based on the particles’ weights to decrease the number of low-weight particles and increase that of high-weight ones, thereby improving the quality of the particle set. The APF dynamically adjusts the number and parameters of particles according to observation data to adapt to changes in the system state. It introduces a certain level of randomness to ensure that the particle set covers the possible regions of the true state space. The advantage of the APF lies in its adaptability and robustness to complex environments, especially when dealing with non-Gaussian noise and nonlinear systems, compensating for the IKF’s shortcomings in these areas.

Secondly, the IKF further enhances the accuracy of the position and velocity estimates. The Iterative Kalman Filter (IKF) is a nonlinear state estimation method that builds upon the traditional Kalman filter (KF) by incorporating an iterative process to improve estimation precision. Compared with the standard KF, the IKF can better handle the system’s nonlinear characteristics. The core of the IKF lies in its gradual linearization of the system model during each iteration to update the state estimate. This process typically involves initialization, where the initial state vector and covariance matrix are set according to the system’s initial conditions. Prediction follows, using the system’s dynamic model to predict the next time step’s state vector and covariance matrix. Then, in the updating stage, observation data and prediction results are incorporated to refine the state vector and covariance matrix based on gain calculation. Finally, the iterative step repeats prediction and updating until the convergence criteria are met or a predetermined number of iterations is reached. The strength of the IKF lies in its adaptability to nonlinear systems and its ability to provide more precise state estimates.

Ultimately, after a series of complex fusion and filtering processes, the system outputs precise information about the vehicle’s position, velocity, direction and yaw angle. Overall, the schematic presents an advanced multisensor fusion technology that combines the advantages of various filters for the vehicle navigation system to achieve high-precision localization in changing environments.

The system model can be described as

$$\left\{\begin{array}{c}{x}_{k+1}=F_{k+1\left|k\right.}{x}_k+{\Gamma }_k{\omega }_k\hfill \\ y_{k+1}=H_{k+1}{x}_{k+1}+v_{k+1}\hfill \end{array}\right.$$

(32)

Above, ${\omega }_k$ and $v_{k+1}$ represent process and measurement noise with variances of $Q$ and $R$, respectively, and the two noises are independently and identically distributed. The specific process of the APF-IKF algorithm is described as follows:

The APF is used to estimate the error. When $k=0$, the quantity of state $x_0^i$ ($i=1,\dots N$) is extracted from prior probability $p\left(x_0\right)$; let ${\tilde{w}}_0^i=1/N$, ${\delta }_k^i=x_k^i$, $i^j=j$, $j=1,2,\dots ,N$; then, calculate ${\mu }_{k+1}^i=E\left[x_{k+1}^i\left|x_k^i\right.\right]$.

The weight of the sample particles can be calculated with Equation (33):

$$w_{k+1}^j{\propto }_k^j{\displaystyle \frac{p\left(y_{k+1}\left|x_{k+1}^j\right.\right)p\left(x_{k+1}^j{x}_k^j\right)}{q\left(x_{k+1}^j,{\mu }_{k+1}^j\left|y_{1:k+1}\right.\right)}}={\displaystyle \frac{p\left(y_{k+1}\left|x_{k+1}^j\right.\right)}{p\left(y_{k+1}\left|{\mu }_{k+1}^j\right.\right)}}$$

(33)

The standard particle weights are

$${\tilde{w}}_{k+1}^j={\displaystyle \frac{w_{k+1}^j}{{\displaystyle {\sum }_{j=1}^N{w}_{k+1}^j}}}$$

(34)

We then measure the weight of the particles. When the weight satisfies Equation (35), these particles are copied to produce new particles, and the other particles are removed.

$${\displaystyle \sum _{j=1}^{i-1}{\tilde{w}}_k^j}<u_i<{\displaystyle \sum _{j=1}^i{\tilde{w}}_k^j}$$

(35)

The $u_i$ is a random number between 0 and 1.

Finally, the eastward and northward speed errors and the latitude and longitude errors can be estimated with Equation (36):

$${\widehat{x}}_{k+1}=E\left(x_{k+1}\right)={\displaystyle \sum _{j=1}^N{x}_{k+1}^{i^j}{\tilde{w}}_{k+1}^j{P}_{k+1}={\displaystyle \sum _{j=1}^N\left(\left(x_{k+1}^{i^j}-E\left(x_{k=1}\right)\right){\left(x_{k=1}^{i^j}-E\left(x_{k+1}\right)\right)}^T\right)}}$$

(36)

The IKF algorithm is used to optimize the results of the APF, and the specific expression is as follows:

When $k=0$,

$${\stackrel{⌢}{x}}_0={\widehat{x}}_{k+1}{P}_0=P_{k+1}$$

(37)

For $k=1:N$,

$${\stackrel{⌢}{x}}_{k\left|k-1\right.}=F_k{x}_{k-1\left|k-1\right.}{P}_{k\left|k-1\right.}=F_k{P}_{k-1\left|k-1\right.}{F}_k^T+Q_k$$

(38)

For $j=1:c$,

$${\stackrel{⌢}{x}}_{k\left|k-1\right.}^j=f\left(x_{k-1\left|k-1\right.}^j\right){\widehat{y}}_{k\left|k-1\right.}^j=h\left({\widehat{x}}_{k\left|k-1\right.}^j\right)P_{k\left|k-1\right.}^j=F_{kj}{P}_{k-1\left|k-1\right.}{F}_{kj}^T+Q_k$$

(39)

$$K_k=P_{k\left|k-1\right.}^j{H}_{kj}^T{\left[H_{kj}{P}_{k\left|k-1\right.}^j{H}_{kj}^T+R_k\right]}^{-1}$$

(40)

$${\widehat{x}}_{kj}={\widehat{x}}_{k\overline{j}}+K_k\left[y_k-h\left({\widehat{x}}_{k\overline{j}}\right)\right]P_{k\left|k\right.}^j=\left[I-K_k{H}_{kj}\right]P_{k\left|k-1\right.}^j$$

(41)

Above, $j=1,2,\dots ,$ is the iteration index.

4. Experimental Results

The RT2500 strap-down inertial navigation system was used in this experiment. Accelerometers and angular accelerometers were integrated into the system to reduce costs. In this study, the non-differential GPS was used in the integrated navigation system. The information collected by the GPS included position and speed. We integrated the data of the single-antenna GPS with the data of the INS. In order to ensure the accuracy and validity of the final estimation results, the differential dual-antenna GPS was used for experimental verification.

Auto box (dSPACE) was adopted as the communication equipment between INS/GPS and MATLAB. Figure 2 shows the experimental equipment.

Data Collection Process and Methodology

(1)

Pre-Experimental Setup

The GPS antenna was mounted on top of the car to ensure that it was clearly visible to the satellite for accurate signal reception. The RT2500 INS was initialized and calibrated according to the manufacturer’s instructions to ensure accurate initial readings. The GPS and the INS converted the collected signals into CAN signals and transmitted them to control desk PC software on the through Auto box. The software application then analyzed the collected GPS and INS CAN signals by calling the compiled MATLAB program.

(2)

Data Collection During Experiment

As the vehicle moved, the INS continuously collected acceleration and angular rate data to track the vehicle’s motion. Simultaneously, the non-differential GPS collected position and speed data at regular intervals, which were transmitted to the MATLAB environment through Auto box. We used MATLAB to build the algorithm model, and the output of the system included information such as the longitude, latitude and heading angle of the vehicle. To verify the validity of the proposed method, we selected a rectangular road in China. As shown in Figure 3, we marked the shape of the road on Google Maps. In order to distinguish the robustness of the algorithm, we applied electromagnetic interference at a certain point in the road.

Figure 4, Figure 5 and Figure 6 present the acceleration data recorded with the IMU, detailing the movement along the three axes. Figure 7, Figure 8 and Figure 9 display the angular velocity data for the corresponding axes of the gyroscope. The collected data encompassed actual measurements, superimposed white noise and accumulated drift, all of which contribute to a reduction in precision in inertial navigation. We fed these unprocessed readings to our algorithmic model, which processed them to determine velocity and location information.

In our analysis, the X, Y and Z axes were designated as representing the transverse, longitudinal and vertical dimensions, respectively. Figure 4 illustrates the side-to-side or lateral acceleration experienced by the vehicle. Conversely, Figure 9 captures the vehicle’s rotational movements around the vertical axis and its yawing maneuvers. The prominent spikes observable in both Figure 4 and Figure 9 serve as indicators of the vehicle turning around four distinct corners, highlighting the dynamic nature of its trajectory.

We chose the differential GPS as the positioning reference curve. As shown in Figure 10, we parsed the raw data from the non-differential GPS and IMU outputs. Then, the three sets of data were compared in the same coordinate system. The horizontal coordinate was latitude and the vertical one longitude. As can be seen from the figure, the GPS’s positioning accuracy is high. However, within the region formed by the longitude of 116.0002 to 116.0005 and the latitude of 36.432 to 36.4325, the GPS data are distorted by interference. Due to the accumulated errors in the inertial navigation system, the positioning errors are gradually amplified.

As shown in Figure 11, we further took the differential GPS as the benchmark and compared the final effect of the KF algorithm and the APF-IKF algorithm in the same coordinate system. In the figure, we can see that in the region where the GPS is not disturbed, the positioning curve analyzed by the KF algorithm deviates from the reference curve, but the positioning error is relatively small. After the GPS is subjected to interference, the location data analyzed by the KF algorithm are affected by the distortion of the GPS data. Therefore, the robustness of the KF algorithm is poor. In contrast, the data analyzed by the APF-IKF algorithm are consistent with the reference curve of the differential GPS output. In the area of GPS interference, the APF-IKF algorithm can accurately estimate the error and compensate for the navigation system’s shortcomings.

The experimental validation confirmed the effectiveness and superiority of the APF-IKF algorithm within the context of integrated navigation systems. Figure 12 and Figure 13 clearly demonstrate that APF-IKF surpasses traditional methods by delivering enhanced accuracy and robustness against external interferences in the determination of course angles and speeds. The velocity errors analyzed with the APF-IKF algorithm were roughly between −0.2 km/h and 0.2 km/h and were all close to the zero line. The KF algorithm was affected by the cumulative error of the IMU, and the maximum value was 0.6° km/h. The APF-IKF algorithm exhibited superior estimation and analytical capabilities for velocity and heading angle errors compared with the conventional Kalman filter (KF) approach. The maximum error of vehicle heading angle determined by the KF algorithm was four times that of the APF-IKF algorithm. Notably, as time progressed, the error trend associated with APF-IKF remained closely clustered around zero, indicating a high degree of stability and consistency in its performance.

Figure 14 and Figure 15 present the heading angle data and the eastward and northward velocity, respectively, processed by the APF-IKF algorithm. During the real-vehicle test, the vehicle navigated in a counterclockwise direction to complete a full loop. Observations from Figure 14 indicate that the vehicle’s heading angle remained relatively constant for the initial 10 s, signifying straight-line travel. Between the 32nd and 40th seconds, there was a gradual increase in the vehicle’s heading angle to 360 degrees, followed by an abrupt shift to 0 degrees. This transition corresponds to the vehicle maneuvering from the northerly to the southerly direction, then the westerly direction and finally the easterly direction. The vehicle completed a circuit and returned to the starting point, with the heading angle at the end aligning closely with that at the outset. The heading angle is a pivotal parameter in trajectory tracking control in autonomous vehicles. Figure 15 depicts the calculations of the vehicle’s lateral and longitudinal speeds based on the APF-IKF algorithm. Within the first 10 s, both the eastward and northward velocities were negative, indicating southwestward motion. These velocity metrics are critical to characterizing the operational stability of an autonomous vehicle, which is vital to studying decision making and dynamic stability in intelligent vehicle systems.

To further validate the system’s performance, we conducted a series of experiments using actual vehicles, during which we arranged a cluster of GPS units to intentionally lose signal at a predetermined time.

Figure 16 illustrates the comparison between the latitude and longitude estimated by the proposed algorithm and the actual coordinates during sudden GPS interruption. Figure 17 presents a comparison between the plane coordinate curve generated by the proposed algorithm and the target coordinate curve under sudden GPS interruption. The figures reveal that at approximately 36.4509 degrees latitude, a disruption in GPS signal occurs, leading to significant deviations in the output latitude and longitude values from their actual counterparts. Additionally, Figure 17 indicates an abrupt failure of GPS at a northward distance of 140 m, corresponding to Figure 16. Subsequently, a consistent error of about 20 cm is maintained by the system. Prior to signal interruption, the system error remains below 10 cm.

5. Conclusions

Traditional filtering approaches fall short in adequately addressing the intricacies of integrated GPSs/INSs characterized by uncertain dynamic models. The incorporation of auxiliary particle filtering offers a significant advantage thanks to its dual weighting mechanism, which demonstrates superiority in managing non-Gaussian noise and consequently leads to more precise estimation outcomes.

In this study, we developed a novel approach that integrates a particle filter with a Kalman filter to bolster the system’s resilience against non-Gaussian noise. A dedicated real-vehicle experimental setup was established to rigorously validate the proposed algorithm. By leveraging differential GPS data, a comparative analysis was conducted to assess the robustness of the GPS/INS navigation system under GPS signal disruption. A comparison was made between the APF-IKF and the conventional Kalman filter (KF) algorithms. The results underscore a substantial enhancement in the system’s anti-interference capabilities without compromising on accuracy.

Moreover, the benefits of the APF-IKF-based integrated navigation system were further substantiated by a meticulous comparison against other systems in terms of longitudinal, latitudinal and vehicular speed parameters along the driving route.

In the process of path-tracking control in autonomous vehicles, the most important parameters applied are the northward and eastward speed, the position data and the vehicle heading angle. These data are precisely output by the APF-IKF integrated navigation system.