Dynamic Adaptive Low Power Adjustment Scheme for Single-Frequency GNSS/MEMS-IMU/Odometer Integrated Navigation in the Complex Urban Environment

Abstract

Positioning accuracy and power consumption are essential performance indicators of integrated navigation and positioning chips. This paper proposes a single-frequency GNSS/MEMS-IMU/odometer real-time high-precision integrated navigation algorithm with dynamic power adaptive adjustment capability in complex environments. It is implemented in a multi-sensor fusion navigation SiP (system in package) chip. The simplified INS algorithm and the simplified Kalman filter algorithm are adopted to reduce the computation load, and the strategy of adaptively adjusting the data rate and selecting the observation information for measurement update in different scenes and motion modes is combined to realize high-precision positioning and low power consumption in complex scenes. The performance of the algorithm is verified by real-time vehicle experiments in a variety of complex urban environments. The results show that the RMS statistical value of the overall positioning error in the entire road section is 0.312 m, and the overall average power consumption is 141 mW, which meets the requirements of real-time integrated navigation for high-precision positioning and low power consumption. It supports single-frequency GNSS/MEMS-IMU/odometer integrated navigation SiP chip in real-time, high-precision, low-power, and small-volume applications.

1. Introduction

The global navigation satellite system has the advantages of globalization, all-weather, and high precision, which are widely used in land vehicle navigation systems [1]. However, environmental conditions may cause GNSS signal loss or attenuation and degrade the navigation accuracy [2]. The inertial navigation system does not depend on the external environment and can autonomously provide the position, speed, and attitude information of moving objects. It has good dynamic performance and high navigation accuracy in a short time. However, due to errors such as bias, drift, and noise of the inertial measurement unit (IMU), the navigation errors will accumulate over time [3,4,5]. GNSS and INS have strong complementarity with each other in many aspects such as error characteristics. Combining GNSS and INS can provide more accurate, continuous, and reliable navigation information (including position, velocity, and attitude) than individual GNSS or INS [6,7].

With the rapid development of micro-electro-mechanical systems (MEMS) technology, the performance of MEMS IMU has been dramatically improved [8]. However, the navigation accuracy of MEMS IMU is still far behind that of laser gyro and fiber optic gyro IMU. Still, it has the advantages of a small size, a light weight, and low power consumption [9]. Furthermore, after combining the GNSS with real-time kinematics (RTK) [10], the GNSS/MEMS-INS integrated navigation system can still have sub-meter navigation accuracy when the GNSS signal outages in a short time. Therefore, it is very suitable for the application of vehicle navigation and wearable equipment, such as crewless micro aerial vehicles [11], land vehicle navigation (LVN) [12,13,14,15], mobile mapping systems [16], wearable sports equipment [17,18], and pedestrians [19,20]. In all these applications, an important common problem is the positioning accuracy and the power consumption performance of GNSS/MEMS-INS during the period of GNSS signal or not, which often occurs in different situations, such as boulevard, tunnels, overhead bridges, and urban streets.

Various methods and improvement schemes are proposed to reduce the power consumption of integrated navigation systems. These methods reduce the power consumption of the system by reducing the computation load.

The improvements in the inertial navigation algorithm are as follows, and they have achieved good results. Zhang et al. reduced the amount of calculation by omitting minor terms on the positioning results (such as Coriolis acceleration, rotation correction, paddle correction, and cone correction) to simplify the INS algorithm [21]. Shin proposed an improved INS mechanization equation; in this equation, the error dynamics equations were derived based on perturbation analysis. The second-order Runge–Kutta method is used to integrate the position, and the quaternion method is used for attitude integration [22]. Jordan and Bortz proposed a simplified rotation vector differential equation for incremental attitude update; the velocity algorithm takes the first-order approximation of the body attitude in the transformed specific force integration [23,24]. Finally, Wu et al. put forward a fast approach to significantly reduce RodFIter’s computation complexity while maintaining almost the same accuracy of attitude reconstruction [25]. It reformulates the original RodFIter in terms of the iterative computation of the Rodrigues vector’s Chebyshev polynomial coefficients and exerts Chebyshev polynomial truncation.

The simplification of Kalman filtering mainly focuses on the following aspects. Since the arithmetic calculations required for continuous scalar measurement are significantly smaller than the corresponding operands of vector measurement processing, Ushaq et al. proposed using a centralized linear Kalman filtering method, which treats vector measurements with uncorrelated errors as scalars [26]. Due to the low speed of vehicles, the nonlinear dynamic equation of the velocity can be simplified to linear form. Mehdi et al. simplified the traditional Kalman filter and avoided matrix multiplication and matrix inversion [27]. As we all know, the state space model of GNSS/INS has some special characteristics of sparse system matrix and symmetric state covariance matrix. Zhu et al. proposed a new rapid computation method for Kalman filtering. The prediction of the state covariance matrix is expanded directly rather than computing by a generic matrix function [28]. Hu et al. presented a practical optimization algorithm with offline derivation and parallel processing methods using the sparseness and/or symmetry of matrices and subdivision and reconstruction of operations process [29]. Li et al. proposed an improved GNSS/INS Kalman filtering algorithm, called P-matrix one-step prediction. In his algorithm, the prediction of the P matrix is only once in an update cycle; that is, the prediction frequency is the same as the measurement update, which saves a lot of operations [30].

Although the low power consumption performance of GNSS/MEMS-INS in urban environments has been investigated in some previous studies, they mainly focused on improving the Kalman filter or simplifying the INS algorithm. This paper proposes a power consumption adaptive control method of a GNSS/MEMS-IMU integrated navigation system in the urban environment. The odometer is integrated to ensure the system’s positioning accuracy in complex scenes. In this research, scene recognition and motion pattern recognition algorithms are also used to control the simplified algorithm and IMU data rate, so that different operating modes can be adopted in different scenarios and motion modes. According to the operating mode, the number of algorithm operations is reduced, and then the clock frequency of the microcontroller unit (MCU) is decreased to reduce chip power consumption. This research will focus on how to simplify the algorithm to reduce the number of operations.

$$P_{total}=P_{MCU}+P_{GNSS}+P_{IMU}+P_{others}$$

(1)

$$P_{MCU}=P_{clockfrequency}+P_{peripherals}+P_{IOport}+P_{others}$$

(2)

From Equation (1), the power consumption of the single-frequency GNSS/MEMS-IMU/odometer integrated navigation SiP chip mainly includes MCU power consumption, GNSS power consumption, IMU power consumption, and other sensor power consumption. Since GNSS power consumption, IMU power consumption, and other sensor power consumption are all fixed values, they cannot be changed. So, only MCU can be used to reduce power consumption. However, as shown from Equation (2), MCU power consumption is mainly composed of clock frequency, internal peripherals, and IO ports. The peripherals used inside the MCU include SPI, serial, encoder, timer, etc. These peripherals are connected to external sensors and cannot be reduced. Therefore, the system power consumption can only be reduced by lowering the clock frequency of the MCU. In this paper, the computational complexity is reduced by decreasing the algorithm complexity, which in turn can reduce the clock frequency of the MCU. The algorithm is verified on a GNSS/MEMS-IMU/odometer small-volume embedded SiP chip, and a real-time vehicle experiment is carried out in Wuhan, China.

The paper is organized as follows: Section 2 presents the system overview and improved algorithm of a single-frequency GNSS/MEMS-IMU/odometer integrated navigation. Next, the computation load of the simplified INS algorithm and simplified Kalman filter algorithm are analyzed in Section 3. Then, the experimental scene and hardware platform are described in Section 4. Next, the corresponding results and discussion are presented in Section 5 and Section 6, respectively. Finally, a summary of the work and conclusions are given in Section 7.

2. Methods

2.1. System Overview

In this paper, the single-frequency RTK GNSS/MEMS IMU/wheel odometer are combined to form a loosely integrated navigation system. It uses a simplified algorithm, low INS update frequency, and changing operation mode to reduce the power consumption without losing the positioning accuracy in the complex urban environment. The flow chart of the adaptive low power consumption algorithm is shown in Figure 1. In addition, the algorithm incorporates scene recognition and motion pattern recognition. Scene recognition is based on the state of the GNSS solution to determine whether the car enters the complex scene. It can be divided into the following five cases: fixed solution, floating solution, differential solution, single-point positioning, and no solution. Through many experiments, the fixed solution or float solution of RTK is used as GNSS observation information, and other cases are treated as no GNSS signal. Motion pattern recognition is based on the speed increment information output by the odometer to determine whether the car has stopped. According to the recognition results, the system can be divided into three working modes: (1) zero speed state; (2) non-zero speed state with GNSS RTK fixed/float solution; (3) non-zero speed state without GNSS RTK fixed/float solution.

