State transformation extended Kalman filter–based tightly coupled strapdown inertial navigation system/global navigation satellite system/laser Doppler velocimeter integration for seamless navigation of unmanned ground vehicle in urban areas

With the rapid development of unmanned ground vehicle industry, how to achieve continuous, reliable, and high-accuracy navigation becomes very important. At present, the integrated navigation with global navigation satellite system and strapdown inertial navigation system is the most mature and effective navigation technology for unmanned ground vehicle. However, this technique depends on the signal accuracy of global navigation satellite system. When the receiver cannot capture four or more satellite signals for a long time or the satellite completely invalid, it cannot provide accurate navigation and positioning information for the unmanned ground vehicle. Therefore, this article combine the observation information of strapdown inertial navigation system, global navigation satellite system, and laser Doppler velocimeter to propose a high-precision seamless navigation technique of unmanned ground vehicle based on state transformation extended Kalman filter. Under different land vehicle driving environments and global navigation satellite system signal quality conditions, the seamless navigation technique is evaluated through global navigation satellite system interruption simulation and land vehicle experiments. The experimental results show that the strapdown inertial navigation system/global navigation satellite system/laser Doppler velocimeter tightly coupled integration seamless navigation has good environmental adaptability and reliability and can maintain high navigation accuracy under high frequency global navigation satellite system–signal blockage conditions in urban areas.


Introduction
In the process of research and application of unmanned ground vehicle (UGV), due to the more diverse task requirements, the navigation technology which can keep high positioning accuracy in various driving environments has been widely concerned. It has also become the key to promote the further development of unmanned technologies. Strapdown inertial navigation system (SINS) is an autonomous navigation system that does not rely on external information. It has the advantages of good concealment and strong anti-interference ability. The disadvantage is that the navigation error accumulates with time. Global navigation satellite system (GNSS) can provide users with all-weather, all-time, continuous 3D position and 3D velocity information, but its disadvantages are poor dynamic performance, susceptibility to electromagnetic interference, and easy to block satellite signals. The integration of SINS and GNSS can overcome the shortcomings of low long-term accuracy of SINS, poor dynamic performance of satellite navigation system, and obtain better performance than using any kind of navigation sensors alone. At present, SINS/GNSS is the most commonly used navigation mode of land vehicle navigation system, which can provide highprecision output with low error for a long time and meet the requirements of real-time positioning. [1][2][3][4][5][6] However, SINS/ GNSS still has shortcomings in some challenging environments. For example, the signal blocking caused by buildings, bridges, and trees in urban environment and the multipath effect in canyon and tunnel will lead to the GNSS equipment unable to output effective navigation information. If GNSS denied for a long time, the integrated system will degenerate into a pure inertial navigation system, and the navigation error will accumulate rapidly with time.
Therefore, in GNSS-signal blockage environments, other sensors are usually introduced to maintain the navigation accuracy of UGV, such as OD, cameras, and laser Doppler velocimeter (LDV) and so on.
Odometer is one of the most commonly used sensors in land vehicle self-contained navigation system. It can provide the vehicle velocity and distance independently by measuring the wheel rotation angle and has good antiinterference performance. [7][8][9][10][11] However, the performance of the OD will also be affected by the state of wheel and the road condition. The jumping, slipping, and deformation of the wheel also will produce errors, which will affect the accuracy of the forward velocity of the vehicle estimated by the encoder. 12 Visual navigation is a new navigation method with the development of computer vision, photogrammetry, and other science and technology. It uses stereo vision, optical flow, pattern recognition, and feature tracking matching to generate navigation information to help the vehicle achieve navigation and positioning. [13][14][15][16] However, as an optical device, the camera used in visual navigation is easily affected by the problems of shadow, field of vision, dynamic movement of the target, and cannot provide stable observation information when the visibility is low.
LDV is a new type of navigation sensor, which uses the good characteristics of laser to measure the driving velocity of the vehicle. [17][18][19] It has the advantages of long-distance measurement, high velocity measurement accuracy, and fast dynamic response. At the same time, as the measurement sensor which can provide vehicle velocity information, LDV has higher measurement accuracy and reliability than OD, because the less influence of vehicle driving conditions due to the noncontact measuring method. 18,20,21 Therefore, SINS/GNSS/LDV integrated navigation is a feasible method to improve the positioning accuracy of UGV in GNSS-signal blockage environments. Compared with the vision sensors, LDV occupies less computing resources, has higher measurement accuracy, and is more reliable and practical in challenging environments.
At present, EKF is one of the most commonly used filter estimation methods in SINS-based land vehicle integrated navigation system. Assuming that the estimation error is small enough, the Kalman gain is calculated by the firstorder linearization propagation of dynamics about the estimated trajectory. However, the variance of EKF may be inconsistent if the estimation result is far from the real state value, that is, the error state covariance matrix calculated by the filter is inconsistent with the real value. 10 10,23 In this article, our first aim is to improve the positioning accuracy of UGV in GNSS-signal blockage conditions, by introducing appropriate sensors to assist SINS and GNSS, so as to realize the seamless navigation of UGV in different environments. Our second aim is to adopt a new filtering method, which can make the integrated navigation system have better filtering stability and higher positioning accuracy. Our main contributions are summarized as follows: 1. A technique for high-precision seamless navigation based on integrated SINS/GNSS/LDV is proposed, which is suitable for richer and more complex application environments; 2. The measurement information of LDV is introduced to suppress the divergence of positioning error of UGV navigation system, when GNSS cannot provide high-quality and effective navigation information; 3. State transformation extended Kalman filter (ST-EKF) is used to replace the traditional EKF as the information fusion framework, and a new SINS/GNSS/LDV error state model and observation model are derived to improve the filtering robustness of the integrated navigation system; 4. The long-distance land vehicle test is carried out, including different driving environments and GNSS signal quality conditions. The application effect of the technique proposed in this article is evaluated by post-processing the experimental data.
The remainder of this article is organized as follows: The second section describes the system error state model of SINS/GNSS/LDV seamless navigation technique, the third section describes the system observation model and technology route, the fourth section introduces the experimental design and equipment, the fifth section discusses the experimental results, and the conclusions are given in the sixth section.

