In-Motion Iterative Fine Alignment Algorithm for On-Board Vehicular Odometer-Aided SINS

This research proposes a novel in-motion fine alignment algorithm for vehicular dead reckoning (DR) with odometer-aided strapdown inertial navigation system (SINS) while the map matching result is used for a group of landmark points to estimate misalignment angles. The proposed algorithm is designed based on principle of similarity, that is, trajectory of DR is similar to the true trajectory that the main difference between these two trajectories is rotation and scale. Further, the results from map matching are introduced as a group of landmark points to estimate the residual of azimuth error angle after coarse alignment and the scale factor error of the odometer. It is theoretically proved that the alignment effectiveness based on the results from map matching is equivalent to that on single zero error landmark point. Finally, digital simulations are conducted to verify the presented algorithm and test the performance.


Introduction
Strapdown inertial navigation system (SINS), considered as a dead reckoning system, utilizes the measurements from gyros and accelerometers to calculate the real-time attitude, position, and velocity of the vehicle. Initial alignment based on a preknown initial position directly determines the performance of SINS. Usually, methods for initial alignment are divided into two different types: self-alignment and transfer alignment. As to the autonomous vehicular application, self-alignment is required. Generally speaking, rapidity, accuracy, and autonomy are the basic requirements of the self-alignment methods. Especially rapid self-alignment is one of the key requirements for the circumstances such as military manoeuvers and rescue missions. However, rapidity and accuracy are contradiction to each other for the noise problem, that is, more time will be consumed to achieve an accurate self-alignment. Thus, in-motion alignment scheme is required for rapid response missions.
Many works have been done in field of in-motion alignment. Park et al. presented SINS/GPS alignment by using carrier phase rate information from on-boarded two-antenna GPS receiver [1]. Li et al. developed a DVL-(Doppler velocity log-) aided in-motion alignment algorithm [2]. Wan et al. presented an in-motion alignment method by using the position information from GPS [3]. Bimal and Ashok also studied DVL-aided in-motion alignment scheme [4]. Zhang et al. utilized the measurements from GPS and odometer (OD) to conduct fast alignment [5]. Cui et al. studied in-motion alignment for SINS/GPS under random misalignment [6]. Kaygısız and Şen presented in-motion alignment for a lowcost GPS/INS under large heading error [7]. Silson developed a rapid in-motion alignment method by using GPS position and velocity measurements [8]. Wang et al. proposed an odometer-aided in-motion alignment algorithm [9]. Yan presented an in-motion alignment algorithm for SINS/OD DR system utilizing a known landmark point based on principle of similarity [10].
It can be found that many of them require the information from GPS. However, GPS measurement information is not always available for signal blocking problem. Thus, more scholars put their attentions on the navigation without GPS that the self-alignment is independent to GPS. Inspired by the concept of principle of similarity using a known landmark point [10], a novel iterative in-motion fine alignment algorithm for vehicular DR based on SINS/ OD is developed in this paper, where a group of virtual landmark points from the map matching result is used to estimate the azimuth error of SINS and the scale factor error of OD. Benefiting from the novel algorithm, the inmotion fine alignment can be achieved without GPS or even a zero velocity stop.
The formulation of the DR algorithm based on SINS/OD is reviewed firstly, and then positioning error analysis for DR is presented in Section 2. Novel iterative in-motion fine alignment algorithm utilizing a group of noised points from map matching result is presented in Section 3. Digital simulations are presented in Section 4. Conclusions are presented in Section 5.

Review of Dead Reckoning Algorithm Based on SINS/OD.
First of all, the coordinate frames used in this paper are defined as follows [11]. The inertial frame i is a stationary Earth-centered frame and shares its polar axis but not rotate with it. The Earth frame e is an Earth-centered-Earth-fixed frame whose x-axis points at the intersection of the Greenwich meridian and the equatorial plane z-axis points to the North Pole. The navigation frame n moves with the vehicle about the surface of the Earth. The axes of n point to the direction of east, north, and geodetic latitude (radial). The vehicular body frame b is a fixed frame to the vehicle body. The y-axis is pointing forward along the symmetric axis of the vehicle while the x-axis is orthogonal to the y-axis and points to the right wing, and z-axis completes the righthand orthogonal set.
It is assumed that the wheels cling to the ground without any side skidding and jumping during moving, the frame of SINS is coincident with the body frame of the vehicle. Then, the vehicle's velocity in the body frame can be governed by where v D is the speed of the vehicle measured by OD. The velocity in navigation frame can be expressed by where v n DE , v n DN , and v n DU are the components along the three axes of navigation frame.
Thus, the position differential equations for DR are modeled as follows [10]: where L, λ, and h are the longitude, latitude, and height, respectively, and R M is the prime radius of curvature while R N is the meridian radius of curvature. By assuming the updating period of OD is T D during which N samples are available, the iterative updating algorithm can be governed by [10]