In the zero-speed state, since the car is stationary, the current position does not change. At this time, whether there is GNSS positioning information or not, the positioning result before the vehicle is stationary will be regarded as the current vehicle position. Meanwhile, the INS update frequency is reduced to 10 Hz, and a simplified mechanization algorithm is adopted. Then, the zero-speed information is taken as the observation to correct the velocity divergence error by the Kalman filter algorithm. Since Kalman filtering mainly includes prediction and update, the P-matrix prediction takes up most of the calculation of a Kalman filtering cycle. Therefore, a one-step P matrix prediction is proposed; that is, the P matrix prediction is performed during the zero velocity updates (ZUPT) process.

In the non-zero speed state with RTK fixed/float solution, GNSS positioning accuracy can be divided into two situations: centimeter-level for fixed solutions and meter-level for float solutions. When the GNSS is fixed, because the MEMS INS has a small divergence error within 1 s, the update frequency of the INS can be reduced to 10 Hz and a simplified mechanization algorithm can be used to reduce the amount of calculation. However, in the RTK float solutions, in order not to reduce the positioning accuracy, an unsimplified INS mechanization algorithm with a data rate of 100 Hz is used, and the odometer information and non-holonomic constraint (NHC) information are combined. Furthermore, one-step P-matrix prediction is also used in this mode.

Finally, when there is no RTK fixed/float solution in a non-zero speed state, the update frequency of INS is increased to 100 Hz. The unsimplified mechanization algorithm is adopted to ensure the positioning accuracy of the system. In addition, the odometer speed information and NHC information are used as observations to assist INS, so that the integrated navigation system maintains sub-meter accuracy even when the GNSS signal is blocked for some time.

The simplified INS algorithm and GNSS RTK fixed solution are fused when GNSS has a fixed solution. The positioning accuracy of the GNSS fixed solution can make up for the positioning error caused by the deficiency of the simplified INS algorithm. This ensures the positioning accuracy and further reduces the amount of calculation. When GNSS has no fixed solution, the odometer and NHC are used to assist the INS with the unsimplified algorithm to ensure the positioning accuracy after the combination of INS and meter-level positioning accuracy GNSS. This method of adjusting the INS algorithm with GNSS not only reduces the amount of calculation to achieve low power consumption, but also ensures the accuracy of integrated navigation and positioning.

2.2. Improvement Algorithm

The INS mechanization algorithm and Kalman filtering algorithm have been widely used in integrated navigation systems. In this research, a combination of scene recognition, motion pattern recognition, a simplified INS mechanization algorithm, and an improved Kalman filter algorithm are used to reduce the power consumption of the integrated navigation system. To achieve the goal of adaptively controlling power consumption without significantly reducing the system’s positioning accuracy, a low computational load INS dynamic model and a simplified Kalman filter algorithm are introduced into the GNSS/MEMS-IMU/odometer integrated navigation algorithm.

2.2.1. INS Dynamic Model

Inertial sensors are essential components for positioning, attitude measurement, and orientation. Its accuracy mainly depends on the performance of gyroscopes and accelerometers [31]. To reduce the system’s power consumption without significantly decreasing the positioning accuracy, a combination of unsimplified and simplified algorithms are used in the MEMS-INS algorithm. When the GNSS signal does not have a fixed solution, the 100 Hz INS mechanization update rate and an unsimplified MEMS-INS algorithm are used to slow down the inertial navigation divergence. When the GNSS signal has a fixed solution, the 10 Hz INS mechanization update rate and a simplified INS algorithm reduce the system’s power consumption.

The unsimplified INS algorithm block diagram is shown in Figure 2, and the dynamic equation may be expressed as [22,32].

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

(3)

$${\dot{\mathit{C}}}_n^e={\mathit{C}}_n^e({\mathit{\omega }}_{en}^n\times )$$

(4)

$$\dot{h}=-v_D$$

(5)

$${\dot{\mathit{C}}}_b^n={\mathit{C}}_b^n({\mathit{\omega }}_{ib}^b\times )-({\mathit{\omega }}_{in}^n\times ){\mathit{C}}_b^n$$

(6)

where ${\mathit{C}}_b^n$ is the rotation matrix from the body frame (b-frame, Forward-Right-Down) to the navigation frame (n-frame, North-East-Down); ${\mathit{f}}^b$ is the specific force in the b-frame; ${\mathit{\omega }}_{ie}^n$ is the angular rate of the earth frame (e-frame) relative to the inertial frame (i-frame) in the n-frame; ${\mathit{\omega }}_{en}^n$ is the angular rate of the n-frame relative to the e-frame in the n-frame; ${\mathit{v}}^n$ is the velocity in the n-frame; ${\mathit{g}}_l^n$ is the normal gravity in the local position in the n-frame; ${\mathit{C}}_n^e$ is the rotation matrix from the n-frame to the e-frame, which represent the geodetic latitude and longitude; $h$ and $v_D$ are the ellipsoid height and velocity in the down direction, respectively; ${\mathit{\omega }}_{ib}^b$ is the angular rate of the b-frame relative to the i-frame in the b-frame; ${\mathit{\omega }}_{in}^n$ is the angular rate of the n-frame relative to the i-frame in the n-frame; $\left(\times \right)$ is skew symmetric matrix.

According to Equation (3), the unsimplified INS speed update can be written as follows:

$${\mathit{v}}_k^n=v_{k-1}^n+\Delta v_{f,k}^n+\Delta v_{g/cor,k}^n$$

(7)

where $\Delta v_{g/cor,k}^n$ is the velocity increment due to the gravity and Coriolis force and $\Delta v_{f,k}^n$ is the velocity increment due to the specific force, which can be written as follows [33]:

$$\Delta v_{g/cor,k}^n=\left[g_{l,k-1/2}^n-(2{\omega }_{ie,k-1/2}^n+{\omega }_{en,k-1/2}^n)\times v_{k-1/2}^n\right]\Delta t_k$$

(8)

$$\Delta v_{f,k}^n=\left[I-[0.5\left({\omega }_{ie,k-1/2}^n+{\omega }_{en,k-1/2}^n\right)\Delta t_k\times ]\right]C_{b(k-1)}^{n(k-1)}\Delta v_{f,k}^{b(k-1)}$$

(9)

$$\Delta v_{f,k}^{b(k-1)}\approx \Delta v_{f,k}^b+\frac{1}{2}\Delta {\theta }_k\times \Delta v_{f,k}^b+\frac{1}{12}\left(\Delta {\theta }_{k-1}\times \Delta v_{f,k}^b+\Delta v_{f,k-1}^b\times \Delta {\theta }_k\right)$$

(10)

where $I$ is a 3-dimensional identity matrix. Equation (10), $\frac{1}{2}\Delta {\theta }_k\times \Delta v_{f,k}^b$ represents the rotational compensation term and $\frac{1}{12}\left(\Delta {\theta }_{k-1}\times \Delta v_{f,k}^b+\Delta v_{f,k-1}^b\times \Delta {\theta }_k\right)$ defines sculling compensation term, $\Delta {\theta }_k$ is the angular increment at a time $t_k$.

The rotation quaternion from the n-frame to the e-frame contains latitude and longitude information. Thus, the position update is solved by quaternion multiplication.

$${\mathit{q}}_{n(k)}^{e(k)}={\mathit{q}}_{e(k-1)}^{e(k)}\ast {\mathit{q}}_{n(k-1)}^{e(k-1)}\ast {\mathit{q}}_{n(k)}^{n(k-1)}$$

(11)

where ${\mathit{q}}_{n(k)}^{n(k-1)}$ and ${\mathit{q}}_{e(k-1)}^{e(k)}$ are the quaternion for the n-frame and e-frame, respectively. The asterisk in the formula represents the multiplication symbol.

The rotation vector is used for attitude update, and the attitude quaternion update algorithm can be described as

$${\mathit{q}}_{b(k)}^{n(k)}={\mathit{q}}_{n(k-1)}^{n(k)}\ast {\mathit{q}}_{b(k-1)}^{n(k-1)}\ast {\mathit{q}}_{b(k)}^{b(k-1)}$$

(12)

where ${\mathit{q}}_{b(k)}^{b(k-1)}$ is the quaternion for the b-frame.

In MEMS inertial sensors, gyro bias and acceleration bias are the primary sources of errors, which affect the positioning accuracy. To reduce the computational load and thus the power consumption, the MEMS-INS mechanization algorithm is simplified. The error sources that have little influence on the positioning are ignored, such as coriolis acceleration, rotation correction, rotation sculling correction, and coning correction. The simplified INS algorithm block diagram is shown in Figure 3. The dynamic equation is defined as

$${\dot{\mathit{v}}}^n={\mathit{C}}_b^n{\mathit{f}}^b+{\mathit{g}}_l^n$$

(13)

$${\dot{\mathit{r}}}^n=D^{-1}{\mathit{v}}^n$$

(14)

$${\dot{\mathit{q}}}_b^n=0.5W{q}_b^n$$

(15)

where $f^b$ is the specific force in the b-frame, $g_l^n$ is the normal gravity in the local position in the n-frame, $D^{-1}$ is the transition matrix, and $\mathit{W}$ is the carrier attitude angular rate. ${\mathit{D}}^{-1}$ and $\mathit{W}$ can be written as follows:

$${\mathit{D}}^{-1}=\left[\begin{array}{ccc}\frac{1}{R_M+h}& 0& 0\\ 0& \frac{1}{\left(R_N+h\right)\cos \phi }& 0\\ 0& 0& -1\end{array}\right]$$

(16)

$$\mathit{W}=\left[\begin{array}{cccc}0& -{\omega }_{nb,x}^b& -{\omega }_{nb,y}^b& -{\omega }_{nb,z}^b\\ {\omega }_{nb,x}^b& 0& {\omega }_{nb,z}^b& -{\omega }_{nb,y}^b\\ {\omega }_{nb,y}^b& -{\omega }_{nb,z}^b& 0& {\omega }_{nb,x}^b\\ {\omega }_{nb,z}^b& {\omega }_{nb,y}^b& -{\omega }_{nb,x}^b& 0\end{array}\right]$$

(17)

where $R_M$ and $R_N$ represent the radius of curvature in the meridian and the prime vertical radius of curvature, respectively; $\phi$ is latitude; and ${\mathit{\omega }}_{nb}^b={\mathit{\omega }}_{ib}^b-{\mathit{C}}_n^b({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)$ is the angular rate of the b-frame relative to the n-frame in the b-frame.

Formula (15) is the differential equation of Formula (28). For simplification purposes, angular rate can be assumed as a constant in a short sampling interval $\Delta t$; that is, a one-sample algorithm of rotation vector is applied. Since the quaternion algorithm is actually a one-sample algorithm, we can obtain the analytic discrete solution of attitude quaternion given by Formula (15).

$${\mathit{q}}_b^n(t_{k+1})=\exp \left\{\frac{1}{2}{\displaystyle {\int }_{t_k}^{t_{k+1}}\mathit{W}dt}\right\}{\mathit{q}}_b^n(t_k)$$

(18)

$$\mathit{S}={\displaystyle {\int }_{t_k}^{t_{k+1}}\mathit{W}dt}={\displaystyle {\int }_{t_k}^{t_{k+1}}\left[\begin{array}{cccc}0& -{\omega }_{nb,x}^b& -{\omega }_{nb,y}^b& -{\omega }_{nb,z}^b\\ {\omega }_{nb,x}^b& 0& {\omega }_{nb,z}^b& -{\omega }_{nb,y}^b\\ {\omega }_{nb,y}^b& -{\omega }_{nb,z}^b& 0& {\omega }_{nb,x}^b\\ {\omega }_{nb,z}^b& {\omega }_{nb,y}^b& -{\omega }_{nb,x}^b& 0\end{array}\right]}dt=\left[\begin{array}{cccc}0& -\Delta {\theta }_x& -\Delta {\theta }_y& -\Delta {\theta }_z\\ \Delta {\theta }_x& 0& \Delta {\theta }_z& -\Delta {\theta }_y\\ \Delta {\theta }_y& -\Delta {\theta }_z& 0& \Delta {\theta }_x\\ \Delta {\theta }_z& \Delta {\theta }_y& -\Delta {\theta }_x& 0\end{array}\right]$$

(19)

$$\exp \left\{\frac{1}{2}{\displaystyle {\int }_{t_k}^{t_{k+1}}\mathit{W}dt}\right\}=e^{\frac{\mathit{S}}{2}}={\mathit{I}}_4\cos \frac{\Delta \theta }{2}+\frac{\mathit{S}}{\Delta \theta }\sin \frac{\Delta \theta }{2}$$

(20)

where ${\mathit{I}}_4$ is a 4-dimensional identity matrix. Substitute Formula (20) into Formula (18),

$${\mathit{q}}_b^n(t_k)=({\mathit{I}}_4\cos \frac{\Delta \theta }{2}+\frac{\mathit{S}}{\Delta \theta }\sin \frac{\Delta \theta }{2}){\mathit{q}}_b^n(t_{k-1})$$

(21)

Sort out the above formula as follows:

$${\mathit{q}}_b^n(t_k)={\mathit{q}}_b^n(t_{k-1})+\left[(\cos \frac{\theta }{2}-1){\mathit{I}}_4+\frac{1}{\theta }\sin \frac{\theta }{2}\mathit{S}\right]{\mathit{q}}_b^n(t_{k-1})$$

(22)

where $\theta =\left|\Delta \mathit{\theta }\right|$; $\mathit{S}=\mathit{W}\Delta t$. Introduce variables

$$c=2(\cos \frac{\theta }{2}-1)=-\frac{{\theta }^2}{4}+\frac{{\theta }^4}{192}+\cdots$$

(23)

$$s=\frac{2}{\theta }\sin \frac{\theta }{2}=1-\frac{{\theta }^2}{24}+\frac{{\theta }^4}{1920}+\cdots$$

(24)

Here, taking $c=-\frac{{\theta }^2}{4}$, $s=1$, substitute them into (22) to obtain the simplified attitude update equation,

$${\mathit{q}}_k={\mathit{q}}_{k-1}+0.5\left(-\frac{{\theta }^2}{4}{\mathit{I}}_4+\mathit{S}\right){\mathit{q}}_{k-1}$$

(25)

Considering the impact caused by the second and third terms on the right-hand side of Equation (10), navigation accuracy may be minimal. Meanwhile, the quaternion algorithm is a one-sample algorithm, which can be used to obtain the analytic discrete solution of attitude quaternion given in Equation (15). Therefore, the simplified INS update algorithm is given by

$${\mathit{v}}_k^n={\mathit{v}}_{k-1}^n+{\mathit{C}}_{b\left(k-1\right)}^{n\left(k-1\right)}\Delta {\mathit{v}}_{f,k}^b+{\mathit{g}}_l^n\Delta t_k$$

(26)

$${\mathit{r}}_k^n={\mathit{r}}_{k-1}^n+D^{-1}\frac{\left({\mathit{v}}_k^n+{\mathit{v}}_{k-1}^n\right)}{2}\Delta t_k$$

(27)

$${\mathit{q}}_k={\mathit{q}}_{k-1}+0.5\left(-\frac{{\theta }^2}{4}{\mathit{I}}_4+\mathit{S}\right){\mathit{q}}_{k-1}$$

(28)

where $\Delta \mathit{\theta }=\Delta {\mathit{\theta }}_{ib}^b-{\mathit{C}}_n^b({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)\Delta t$ is the compensated angular increment.

2.2.2. Simplification of Kalman Filter and Model

In a standard Kalman filter, the prediction state covariance matrix (P matrix) calculation time accounts for about 70% of the Kalman filtering process [30]. To decrease the computation load, a simplified Kalman filter algorithm in the integrated navigation is proposed. This section introduces the simplified Kalman filter algorithm model, error state model, GNSS measurement model, odometer measurement model, and NHC measurement model.

• Simplification Kalman filter model

In traditional integrated navigation Kalman filters, P matrix prediction and error state updates are performed together, significantly increasing the computation load, which is not suitable for systems with real-time and low power consumption requirements. In our research, the Kalman filter algorithm is improved, and the P matrix prediction is performed together with the measurement update, which dramatically reduces the computation load. The improved algorithm is as follows:

Prediction:

$$x_{k,k-1}={\Phi }_{k,k-1}{x}_k$$

(29)

Update:

$$P_{k,k-1}={\Phi }_{k,k-1}{P}_k{\Phi }_{k,k-1}^T+Q_{k-1}$$

(30)

$$K_k=P_{k,k-1}{H}_k^{\mathrm{T}}{\left(H_k{P}_{k,k-1}{H}_k^{\mathrm{T}}+R_k\right)}^{-1}$$

(31)

$$x_k=x_{k,k-1}+K_k\left(z_k-H_k{x}_{k,k-1}\right)$$

(32)

$$P_k=\left(I-K_k{H}_k\right)P_{k,k-1}{\left(I-K_k{H}_k\right)}^{\mathrm{T}}+K_k{R}_k{K}_k^{\mathrm{T}}$$

(33)

where $x$ is the error state vector, which can be omitted since it is reset to zero after each feedback and has no predictable meaning; $\Phi$ represents the state transition matrix; $P$ is the error state covariance matrix; $Q$ is the system noise covariance matrix; $K$ is the Kalman filter gain matrix which determines the weights of measurement information when error state is updated; $H$ represents the design matrix; $R$ stands for the covariance matrix of measurement noise; $z$ represents the measurement vector. All the subscripts indicate the state change. For example, $x_{k,k-1}$ means the error state vector from epoch $k-1$ to epoch $k$.

The state transition matrix $\Phi$ can be written as follows:

$$\Phi =\left[\begin{array}{ccccc}{\mathit{I}}_3+(-{\mathit{\omega }}_{en}^n\times )\Delta t& {\mathit{I}}_3& 0& 0& 0\\ F_{21}& {\mathit{I}}_3+[(-2{\mathit{\omega }}_{ie}^n-{\mathit{\omega }}_{en}^n)\times ]\Delta t& ({\mathit{f}}^n\times )\Delta t& 0& {\mathit{C}}_b^n\Delta t\\ 0& 0& {\mathit{I}}_3+(-{\mathit{\omega }}_{in}^n\times )\Delta t& -{\mathit{C}}_b^n\Delta t& 0\\ 0& 0& 0& -1/t_{gb}& 0\\ 0& 0& 0& 0& -1/t_{ab}\end{array}\right]$$

(34)

where $F_{21}=\left[\begin{array}{ccc}\frac{g_l^n}{\sqrt{R_M{R}_N}+h}\Delta t& 0& 0\\ 0& \frac{g_l^n}{\sqrt{R_M{R}_N}+h}\Delta t& 0\\ 0& 0& \frac{-2g_l^n}{\sqrt{R_M{R}_N}+h}\Delta t\end{array}\right]$, ${\mathit{I}}_3$ is a 3-dimensional identity matrix, ${\mathit{f}}^n$ is the specific force in the n-frame; $t_{gb}$ is the gyro zero-bias correlation time, $t_{ab}$ is the accelerometer zero bias correlation time.

In Equation (30), the update accuracy of the P matrix mainly depends on the state transition matrix $\Phi$. Taking the P matrix prediction of three adjacent epochs $k$, $k+1$ and $k+2$ in the traditional Kalman filter as an example to derive the update of the P matrix in the improved Kalman filter, the process is as follows:

Epoch $k$:

$$Q_k=\frac{1}{2}({\Phi }_{k+1,k}{\overline{Q}}_k+{\overline{Q}}_k{\Phi }_{k+1,k}^{\mathrm{T}})\Delta t$$

(35)

$$P_{k+1,k}={\Phi }_{k+1,k}{P}_k{\Phi }_{k+1,k}^{\mathrm{T}}+Q_k$$

(36)

Epoch $k+1$:

$$Q_{k+1}=\frac{1}{2}({\Phi }_{k+2,k+1}{\overline{Q}}_{k+1}+{\overline{Q}}_{k+1}{\Phi }_{k+2,k+1}^{\mathrm{T}})\Delta t$$

(37)

$$\begin{array}{ll}{P}_{k+2,k+1}& ={\Phi }_{k+2,k+1}{P}_{k+1}{\Phi }_{k+2,k+1}^{\mathrm{T}}+Q_{k+1}\\ & =({\Phi }_{k+2,k+1}{\Phi }_{k+1,k})P_k{({\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}+\\ & Q_{k+1}+{\Phi }_{k+2,k+1}{Q}_k{\Phi }_{k+2,k+1}^{\mathrm{T}}\end{array}$$

(38)

Epoch $k+2$:

$$Q_{k+2}=\frac{1}{2}({\Phi }_{k+3,k+2}{\overline{Q}}_{k+2}+{\overline{Q}}_{k+2}{\Phi }_{k+3,k+2}^{\mathrm{T}})\Delta t$$

(39)

$$\begin{array}{ll}{P}_{k+3,k+2}& ={\Phi }_{k+3,k+2}{P}_{k+2}{\Phi }_{k+3,k+2}^{\mathrm{T}}+Q_{k+2}\\ & =({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})P_k{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}+\\ & Q_{k+2}+{\Phi }_{k+3,k+2}{Q}_{k+1}{\Phi }_{k+3,k+2}^{\mathrm{T}}+({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})Q_k{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})}^{\mathrm{T}}\end{array}$$

(40)

where $Q$ is the constant spectral density matrix; ${\overline{Q}}_k\approx {\overline{Q}}_{k+1}\approx {\overline{Q}}_{k+2}$ is the system noise covariance matrix, $\Delta t$ is the sampling interval of the navigation system.

Compared with (36), (38), and (40), it is found that the structures of the three formulas are similar, and the matrix plays a crucial role in each epoch of the P matrix. The second term on the right side of Equation (40) is the current constant spectral density matrix, which is easy to calculate. It is mainly because the latter two terms on the right side of the equal sign are challenging to handle. Therefore, these two terms are expanded and named $s_1$ and $s_2$ as follows:

$$\begin{array}{ll}{s}_1& ={\Phi }_{k+3,k+2}{Q}_{k+1}{\Phi }_{k+3,k+2}^{\mathrm{T}}\\ & =\frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})({\overline{Q}}_{k+1}{\Phi }_{k+3,k+2}^{\mathrm{T}})+({\Phi }_{k+3,k+2}{\overline{Q}}_{k+1}){({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})}^{\mathrm{T}}]\Delta t\end{array}$$