System error state model
The ST-EKF algorithm was first proposed in Wang et al. 23 and has been extended to be applied in integrated navigation systems such as SINS/GNSS, SINS/OD. 10,23 Experiments showed that ST-EKF had better filtering robustness than EKF in high dynamic environment. In this section, a new velocity error equation under the framework of ST-EKF was deduced and explained from the perspective of solving the covariance inconsistency problem. As well as that, the state transition matrix of the system was redefined, and the error state model and observation model of the SINS/GNSS/LDV integrated navigation system were established.

Definition of velocity error based on ST-EKF
The commonly used linear velocity error differential equation is defined as 24 where n and dv n eb are attitude misalignment angle vector and velocity error vector, respectively; C n b is the direction cosine matrix; f n is specific force vector in navigation frame; df b is accelerometer bias vector; ! n ie is the earth rotation vector; ! n in is the angular rate vector of the navigation frame relative to the inertial frame;! n en ¼ ! n in À ! n ie ; ½^ represents the transformation of a vector into a skew symmetric matrix; d! n ie and d! n en are the errors of ! n ie and ! n en , respectively; v n eb is the velocity in the real navigation frame. Since (1) contains specific force term and dv n eb ¼ṽ n eb À v n eb , the calculation accuracy ofṽ n eb is easily affected by the accuracy of specific force and attitude solution, which leads to the inconsistency of covariance estimation in the calculation of system matrix of the traditional integrated navigation system based on EKF, 22 thus damaging the accuracy of integrated navigation.
In ST-EKF, the velocity error is defined more strictly in the same coordinate system 10,23 where dv n is the new definition of velocity error; expðÁÞ represents the exponent of the matrix.
The following formula can be obtained by differentiating the new velocity error state where g n is the gravity vector in navigation frame; e b is the vector of gyro constant biases; r b is the vector of accelerometer constant biases; w g and w a are the white noises of the gyros and accelerometers, respectively. It can be seen from (3) that the specific force term is no longer included, but is replaced by gravity vector related term. For general local land vehicle navigation problem, the variation of g n is very small. Therefore, integrated navigation based on ST-EKF can effectively avoid the problem of inaccurate calculation caused by specific force noise and drastic changes in attitude, and ensure the robustness of filtering. 10,23