Position Error Analysis for Dead Reckoning Algorithm
Based on SINS/OD 2.2.1. Sensor Error Models. Usually, initial attitude matrix for SINS from its body frame to the navigation frame can be obtained from coarse alignment by taking advantage of double-vector method, with the measurements of gyro and accelerometer [12]. Generally speaking, after a fast coarse alignment, horizontal attitude angle error would be small, but the azimuth angle error could not be well estimated. Thus, the main uncertainty for SINS after coarse alignment is the azimuth angle error. The residual of the azimuth angle error is modeled as a constant Then, the attitude matrix approximates to the following expressionĈ where φ denotes the vector 0 0 φ T while ⋅ × stands for the cross-product operation. Next, the accuracy of OD is another key parameter to the dead reckoning system. The speed accuracy of OD is mainly affected by the status of the road surface, tire inflation, and abrasion [13]. Because it is impossible to precisely model these factors, the scale factor error δK D for OD is considered in this manuscript. The scale factor error is assumed to include two parts: random constant and noise which are modeled as a one-order Markov process 2 Journal of Sensors where δK Db denotes the random constant, δK Dm is the noise with a correlation time τ, and w Dm is a Gaussian noise.

Error Model for Dead
Reckoning. Defining the velocity measurement error of OD in the body frame is δv b D , and then the velocity in the navigation frame can modeled aŝ Substituting (6) into (8) yieldŝ and then the velocity error in navigation frame can be obtained Substituting (1) into (10) produces Next, by conducting partial differential operation to (3) with respect to the time variable, the position error equations for DR system based on SINS/OD can be achieved Denoting δp = δL, δλ, δh T , rephrasing (12) into matrix form, and substituting (11) into it yield

Iterative In-Motion Fine Alignment Algorithm for DR
As can be seen from (13), the position uncertainty of DR mainly depends on the azimuth error φ, the scale factor error δK D , and the error δp of the preknown initial position, among which δp is usually unknown and can be considered to be pretty small. Thus, in order to achieve a good alignment performance, the azimuth error and scale factor error are required to be accurately estimated.
In [10], an in-motion alignment scheme is proposed where one accurate landmark point is required for the algorithm. However, accurate landmark point is usually unavailable if the vehicle does not pass by the required area. In [14], the experiments have shown that a couple of points can be obtained by map matching algorithm based on SINS/OD/ Map database, where there is no requirement of appointed route for the vehicle. Thus, the map-matched points can be considered as a group of virtual landmark points to be used to estimate the misalignment error. In the following parts, the map-matched points are assumed to be available.

Fine Alignment Method Based on Single Landmark Point.
As depicted in Figure 1, the DR trajectory can be considered as the rotating and stretching trajectory of the true trajectory based on the principle of similarity [10], that is, where φ U is the up-direction attitude error angle, and ΔS h is the horizontal projection of the true trajectory, φ U 3 = 0, 0, φ U T . It should be emphasized that this conclusion is under the assumptions: (a) azimuth angle error is a small constant; (b) horizontal attitude error can be ignored.
Then, φ U can be easily derived based on the triangle geometry as follows: Because the horizontal attitude angel error is usually pretty small, φ U approximates to the azimuth angle error, that is, δψ ≈ φ U .

Journal of Sensors
Next, the scale factor error of OD can be achieved by using the true distance and the DR distance as follows: As shown in Figure 2, P i is the ith truth position, and P i is the output of DR. P i ′ or P i ′′ is a potential of the map-mapped points which depend on the map matching result. Some of the map-mapped points used as landmark will lead to a smaller azimuth than P i while other map-mapped points will lead to a larger azimuth than. P i . As to descript the problem more compactly, P ′ is used to denote the map-mapped points of P. Thus, when n map-mapped points are available, the estimated azimuth angle error can be modeled as where ε i is random noise which is uniformly distributed in −c, c ; c is a random constant that depends on the map matching algorithm. Then, n noises satisfy Next, the following equation based on n measurements can be obtained Surprisingly, the estimation mean of the azimuth angle error based on n map-mapped points is equal to the result based on one accurate landmark point.
As a result, the estimation algorithm based on the n mapmapped points can be obtained Till now, the estimation for δψ and δK D based on the results of map matching has been derived. However, several assumptions are required to make for the derivation. Therefore, there will still have residual errors after the error compensation by using the estimation results shown in (21) and (22). In order to compensate these residuals, the iterative alignment method is considered in this paper.
(i) Do a fast coarse initial alignment for SINS and then drive the vehicle.
(ii) Estimate δψ and δK D according to (21) and (22) when map-mapped points are available.
(iii) Use δψ to modify the initial attitude matrix and use δK D to update the scale factor of OD.
(iv) Redo the calculation of DR with the memorized measurements of SINS and OD, that is, angular velocity, specific force, and speed, and then use the newest DR points to estimate δψ and δK D again.