(41)

$$\begin{array}{ll}{s}_2& =({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})Q_k{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})}^{\mathrm{T}}\\ & =\frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k}){\overline{Q}}_k{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})}^{\mathrm{T}}+\\ & ({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}){\overline{Q}}_k{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}]\Delta t\end{array}$$

(42)

Since the first term of (40) is to multiply the state transition matrix of each epoch, the calculation form is simple, and the operations amount is small. Therefore, for ease of calculation, the last two terms in (40) are constructed into the form of the first term, that is, $s_1$ and $s_2$ are approximate, as shown in $s_3$ and $s_4$ below:

$$s_3=\frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k}){\overline{Q}}_{k+1}+{\overline{Q}}_{k+1}{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}]\Delta t$$

(43)

$$s_4=\frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k}){\overline{Q}}_{k+1}+{\overline{Q}}_{k+1}{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}]\Delta t$$

(44)

Since $\Delta t$ is very small, the state transition matrix in the above formula can be written as follows:

$${\Phi }_{k+3,k+2}\approx I+F_3\Delta t$$

(45)

$${\Phi }_{k+2,k+1}\approx I+F_2\Delta t$$

(46)

$${\Phi }_{k+1,k}\approx I+F_1\Delta t$$

(47)

Substitute (45), (46), (47) into $s_3-s_1$ and $s_4-s_2$ to perform high-order small quantity analysis, as shown below:

$$\begin{array}{ll}{s}_4-s_2& =\frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k}){\overline{Q}}_{k+1}+{\overline{Q}}_{k+1}{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}]\Delta t\\ & -\frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k}){\overline{Q}}_k{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})}^{\mathrm{T}}+({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}){\overline{Q}}_k{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}]\Delta t\\ & =-\frac{1}{2}(\Delta t^2+F_1\Delta t^3+F_2\Delta t^3+F_3\Delta t^3+F_2{F}_1\Delta t^4+F_3{F}_1\Delta t^4+F_3{F}_2\Delta t^4+F_3{F}_2{F}_1\Delta t^5){\overline{Q}}_k{(F_2+F_3+F_3{F}_2\Delta t)}^{\mathrm{T}}\\ & -\frac{1}{2}(F_2+F_3+F_3{F}_2\Delta t){\overline{Q}}_k{(\Delta t^2+F_1\Delta t^3+F_2\Delta t^3+F_3\Delta t^3+F_2{F}_1\Delta t^4+F_3{F}_1\Delta t^4+F_3{F}_2\Delta t^4+F_3{F}_2{F}_1\Delta t^5)}^{\mathrm{T}}\end{array}$$

(48)

$$\begin{array}{ll}{s}_3-s_1& =\frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k}){\overline{Q}}_{k+1}+{\overline{Q}}_{k+1}{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}]\Delta t\\ & -\frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})({\overline{Q}}_{k+1}{\Phi }_{k+3,k+2}^{\mathrm{T}})+({\Phi }_{k+3,k+2}{\overline{Q}}_{k+1}){({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1})}^{\mathrm{T}}]\Delta t\\ & =\frac{1}{2}(\Delta t^2+F_2\Delta t^3+F_3\Delta t^3+F_2{F}_3\Delta t^4)F_1\overline{Q}-\frac{1}{2}(\Delta t^2+F_2\Delta t^3+F_3\Delta t^3+F_2{F}_3\Delta t^4)F_3^{\mathrm{T}}\overline{Q}\\ & +\frac{1}{2}\overline{Q}{F}_1^{\mathrm{T}}{(\Delta t^2+F_2\Delta t^3+F_3\Delta t^3+F_2{F}_3\Delta t^4)}^{\mathrm{T}}-\frac{1}{2}\overline{Q}{F}_3{(\Delta t^2+F_2\Delta t^3+F_3\Delta t^3+F_2{F}_3\Delta t^4)}^{\mathrm{T}}\end{array}$$

(49)

Observation (48) and (49) shows that all terms in the two equations are high order small quantities of $\Delta t$. Thus, two equations can be approximated as a zero matrix within an acceptable range. Therefore, the P matrix prediction from $k$ epochs to $k+2$ epochs can be described as

$$\begin{array}{ll}{P}_{k+3,k}& =({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})P_k{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}\\ & +\frac{1}{2}({\Phi }_{k+3,k+2}{\overline{Q}}_{k+2}+{\overline{Q}}_{k+2}{\Phi }_{k+3,k+2}^{\mathrm{T}})\Delta t\\ & +2\cdot \frac{1}{2}[({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k}){\overline{Q}}_{k+1}+{\overline{Q}}_{k+1}{({\Phi }_{k+3,k+2}{\Phi }_{k+2,k+1}{\Phi }_{k+1,k})}^{\mathrm{T}}]\Delta t\end{array}$$