System error model of SINS/GNSS/LDV
The 20-dimensional error state vector x of SINS/GNSS/ LDV integrated navigation system is defined as where x SINS , x GNSS , x LDV represent the error state vectors corresponding to SINS, GNSS, and LDV respectively, which can be expressed as where dr n is the position error vector; cdt u is the equivalent distance error relates to the receiver clock error; cdt ru is the equivalent distance rate error relates to the clock drift; dk D is the scale factor error of LDV; da represents the installation error between SINS and vehicle body. Generally, only the pitch angle installation error da y and yaw angle installation error da z is considered. da y is generally not constant because of the bumps in the road, so can be modeled as the first-order Markov process. Similarly, the scale factor error of LDV will also be unstable for long-distance land vehicle navigation, so it can also be modeled as first-order Markov process.
The system error equation of SINS/GNSS/ LDV integration can be constructed as where x is the error state vector; F is the state transition matrix; G is the noise shaping matrix; w is the process noise vector. These terms are defined as The elements in w respectively represent white noises of gyros, accelerometers, equivalent distance error relates to the receiver clock error, equivalent distance rate error relates to the clock drift, scale factor error Markov process, and pitch angle installation deviation error Markov process.
The submatrix related to each sensor in the system matrix F and the noise transfer matrix G can be expressed as where t dk is the scale factor error correlation time of LDV; t day is the pitch angle installation error correlation time of LDV; v N , v E are the north and east velocities respectively; R N is the radius of curvature in the meridian; R E is the radius of curvature in the prime vertical; L, h are the latitude and altitude respectively; O is the earth rotation rate.

System observation model
In GNSS-signal blockage environments, in order to design an UGV seamless navigation system with good reliability and positioning accuracy, this article introduces LDV as auxiliary navigation equipment to improve the stability and performance of SINS/GNSS tightly integrated navigation system, and the technique route is shown in Figure 1.

GNSS observation model
Tightly coupled systems is a high-level integration. The pseudo range and pseudo range rate is a commonly used tightly coupled mode. The original information after receiving the pseudo range and pseudo range rate signals provided by GNSS receiver. The errors of each pseudo range and pseudo range rate signal are independent and uncorrelated. The deeply coupled systems involves the internal design of the sensor, which is relatively complex and will not be discussed here. Therefore, this article mainly adopts the tightly coupled method to integrate SINS and GNSS information.
The pseudo range rate solved by SINS of the satellite with pseudo-random number (PRN) k is defined as where e k is the sight line vector from SINS to the satellite with PRN k, v e eb is the velocity vector of SINS in ECEF coordinate system, v e k;sat is the velocity vector of the satellite with PRN k in ECEF coordinate system. The pseudo range solved by SINS of the satellite with PRN k is defined as r INS;k ¼ ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi ðx I À x s;k Þ 2 þ ðy I À y s;k Þ 2 þ ðz I À z s;k Þ 2 q where r e k;SAT ¼ ðx s;k ; y s;k ; z s;k Þ is the position of the satellite with PRN k in the ECEF coordinate system, r e eb ¼ ðx I ; y I ; z I Þ is the position of the SINS in the ECEF coordinate frame.
where ðx; y; zÞ is the true position of the receiver in the ECEF coordinate system, r k is the true distance between the satellite with PRN k and GNSS receiver dz GN SS is defined as the difference between the pseudo range and pseudo range rate measured by SINS and GNSS in the integrated navigation system. If the number of observable satellites is r, dz GN SS can be expressed as where C ECEF lLh is the transfer matrix that projects the latitude, longitude, and altitude errors into the ECEF coordinate system, n r;rÂ1 is the measurement noise vector of the pseudo range, and n _ r;rÂ1 is the measurement noise vector of the pseudo range rate.

LDV observation model
Assume that the installation angles between IMU and the vehicle body are small angles, thus direction cosine matrix C m b can be written as where da is the installation error angle between sins and vehicle body. The projection of the velocity solved by the SINS in the vehicle frame (m frame) can be expressed as The LDV measurement vector dz LDV can be obtained by computing the difference betweenṽ m eb and the LDV velocity under m frame

Observation model for SINS/GNSS/LDV
Combined with (27) and (35), the observation equation of SINS/GNSS/LDV integrated navigation system can be defined as is the observation noise vec- is the observation matrix.
where v LDV is the forward velocity measured by LDV.

Experimental design
In The experimental land vehicle is equipped with navigation-grade high precision FOGIMU, NEO-M8T GNSS receiver and RLDV-300 LDV. The material object of the sensors is shown in Figure 2. The sensor configuration and working mode of the experimental vehicle are shown in Figure 3. The specifications of the sensors are listed in Table 1.
The land vehicle carried out initial alignment with stationary period 180s. The reference position of the vehicle is calculated by the post smoothing algorithm of SINS/differential satellite integrated navigation, which is used for the