Numerical Simulation Results
In order to verify the validity and test the performance of the proposed algorithm, a set of digital simulations have been conducted. The parameters for the motion of the vehicle are presented in Table 1. The motions of station keeping, acceleration, uniform, and turning are mainly included where the nominal trajectory is shown in Figure 3 while the nominal azimuth is depicted in Figure 4.   Journal of Sensors The random drifting rate of gyro is set to be 0.1°/h while the intensity of the random noise is selected as 0.02°/h. The bias of the accelerometer is 2 × 10 −4 g whilst the intensity of the random noise is 1 × 10 −4 g. The scale factor error of the OD is 0.3%. Moreover, the initial attitude error angles are 0 7 ′ and 0 5 ′ for horizontal and 30 ′ for azimuth.
Additional, the CEP of MM points is 3 m while the modification is conducted at epoch 500 s. Total simulation period is 600 s while the step is 0.1 s.
As shown in Table 2, four comparison cases are simulated: (1) fine alignment aided by one accurate landmark point, (2) fine alignment aided by one landmark point with a 2 m uncertainty, (3) fine alignment aided by landmark points from map matching with a 2 m uniformly distributed noise, and (4) iterative fine alignment aided by landmark points from map matching with a 2 m uniformly distributed noise.
The alignment results for case 1 are shown in Figure 5. The estimation value for δK D is 0.0022 according to (22), which is about 26.7% different to the nominal scale factor error. The estimation for δψ is about 26 170 ′ according to (21) while the after-correction azimuth angle error is 4 634′. One thing should be pointed out is that the gyro drifts about 0 83′ at the end of 500 s.
The results for case 2 are presented in Figure 6, where estimations for δK D and δψ are 0.0021 and 25 898′, respectively. The estimation error of δK D is about 22%. The aftercorrection azimuth angle error is 4 905 ′ . In this case, the vehicle moves 9.7 km for 500 s while the displacement is 7 km. Thus, a 2 m uncertainty would introduce a 2/7000 rad (≈1 029 ′ ) error. Additionally, 0 83 ′ gyro drift is the same as case 1.
As shown in Figure 7 are the results for case 3. One thousand map matching points with zero-mean 2 m standard deviation noise during 490 s to 500 s are utilized as the landmark point group. The same as the other cases, the estimation and correction are conducted at epoch 500 s. The estimation values for δK D and δψ are 0.0023 and 26 121′, respectively. The estimation error of δK D is about 23% with respect to the nominal value. The after-correction azimuth angle error is 4 680′. As a result, it can be concluded that the fine alignment for azimuth angle based on map matching points group almost shares the same accuracy with that based on one landmark point whose position is accurately known.
As shown in Figure 8 are the results for case 4. The algorithm executed in this case is presented at the end of  Further, by comparing Figures 5-8, it can be seen that horizontal attitude angle errors of all cases could not be corrected because the proposed algorithm can only estimate the azimuth angle. Anyway, the residual of horizontal angle errors after coarse alignment is small that these errors drift slowly, that is, the drifting of 600 s is no more than 1′. Moreover, the azimuth errors of four cases have a hopping at epochs 50, 70, 170, and 180 s. The reason for this problem is that these epochs are the starting and ending time of the turning motion which would continually change the azimuth          Journal of Sensors angle. Above all, estimation error of the cases can be summarized in Table 3.
In summary, the proposed iterative fine alignment algorithm can achieve the highest accuracy while the alignment based on map matching points shares the same accuracy level with that based on one zero error landmark point. However, the iterative algorithm requires to store the sensor data for a while and more computational resources to redo the DR calculation. In MATLAB environment, the time consumption for the recalculation is less than 5 s and the storage for the data is no more than 10 M. Therefore, the requirements of the storage and computing time are easy to be satisfied for modern computer technology.

Conclusions
This paper developed a novel iterative fine alignment for inmotion vehicle based on the virtual landmark points from the results of map matching. In detail, firstly, the analysis of position error for dead reckoning algorithm based on SINS/OD was conducted. Depending on the result of the error analysis, the azimuth error and scale factor error were chosen to be the estimated variables for fine alignment. Then, the fine alignment method by using single landmark point was reviewed, based on which the novel iterative fine alignment algorithm by utilizing the map matching results as virtual landmark points was developed. Lastly, the accuracy of the proposed algorithm was determined and presented by four comparison simulation cases. As expected, the proposed iterative alignment algorithm could achieve the accuracy level of second of arc. Detailed, the initial attitude error of SINS and the scale factor error could be effectively estimated and compensated based on the proposed algorithm while no requirement of other aided information or zero velocity is required.
Next, the proposed algorithm will be further investigated by in-motion vehicular experiments.

Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
The authors declare that they have no conflicts of interest.