(50)

Similarly, the P matrix prediction is extended to n epochs, namely from k epochs to $k+n$ epochs, as follows:

$$\begin{array}{ll}{P}_{k+n,k}& ={\Phi }_{k+n,k}{P}_k{\Phi }_{k+n,k}^{\mathrm{T}}+\frac{1}{2}({\Phi }_{k+n,k+n-1}{\overline{Q}}_{k+n-1}+{\overline{Q}}_{k+n-1}{\Phi }_{k+n,k+n-1}^{\mathrm{T}})\Delta t\\ & +\frac{1}{2}({\Phi }_{k+n,k}{\overline{Q}}_{k+n-1}+{\overline{Q}}_{k+n-1}{\Phi }_{k+n,k}^{\mathrm{T}})(n-1)\Delta t\end{array}$$

(51)

where

$${\Phi }_{k+n,k}={\Phi }_{k+n,k+n-1}{\Phi }_{k+n-1,k+n-2}\cdots {\Phi }_{k+2,k+1}{\Phi }_{k+1,k}$$

(52)

• Error State Model

To improve the navigation accuracy of low-cost MEMS inertial sensors while reducing the computation tasks, the bias error of the gyroscope and accelerometer and the scale factor of the odometer are augmented into the filter state and estimated online [34]. Therefore, the error state vector of Kalman filtering can be written as:

$$\delta x={\left[\begin{array}{cccccc}{\left(\delta r^n\right)}^{\mathrm{T}}& {\left(\delta v^n\right)}^{\mathrm{T}}& {\varphi }^{\mathrm{T}}& b_g{}^{\mathrm{T}}& b_a{}^{\mathrm{T}}& s_{odo}{}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(53)

where $\delta r^n={\left[\begin{array}{ccc}\delta r_N& \delta r_E& \delta r_D\end{array}\right]}^{\mathrm{T}}$ and $\delta v^n={\left[\begin{array}{ccc}\delta v_N& \delta v_E& \delta v_D\end{array}\right]}^{\mathrm{T}}$ are the position and velocity errors in the n-frame, respectively; $\varphi ={\left[\begin{array}{ccc}{\varphi }_{roll}& {\varphi }_{pitch}& {\varphi }_{heading}\end{array}\right]}^{\mathrm{T}}$ is the attitude error; $b_g$ is the gyro bias; $b_a$ is the accelerometer bias; $s_{odo}$ denotes the odometer scale factor. The first five and the last terms of Equation (53) correspond to a three-dimensional vector and one-dimensional vector, respectively.

• GNSS Measurement Model

The position of the phase center of the GNSS antenna is calculated from the INS navigation results and the measured value of the lever arm as follows [35]:

$${\mathit{r}}_{GNSS}^n={\mathit{r}}_{IMU}^n+{\mathit{D}}_R^{-1}{\mathit{C}}_b^n{\mathit{l}}_{GNSS}^b$$

(54)

where ${\mathit{r}}_{IMU}^n$ is the position of the IMU measurement center, and ${\mathit{l}}_{GNSS}^b$ is the lever arm from the IMU measurement center to the GNSS antenna phase center resolved in the b-frame.

For loosely coupled integration, the measurement vector is expressed as the difference between the position estimated by INS and the position solved by GNSS, and the lever arm effect from the INS center to the GNSS antenna is considered. Hence, the measurement equation based on GNSS position can be expressed as

$${\mathit{z}}_{r_{GNSS}}={\mathit{C}}_e^n({\widehat{\mathit{r}}}_{GNSS}^e-{\tilde{\mathit{r}}}_{GNSS}^e)={\mathit{C}}_e^n({\widehat{\mathit{r}}}_{IMU}^e-{\tilde{\mathit{r}}}_{GNSS}^e)+{\mathit{C}}_b^n{\mathit{l}}_{GNSS}^b$$

(55)

where ${\mathit{C}}_e^n$ is the rotation matrix from the earth frame (e-frame) to the n-frame; ${\widehat{\mathit{r}}}_{GNSS}^e$ and ${\tilde{\mathit{r}}}_{GNSS}^e$ are the estimated position of the GNSS antenna phase center and the measurement position of the GNSS receiver in the e-frame, respectively; ${\widehat{\mathit{r}}}_{IMU}^e$ is the estimated position of the IMU measurement center in the e-frame.

• Odometer Measurement Model

The relationship between the vehicle velocity and the IMU velocity can be established by the lever arm, which represents the spatial position relationship between the vehicle and the IMU measurement center, and can be expressed as [36]

$${\widehat{\mathit{v}}}_{odo}^v={\mathit{C}}_b^v{\mathit{C}}_n^b{\mathit{v}}_{IMU}^n+{\mathit{C}}_b^v({\mathit{\omega }}_{nb}^b\times ){\mathit{l}}_{odo}^b$$

(56)

where ${\mathit{C}}_b^v$ is the rotation matrix from b-frame to the vehicle frame (v-frame); ${\mathit{C}}_n^b$ stands for the rotation matrix from the n-frame to the b-frame; ${\mathit{v}}_{IMU}^n$ is the velocity of the IMU measurement center; ${\mathit{l}}_{odo}^b$ is the lever arm from the IMU measurement center to the odometer center, which is resolved in the body frame. The estimated velocity at the odometer center is shown as ${\widehat{\mathit{v}}}_{odo}^v={\left[\begin{array}{ccc}{\widehat{v}}_{odo,x}^v& {\widehat{v}}_{odo,y}^v& {\widehat{v}}_{odo,z}^v\end{array}\right]}^{\mathrm{T}}$.

The odometer output velocity measurement is as follows

$${\tilde{v}}_{odo,y}^v=v_y^v$$

(57)

where ${\tilde{v}}_{odo,y}^v$ is the one-dimensional velocity measurement in the v-frame and $v_y^v$ is the speed output from the odometer. However, the actual velocity measurement can be expressed as [37]

$${\tilde{v}}_{odo,y}^v=v_y^v-e_y^v$$

(58)

where $e_y^v$ is the odometer velocity measurement noise, which is modeled as Gaussian white noise. Thus, the velocity error measurement equation in the v-frame can be expressed as

$$\begin{array}{ll}{\mathit{z}}_{v_{odo}}& ={\widehat{v}}_{odo,y}^v-{\tilde{v}}_y^v\\ & ={\left({\mathit{C}}_b^v{\mathit{C}}_n^b\delta {\mathit{v}}_{IMU}^n-{\mathit{C}}_b^v{\mathit{C}}_n^b({\mathit{v}}_{IMU}^n\times )\varphi -{\mathit{C}}_b^v({\mathit{l}}_{odo}^b\times )\delta {\mathit{\omega }}_{ib}^b\right)}_y+e_y^v\end{array}$$

(59)

where ${\widehat{v}}_{odo,y}^v$ is the forward estimated velocity at the odometer center.

• NHC Measurement Model

For land vehicle applications, vehicles carrying a GNSS/MEMS-IMU integrated navigation system can maintain reliable and continuous rigid contact with the road while driving on the road. Thus, the motion of the land vehicle on the road is governed by two non-holonomic constraints because the vehicle does not jump off the road or slide on the road. Therefore, the lateral and vertical velocity of the vehicle are zero, and the measurement in the vehicle frame can be expressed as [38,39,40,41,42]

$$\begin{gathered} {\tilde{v}}_x^v\approx 0 \\ {\tilde{v}}_z^v\approx 0 \end{gathered}$$

(60)

where ${\tilde{v}}_x^v$ and ${\tilde{v}}_z^v$ represent the velocity components of the vehicle in the plane perpendicular to the forward direction (y-axis).

The NHC velocity error measurement equation in the vehicle frame can be expressed as

$${\mathit{z}}_{v_{odo}}=\left[\begin{array}{c}{\widehat{v}}_{odo,x}^v-{\tilde{v}}_x^v\\ {\widehat{v}}_{odo,z}^v-{\tilde{v}}_z^v\end{array}\right]={\left({\mathit{C}}_b^v{\mathit{C}}_n^b\delta {\mathit{v}}_{IMU}^n-{\mathit{C}}_b^v{\mathit{C}}_n^b({\mathit{v}}_{IMU}^n\times )\varphi -{\mathit{C}}_b^v({\mathit{l}}_{odo}^b\times )\delta {\mathit{\omega }}_{ib}^b\right)}_{x,z}+\left[\begin{array}{c}{e}_x^v\\ e_z^v\end{array}\right]$$

(61)

where ${\widehat{v}}_{odo,x}^v$ and ${\widehat{v}}_{odo,z}^v$ are the lateral and vertical estimated velocity at the odometer center, respectively; $e_x^v$ and $e_z^v$ are the lateral and vertical velocity measurements noise, respectively; the symbol ${\left(\right)}_{x,z}$ represents the first and third rows of the three-dimensional vector.

When the vehicle is stationary, the odometer-derived speed $v_y^v$ is zero, and in this case, a zero-velocity update can be used to update the INS solution [43,44].

3. Analysis of Computation Load

In order to evaluate the efficiency of the algorithm, the operands (1 s period) of addition (A), subtraction (S), multiplication (M), division (D), square roots (SR), and trigonometrics (T) of the INS mechanization and Kalman filter in the integrated navigation algorithm are investigated.

Table 1. Operand number (in 1 s cycle) and improved efficiency between standard and simplified INS algorithms.

	M	A&S	D	SR	T
Standard algorithm (100 Hz)	36,600	32,000	5400	700	2200
Modified algorithm (100 Hz)	10,400 (71.5%)	8800 (72.5%)	2800 (48.2%)	400 (42.9%)	1400 (36.4%)
Modified algorithm (10 Hz)	1040 (97.2%)	880 (97.3%)	280 (94.8%)	40 (94.3%)	140 (93.7%)

,

Table 2. Operand number (in 1 s cycle) and improved efficiency between standard and simplified Kalman filtering algorithms in the ZUPT process.

	M	A&S	D	SR	T
Standard algorithm	241,773	224,489	232	118	17
Modified algorithm	9894 (96%)	7877 (96.5%)	133 (42.6%)	118 (0%)	17 (0%)

,

Table 3. Operand number (in 1 s cycle) and improved efficiency between standard and simplified Kalman filtering algorithms in the GNSS measurement update process.

	M	A&S	D	SR	T
Standard algorithm	215,928	203,851	234	120	31
Modified algorithm	52,728 (75.6%)	39,151 (80.8%)	135 (42.3%)	120 (0%)	31 (0%)

and

Table 4. Operand number (in 1 s cycle) and improved efficiency between standard and simplified Kalman filtering algorithms in the odometer and NHC measurement update process.

	M	A&S	D	SR	T
Standard algorithm	222,353	211,036	232	118	17
Modified algorithm	61,777 (72.1%)	47,782 (77.3%)	133 (42.6%)	118 (0%)	17 (0%)

, respectively, count the number of operations in INS mechanization and the Kalman filter in the process of ZUPT, GNSS measurement update, odometer measurement update, and NHC measurement update.

Table 1 shows the operands required for different INS mechanizations at different data rates. From this table, it is clear that the modified algorithm has a better computational efficiency compared to the standard one, and the number of operations can be reduced by about 72.5% at a 100 Hz data rate. On the other hand, the computation load is also affected by the data rate. The number of operations can be reduced by about 97.3% at a 10 Hz data rate.

Table 2 shows the operands required by the standard and simplified Kalman filter algorithm in the ZUPT process. In the standard algorithm, Kalman prediction and update are 100 Hz and 1 Hz, respectively. However, in the simplified algorithm, the Kalman prediction and update are both 1 Hz. It can be seen from the table that the simplified algorithm has a more significant impact on the number of operations, which can be reduced by about 96.5%.

Table 3 shows the number of operations required for different Kalman filter algorithms in the GNSS measurement update process. Similar to Table 2, the simplified algorithm (prediction and update are both 1 Hz) significantly impacts the computation load, which can be reduced by about 80.8%.

Table 4 shows the number of operations required by different Kalman filter algorithms during odometer and NHC measurement updating. Similarly, after improving the algorithm, the computation load can be reduced by nearly 77.3%.

In summary, the INS mechanization and Kalman filter algorithm modification can reduce the number of operations. In addition, the data rate has a significant impact on the computation load [45]. Therefore, combining the simplified algorithm with reducing the sampling rate can significantly improve computational efficiency.

4. Experimental Scene Introduction and Hardware Platform Description

To evaluate the dynamic adaptive power adjustment strategy and positioning accuracy of single-frequency GNSS/MEMS-IMU/odometer simplified integrated navigation algorithms in complex scenes. The real-time performance verification experiment is carried out in complex scenes, including boulevard, tunnel, overhead bridge, and urban street. The trajectory is shown in Figure 4, and the natural environment of six complex scenes in Figure 4 is shown in Figure 5. It includes boulevard 1, tunnel 1, boulevard 2, tunnel 2, overhead bridge, and urban street. Boulevard 1 has dense trees on both sides and narrow roads. Tunnel 1 and tunnel 2 are V-shaped tunnels which go downhill first and then uphill, and there are turning sections inside the tunnels. Boulevard 2 is near the mountain. On the right side of the road is the hillside. At the same time, there are dense trees on both sides of the road. The overhead bridge scene is that the overhead bridge almost blocks the sky on the left side of the road, and the receiver can track only the GNSS signal on the right side of the sky. City streets are scenes with buildings and street trees on both sides of the road. In these scenes, the satellite signal is easy to be blocked, or there is multipath interference, which affects the positioning accuracy of the receiver.

The vehicle-mounted hardware experimental platform and reference equipment used in this research are shown in Figure 6. The SiP packaging process integrates multiple sensors into one chip to form a multi-sensor fusion navigation SiP chip (including MEMS IMU, a magnetometer, single-frequency GNSS, MCU, NB-IoT, e-sim, and barometer, temperature, and humidity sensors). At the same time, the navigation level equipment NovAtel SPAN CPT-6 (dual-frequency GNSS RTK/fiber IMU tightly coupled system) is taken as a reference to evaluate the positioning accuracy of SiP chip.

Table 5. Performance specifications of the IMU in the experiment.

		MEMS Grade	Navigation Grade
Gyro	Bias (deg/h)	10	0.027
	ARW (deg/sqrt(h))	0.27	0.0667
Accelerometer	Bias (mGal)	1800	50
	VRW (m/s/sqrt(h))	0.042	0.03

shows the performance specifications of the different levels of IMUs in the two devices. The IMU data rate of the two devices is 100 Hz, and the real-time navigation result output frequency is 1 Hz and 100 Hz, respectively. In addition, vehicle speed information is introduced to improve the SiP chip’s positioning accuracy when the GNSS signal is blocked. The odometer is installed on the vehicle’s right rear wheel to measure the vehicle’s speed, and this information is provided to the SiP chip for assisting MEMS IMU navigation.

In the real-time vehicle navigation experiment, to measure the power consumption of SiP chips, a high-precision power meter is used to record and store the chip’s power consumption in real-time but does not include NB-IoT and e-sim.

5. Results

5.1. Simplified INS Algorithms Performance Verification

The position drift of the system after GNSS outages is the most critical indicator to measure the accuracy of INS. To verify the performance of the INS, the INS will work completely independently without the assistance of other external sensors, such as GNSS and odometers. The trajectory of the inertial navigation performance verification experiment is shown in Figure 7. The blue markers in the figure in this trajectory select six sections and simulate GNSS interruption for 60 s in each section. The road section includes bumps, turns, up and down slopes, acceleration, and deceleration.

The RMS statistical results of the position drift error of the unsimplified INS algorithm with a 100 Hz data rate, simplified INS algorithm with a 100 Hz data rate, and simplified INS algorithm with a 10 Hz data rate within 60 s of GNSS outages are shown in Figure 8. The position error of the unsimplified INS algorithm with a 100 Hz data rate are used as reference. The main difference between simplified and unsimplified algorithms is that it omits Coriolis acceleration, rotation correction, sculling correction, and coning correction. It can be seen from the figure that the error of ➁ and ➂ are relatively large. The main reason is that ➁ is a turning scene. According to the reference [21], the prominent position drift error in this section is mainly caused by the omission of the rotation correction. On the other hand, ➂ is primarily due to the higher speed and turning. Therefore, the position drift error in this section is mainly caused by the omission of Coriolis acceleration and rotation correction [21].

Compare the positioning results of INS with NovAtel SPAN CPT-6 to obtain the positioning error, and perform quantitative analysis, as shown in

Table 6. Statistic values of position drift error in 60 sec GPS outages.

Scenarios		1	2	3	4	5	6
RMS (m)	100 Hz, Unsimplified	9.5	6.17	17.88	3.22	8.3	9.98
	100 Hz, Simplified	9.67	6.58	18.62	3.02	8.23	10.06
	Degradation (%)	1.7	6.6	4.1	−6.2	−0.8	0.8
	10 Hz, Simplified	10.59	9.06	25.45	3.17	7.74	10.84
	Degradation (%)	11.5	46.8	42.3	−1.5	−6.7	8.6

. The degradation in Table 6 represents the degradation percentage of the positioning error of the simplified algorithm relative to the positioning error of the unsimplified algorithm. Compared with the unsimplified algorithm with a data rate of 100 Hz, the simplified algorithm has an error of less than 2% in the other four scenarios except for scenarios ➁ and ➂. It can also be seen from the table that reducing the data rate has a more significant impact on the performance of the inertial navigation system.

5.2. Complex Scene Simplified Algorithm Verification

The GNSS receiver has high positioning accuracy in open scenarios, and it is not suitable to evaluate the simplified algorithm. Therefore, the route in Figure 4 is selected to verify the performance of the simplified algorithm (simplified Kalman filter and simplified INS mechanization). It is mainly divided into zero velocity state scenarios, non-zero velocity state scenarios with RTK fixed/float solution, and non-zero velocity state scenarios without RTK fixed/float solution.

5.2.1. Simplified Kalman Filter Algorithm

• Zero velocity state scenarios

In this research, a reliable high-resolution pulse encoder is used as an odometer in the integrated navigation system. The encoder can generate 4096 pulses per circle. The TIM_encoder peripheral of MCU detects the number of pulses and calculates the vehicle speed. The TIM_encoder peripheral has a filtering function to filter out some pulse interference signals. The integrated navigation system determines whether to execute ZUPT by detecting the vehicle’s speed, and the speed threshold is 0.1 m/s. This value can prevent system misjudgment caused by interference signals. When the vehicle velocity is zero, the integrated navigation system performs a 10 Hz data rate simplified INS mechanization and adopts the simplified Kalman filter algorithm with one-step P matrix prediction. The results of the simplified and unsimplified algorithms are compared with NovAtel SPAN CPT-6, and the positioning error of the unsimplified Kalman filter algorithm and unsimplified INS mechanization algorithm with 100 Hz data rate are used as a reference. In Figure 9a, the position and velocity errors of the unsimplified Kalman filter algorithm are shown in the red circle. The position error remains unchanged, while the velocity error converges to near zero. Figure 9b shows the position and velocity errors of the simplified Kalman filter algorithm. The result is the same as Figure 9a. The position error remains unchanged, and the speed error converges to zero. Comparing them in

Table 7. Statistic values of the position error of the unsimplified and simplified Kalman filter algorithm in zero velocity scenario.

Scenarios		1
RMS (m)	Unsimplified	0.04
	Simplified	0.04
	Degradation (%)	-

shows that the simplified Kalman filter algorithm and the unsimplified Kalman filter algorithm have little effect on the position error after ZUPT.

• Non-zero velocity state scenarios with RTK fixed/float solution

Boulevard 1, boulevard 2, overhead bridge, and urban street in Figure 4 are selected to verify the performance of the simplified Kalman filter algorithm in non-zero velocity state scenarios with RTK fixed/float solution. In these scenarios, if the GNSS is a fixed solution, the system only performs the simplified INS mechanization algorithm with 10 Hz data rate and simplified Kalman filter algorithm. However, if it is a float solution, the INS data rate becomes 100 Hz, and the odometer information and NHC information are combined to assist INS. In this case, the results of the unsimplified and simplified algorithms are compared with NovAtel SPAN CPT-6, and the positioning error of the unsimplified Kalman filter algorithm with a 100 Hz IMU data rate is used as a reference. Figure 10 shows the position errors comparison between the unsimplified Kalman filter algorithm and the simplified Kalman filter algorithm. It can be seen from

Table 8. Statistic values of the position error of the unsimplified and simplified Kalman filter algorithm in boulevard 1, boulevard 2, overhead bridge, and urban street.

Scenarios		1	2	3	4
RMS (m)	Unsimplified	0.19	0.44	0.57	0.33
	Simplified	0.205	0.44	0.57	0.33
	Degradation (%)	7.8	-	-	-

that the simplified algorithm can roughly maintain the same positioning accuracy as the unsimplified Kalman filter algorithm in the non-zero speed state with RTK fixed/float solution.

• Non-zero velocity state without RTK fixed/float solution

Tunnel 1 and tunnel 2 in Figure 4 are selected as the test scenarios to verify the performance of the simplified Kalman filter algorithm in non-zero velocity state without RTK fixed/float solution. In these two scenarios, there is no GNSS signal, only the speed information output by the odometer and the lateral and longitudinal constraints of the vehicle, that is, NHC information to assist INS. The update rate of the speed and NHC is 10 Hz, and the IMU data rate is 100 Hz. In this case, the results of the unsimplified and simplified algorithms are compared with NovAtel SPAN CPT-6, and the positioning error of the unsimplified Kalman filter algorithm with a 100 Hz IMU data rate is used as a reference. Figure 11 shows a comparison of the position error between the unsimplified and simplified Kalman filter algorithm. It can be seen from the figure that the simplified algorithm has little effect on the positioning accuracy compared with the unsimplified Kalman filter algorithm. According to

Table 9. Statistic values of the position error of unsimplified and simplified Kalman filter algorithm in tunnel 1 and tunnel 2.

Scenarios		1	2
RMS (m)	Unsimplified	0.62	0.81
	Simplified	0.64	0.87
	Degradation (%)	3.2	7.4

, the position error of the simplified algorithm in tunnel 1 degrades by 3.2% relative to the unsimplified algorithm, and in tunnel 2 it degrades by 7.4%. However, this degradation is within an acceptable range.

5.2.2. Simplified INS Mechanization

The zero-velocity state scenario, boulevard 1, tunnel 1, boulevard 2, tunnel 2, overhead bridge, and urban street in Figure 4 are selected as the test scenarios to verify the performance of the simplified INS mechanization algorithm. GNSS signal is interrupted in these test sections, and only the odometer and NHC assisted the INS. Under this condition, the simplified INS mechanization and unsimplified INS mechanization algorithm are quantitatively analyzed. The positioning error obtained by comparing the positioning results of the simplified and unsimplified algorithms with NovAtel SPAN CPT-6 is shown in Figure 12. It can be seen from the figure that the positioning error of the 100 Hz data rate simplified algorithm is almost the same as that of the 100 Hz data rate unsimplified algorithm, indicating that the simplified INS algorithm has little effect on the positioning accuracy in complex scenarios. However, compared with the positioning error of the simplified algorithm with a data rate of 10 HZ and the unsimplified algorithm with a data rate of 100 Hz, the positioning accuracy of the former deteriorates more severely on some road sections. According to

Table 10. Statistic values of position drift error of the different INS algorithms and different data rates in the scenarios of zero velocity state, boulevard 1, tunnel 1, boulevard 2, tunnel 2, overhead bridge, and urban street.

Scenarios		1	2	3	4	5	6	7
GNSS outages duration (s)		33	150	24	90	24	15	60
RMS (m)	100 Hz, Unsimplified	0.07	9.622	0.239	5.693	0.297	1.038	0.383
	100 Hz, Simplified	0.07	9.661	0.238	5.705	0.296	1.039	0.379
	Degradation (%)	-	0.41	−0.41	0.21	−0.33	0.096	−1.04
	10 Hz, Simplified	0.0703	10.295	0.322	5.88	0.299	1.052	0.393
	Degradation (%)	0.43	6.99	34.7	3.28	0.67	1.34	2.61

, compared with the unsimplified algorithm with a 100 Hz data rate, the maximum deterioration of the simplified INS algorithm with a 100 Hz data rate is only 0.21%. It is even slightly better than the unsimplified algorithm on some road sections. Due to the reduced sampling rate, the simplified algorithm with a 10 Hz data rate in tunnel 1 is 34.7% worse than the unsimplified algorithm with a 100 Hz data rate, but this is within an acceptable range. In the seven scenarios, the GPS interruption time is different, the longest is 150 s, and the shortest is only 15 s, while the maximum positioning error is 10.295 m, and the minimum is 0.07 m. However, the positioning error is not related to time, mainly caused by factors such as the vehicle speed, system state, and road environment.

5.3. Complex Scene Positioning Performance Verification

GNSS signals are susceptible to environmental interference, resulting in decreased positioning accuracy, especially single-frequency GNSS [46,47]. The horizontal positioning error of GNSS in the experimental route is shown in Figure 13. In the figure, the GNSS solution status is marked with green, yellow, and red on the x-axis. Green indicates that the current GNSS status is a fixed solution, and the positioning error can reach about 2 cm. Yellow means that the GNSS status is a float solution, and the positioning error is less than 1 m. However, serious errors occasionally occur, and the maximum value has reached 6 m. Red indicates the differential solution mainly caused by the GNSS signal being blocked in tunnel 1 and tunnel 2. It can be seen from the figure that most of the position results in the whole experimental route are float solutions. The GNSS positioning error is the largest at the two tunnels. As shown in the enlarged part of the figure, the maximum value exceeds 110 m.

The six complex scenarios in Figure 4 are selected to verify the real-time positioning performance of the simplified algorithm (including the simplified INS mechanization algorithm and simplified Kalman filter algorithm). The RMS statistical horizontal position error of six scenes (boulevard 1, tunnel 1, boulevard 2, tunnel 2, overhead bridge, and urban street) are shown in Figure 14. The positioning error of the unsimplified algorithm is blue, and the simplified algorithm is red. In this case, the results of the simplified and unsimplified algorithms (including the 100 Hz data rate unsimplified INS mechanization algorithm and unsimplified Kalman filtering algorithm and integrating odometer information and NHC information) are compared with NovAtel SPAN CPT-6, and the positioning error of the unsimplified algorithm is used as a reference. It can be seen from Figure 14 that there is little difference in the position error between the unsimplified and simplified algorithms. The positioning errors of the six scenes in the figure are analyzed quantitatively, as shown in

Table 11. Statistic values of the position error of the unsimplified algorithm and the simplified algorithm.

Scenarios		1	2	3	4	5	6
RMS (m)	Unsimplified	0.16	0.3635	0.3906	0.6429	0.649	0.3256
	Simplified	0.16	0.3636	0.3901	0.6431	0.6489	0.3257
	Degradation (%)	-	0.028	−0.13	0.03	−0.015	0.03

. The degradation of the RMS statistical position error of the six scenes is less than 0.1%. Even in some scenarios, the value of the simplified algorithm is better than the value of the unsimplified algorithm, which is theoretically reasonable.

The simplified integrated navigation algorithm in the whole road section controls the data rate and selects the observation information to update the measurement according to different scenes and motion modes. The real-time position error is shown in Figure 15. At the same time, it also shows the position error of the unsimplified integrated navigation algorithm in the whole experimental route. It can be seen from the figure that the north and east direction position errors of the simplified algorithm have little degradation compared to the unsimplified algorithm. However, in the down direction, the simplified algorithm has a large fluctuation in the early stage. The main reason is that GNSS is a fixed solution during this period, and the IMU data rate is only 10 Hz, and there is no external sensor assistance. Generally speaking, in the process of vehicle navigation, vertical error is rarely concerned. Therefore, compared with the unsimplified algorithm, the horizontal positioning error obtained by the simplified algorithm has an acceptable degree of degradation.

For vehicle navigation, velocity is another crucial parameter. Therefore, in a complex environment, it is necessary to analyze the speed accuracy of the simplified integrated navigation algorithm. The real-time velocity error of the single-frequency GNSS RTK/MEMS-IMU/odometer integrated navigation with simplified and unsimplified algorithms in the whole experimental route is shown in Figure 16. The error curves in the north and east directions are similar, and the maximum values are less than 0.25 m/s. However, the error of the simplified algorithm in the early stage of the down direction fluctuates wildly, but it tends to be consistent after the later convergence. It can be seen from the figure that no matter whether the algorithm is unsimplified or simplified, the errors in the north and east directions are relatively small, and the trends of the two are basically the same.

Similar to position and velocity, attitude accuracy is also necessary for vehicle navigation applications. Figure 17 shows the attitude error curve of the integrated navigation system in the experiment route. The roll, pitch, and yaw errors of the simplified algorithm in the first 200 s are quite different from those of the unsimplified algorithm. The slower convergence speed of INS mainly causes this during this period. Since the IMU used in the research is MEMS grade, yaw angle is more susceptible to drift than roll angle and pitch angles. In other words, the yaw angle error increases faster than the roll and pitch, which can also be observed in Figure 17. However, the more significant influence on the positioning error in the integrated navigation algorithm is the yaw rather than the roll and pitch. It can be seen from the figure that the simplified algorithm has a significant fluctuation in the yaw error in the first 100 s and then stabilizes and is less than 2 degrees.

The real-time position, velocity, and attitude RMS and STD statistics of the entire experimental route are shown in

Table 12. Statistic values of the position, velocity, and attitude errors of the unsimplified and the simplified algorithm in the experimental route.

			RMS	STD
Position	North (m)	Unsimplified	0.260	0.208
		Simplified	0.260	0.207
		Degradation (%)	0.00	−0.48
	East (m)	Unsimplified	0.218	0.201
		Simplified	0.214	0.200
		Degradation (%)	−1.83	−0.49
	Down (m)	Unsimplified	4.323	0.998
		Simplified	4.473	1.087
		Degradation (%)	3.46	8.91
Velocity	North (m/s)	Unsimplified	0.029	0.028
		Simplified	0.031	0.030
		Degradation (%)	6.89	7.14
	East (m/s)	Unsimplified	0.035	0.035
		Simplified	0.036	0.036
		Degradation (%)	2.85	2.85
	Down (m/s)	Unsimplified	0.032	0.032
		Simplified	0.055	0.053
		Degradation (%)	71.8	65.6
Attitude	Roll (deg)	Unsimplified	0.567	0.483
		Simplified	0.534	0.375
		Degradation (%)	−5.82	−22.3
	Pitch (deg)	Unsimplified	0.129	0.128
		Simplified	0.140	0.137
		Degradation (%)	8.52	7.03
	Heading (deg)	Unsimplified	0.721	0.352
		Simplified	0.734	0.361
		Degradation (%)	1.80	2.55

. According to the statistical results of north and east position errors, the horizontal position RMS and STD of the unsimplified integrated navigation algorithm are 0.339 m and 0.289 m, respectively. In comparison, the simplified algorithm is 0.336 m and 0.287 m, respectively. Therefore, the horizontal position error of the simplified algorithm is slightly better than that of the unsimplified algorithm, which is reasonable in theory. For the unsimplified and simplified algorithms, the north, east, and down direction velocities RMS and STD are less than 0.055 m/s. However, the simplified algorithm degrades 71.8% and 65.6% in terms of down direction velocity compared with the unsimplified algorithm. The main reason for this error is that the ground-up position error is large, and there is no speed observation information in the vertical direction. In terms of attitude error, the roll RMS and STD of the simplified algorithm are smaller than those of the unsimplified algorithm, while the pitch and heading are degraded. Compared with the unsimplified algorithm, the pitch RMS and STD of the simplified algorithm are devalued by 8.52% and 7.03%, respectively, but this degradation is within an acceptable range.

5.4. Complex Scene Real-Time Power Consumption Verification

The performance of the simplified algorithm in the multi-sensor fusion navigation SiP is evaluated. The algorithm can meet the requirements of sub-meter high-precision positioning and low power consumption. The vehicle power meter and power supply are used to measure the power consumption of SiP chips in real-time and high-precision navigation in complex scenes, and the power consumption data in the process is saved. The result of power consumption data plotting is shown in Figure 18, and its average value is 141 mW.

In a word, compared with the unsimplified algorithm, the position, speed, and attitude degradation of the simplified algorithm are relatively small, and the algorithm running on the SiP chip consumes less power, which meets the requirements of low power consumption without sacrificing the positioning accuracy.

6. Discussion

Positioning accuracy and power consumption in a complex urban environment are two opposing indicators. The trade-off between these two indicators is an urgent matter for integrated navigation chips. According to the presented results, the single-frequency GNSS/MEMS-IMU/odometer integrated navigation simplified algorithm with adaptive power consumption adjustment can meet the requirements of positioning accuracy and power consumption at the same time in complex scenarios. It can be seen from Table 1 that the simplified INS algorithm omits small items that have little effect on the positioning accuracy, so that the calculation amount is reduced by about 72.5% compared with the unsimplified algorithm. However, if a combination of the simplified algorithm and down-sampling is applied, the computation load can be reduced by about 97.3%, and the degradation of positioning error is within an acceptable range. Furthermore, the one-step prediction method used in the simplified Kalman filter algorithm dramatically reduces the computation of the Kalman filter. It can be seen from Figure 9, Figure 10 and Figure 11 that in different scenarios, using different observation information for measurement update, the simplified algorithm has little impact on the positioning accuracy compared with the unsimplified algorithm.

The real-time test results of single-frequency GNSS/MEMS-IMU/odometer simplified integrated navigation algorithm with adaptive power consumption adjustment in complex scenes are shown in Figure 15, Figure 16 and Figure 17. It can be seen from the figure that the position, velocity, and attitude error curves of the simplified and unsimplified algorithms are highly similar, except for the time when the system converges at the initial moment. The main reason for the longer convergence time is that the IMU data rate is 10 Hz and only GNSS observation information is available. The real-time power consumption after the simplified algorithm is shown in Figure 18. The average power consumption in the entire test section is 141 mW, including GNSS, MEMS-IMU, magnetometers, barometers, temperature and humidity sensors, and MCU.

7. Conclusions

This paper proposes a single-frequency GNSS/MEMS-IMU/odometer real-time high-precision simplified integrated navigation algorithm with dynamic power adaptive adjustment capability in complex environments. It is implemented in a multi-sensor fusion navigation SiP chip. The simplified INS algorithm and the simplified Kalman filter algorithm are combined with the strategy of adaptively adjusting the data rate and selecting the observation information for measurement update in different scenarios and sports modes to achieve high-precision positioning and low power consumption in complex scenarios.

In this paper, the simplified INS algorithm and the simplified Kalman filter algorithm are analyzed theoretically, and the computational complexity of the algorithm is counted. At the same time, the real-time verification of complex scenes is carried out. According to the statistical results, the simplified algorithm dramatically reduces the computation load compared with the unsimplified algorithm. Furthermore, the experimental results show that the positioning accuracy of the simplified algorithm degrades very little compared with the unsimplified algorithm.

In summary, the proposed simplified algorithm will not cause a decrease in navigation accuracy while reducing power consumption. In other words, the algorithm simplification does not sacrifice accuracy. The test results of a single-frequency GNSS/MEMS-IMU/odometer simplified integrated navigation algorithm with adaptive dynamic power adjustment in complex scenes are shown in Table 12. The results show that it is feasible to apply the proposed simplified integrated navigation algorithm to vehicle navigation. It is of great significance for applications that require demanding a real-time response.