Land vehicle experiment in GNSS-denied environment
In the environment of high shielding such as tunnel, UGV cannot receive GNSS information, SINS/GNSS integrated navigation system degenerates into a pure inertial system, and the navigation error will accumulate rapidly with time. The method adopted in this article is to maintain the positioning accuracy of UGV by introducing the measurement information of LDV. Therefore, this experiment mainly verifies the application effect and improvement of SINS/ LDV integrated navigation algorithm based on ST-EKF in GNSS-denied environment.
This land vehicle experiment was carried out on Changsha Beltway, with a driving time of 1400s and a cumulative distance of about 21 km. The driving trajectory of this experiment is shown in Figure 4.
The horizontal position error of the three algorithms are shown in Figure 5. Table 2 shows RMSE of the horizontal position error.
It can be found from Figure 5 and Table 2, the navigation error of pure inertial navigation system accumulates rapidly in GNSS-denied environment, while SINS/LDV integrated navigation can effectively suppress the divergence of error. At the same time, SINS/LDV (ST-EKF) algorithm has higher positioning accuracy than SINS/LDV (EKF) under the same conditions, which can better help UGV complete navigation tasks in the absence of satellite information.

Land vehicle experiment in GNSS-signal blockage environment
In GNSS-signal blockage environment, the number of observable satellites received by UGV may be affected, thus affecting the positioning accuracy of SINS/GNSS integrated navigation system. This experiment mainly compares and verifies the positioning accuracy of SINS/ GNSS and SINS/GNSS/LDV tightly integrated navigation algorithm based on ST-EKF when there are less than four observable satellites.     This land vehicle experiment was carried out on Hua-Chang expressway, with a driving time of 900s and a cumulative distance of about 21 km. The driving trajectory of this experiment is shown in Figure 6. During the driving process, the land vehicle has experienced different satellite observation conditions. From 1 to 200s, there are only three satellites available (from 0 to 4.5 km). From 201 to 300s, there are only two satellites available (from 4.5 to 6.7 km). From 351 to 550s, there are only one satellites available (from 7.9 to 13 km). During the rest of the land vehicle experiment, there are six observable satellites.
The horizontal position error of the three algorithms are shown in Figure 7. Table 3 shows RMSE of the horizontal position error.
It can be found from Figure 7 and Table 3, SINS/GNSS (ST-EKF) tightly integrated algorithm has higher positioning accuracy than SINS/GNSS (EKF) under the same conditions, that is, it can better adapt to the adverse conditions of insufficient observable satellites.
At the same time, after introducing LDV information, the positioning accuracy has been further improved, which further proves the effectiveness of the seamless navigation algorithm proposed in this paper in GNSS-signal blockage environment.

Land vehicle experiment for long distance
When UGV performs long-distance driving tasks, it may experience various complex and challenging environments due to its large travel span, which puts forward higher requirements for the seamless navigation ability of UGV navigation system in different environments. Therefore, this experiment mainly compares and verifies the environmental adaptability and positioning accuracy of SINS/ GNSS/LDV seamless navigation technique based on ST-EKF in long-distance driving tasks with a distance of more than 200 km. This land vehicle experiment starts from Yueyang and ends in Changsha, passing through three cities, with a driving time of about 4.2 h and a cumulative distance of 227 km. In the process of driving, there are many different complex scenes. It is a challenge for land vehicle to achieve high-precision navigation and positioning when it pass through tunnels and urban roads with serious signal blocked. The driving trajectory of this experiment is shown in Figure 8. The special scene encountered by the satellite in the process of land vehicle driving can be shown in Figure 9. Figure 10 show the variation trend of position error and velocity error with time.
In order to evaluate the horizontal positioning accuracy of the three integrated navigation algorithms more intuitively, Figure 11 shows the variation trend of the horizontal positioning error and accuracy of these algorithms. Table 4 shows RMSE and the percentage of RMSE in the total distance of these algorithms.
By observing Figures 10 to 11 and Table 4, it can be found that after adopting SINS/GNSS/LDV seamless navigation technique, the land vehicle navigation system has     stronger environmental adaptability and reliability. Compared with the traditional SINS/GNSS tightly coupled integration navigation technique, it can maintain high horizontal positioning accuracy in the GNSS-signal blockage environments, which proves that the technique has value of helping UGV realize seamless navigation.

Conclusions
Aiming at the problem of UGV navigation in the GNSSsignal blockage environments, this article proposes a SINS/ GNSS/LDV tightly coupled seamless navigation technique. The post-processing of the measured vehicle experimental data shows that this technique has good reliability and can maintain high navigation and positioning accuracy even when the GNSS information cannot be received normally. Therefore, this technique can help UGV maintain enough navigation accuracy in the GNSS-signal blockage environments and realize seamless navigation in the real sense, which has good engineering application value.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.