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

Authors: Baichun Gong; Chenglong He; Xiaoyue Wang; Xin Li
Journal: Journal of Sensors
 (2018)

---

## Abstract

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.


---

## Body

## 1. 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 low-cost 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 in-motion 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 Section2. 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.


## 2. Problem Statements

### 2.1. 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 right-hand 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(1)vDb=0vD0T,where vD is the speed of the vehicle measured by OD.The velocity in navigation frame can be expressed by(2)vDn=CbnvDb=vDEnvDNnvDUnT,where vDEn, vDNn, and vDUn are the components along the three axes of navigation frame.Thus, the position differential equations for DR are modeled as follows [10]:
(3)L̇=vDNnRM+h,λ̇=vDEnRN+h⋅cosL,ḣ=vDUn,where L, λ, and h are the longitude, latitude, and height, respectively, and RM is the prime radius of curvature while RN is the meridian radius of curvature.By assuming the updating period of OD isTD during which N samples are available, the iterative updating algorithm can be governed by [10]
(4)Li=Li−1+TDRM+hi−1⋅1N∑j=1NvDNnj,λi=λi−1+TDRN+hi−1⋅cosLi−1⋅1N∑j=1NvDEjnj,hi=hi−1+TDN∑j=1NvDUjnj.


### 2.2. 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
(5)φ̇=0.Then, the attitude matrix approximates to the following expression(6)C^bn=Cnn′Cbn=I‐φ×Cbn,where φ denotes the vector 00φ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 δKD 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
(7)δKD=δKDb+δKDm,δK̇Db=0,δK̇Dm=−1τδKDm+wDm,where δKDb denotes the random constant, δKDm is the noise with a correlation time τ, and wDm is a Gaussian noise.


#### 2.2.2. Error Model for Dead Reckoning

Defining the velocity measurement error of OD in the body frame isδvDb, and then the velocity in the navigation frame can modeled as
(8)v^Dn=C^bnvDb+δvDb.Substituting (6) into (8) yields
(9)v^Dn=vDn−φ×vDn+CbnδvDb,and then the velocity error in navigation frame can be obtained
(10)δvDn=v^Dn−vDn=−φ×vDn+CbnδvDb.Substituting (1) into (10) produces
(11)δvDn=CbnvDb×φ+CbnvDb⋅δKD.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
(12)δL̇=1RM+hδvDNn−vDNnRM+h2δh,δλ̇=secLRM+hδvDEn+vDEn⋅secL⋅tanLRM+hδL+vDEn⋅secLRM+h2δh,δḣ=δvDUn.Denotingδp=δL,δλ,δhT, rephrasing (12) into matrix form, and substituting (11) into it yield
(13)δṗ=AδvDn+Bδp=ACbnvDb×⋅φ+Bδp+ACbnvDb⋅δKD,where
(14)A=01RM+h0secLRN+h00001,B=00vDNnRM+h2vDEnsecLtanLRN+h0vDEnsecLRN+h2000.


## 2.1. 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 right-hand 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(1)vDb=0vD0T,where vD is the speed of the vehicle measured by OD.The velocity in navigation frame can be expressed by(2)vDn=CbnvDb=vDEnvDNnvDUnT,where vDEn, vDNn, and vDUn are the components along the three axes of navigation frame.Thus, the position differential equations for DR are modeled as follows [10]:
(3)L̇=vDNnRM+h,λ̇=vDEnRN+h⋅cosL,ḣ=vDUn,where L, λ, and h are the longitude, latitude, and height, respectively, and RM is the prime radius of curvature while RN is the meridian radius of curvature.By assuming the updating period of OD isTD during which N samples are available, the iterative updating algorithm can be governed by [10]
(4)Li=Li−1+TDRM+hi−1⋅1N∑j=1NvDNnj,λi=λi−1+TDRN+hi−1⋅cosLi−1⋅1N∑j=1NvDEjnj,hi=hi−1+TDN∑j=1NvDUjnj.


## 2.2. 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
(5)φ̇=0.Then, the attitude matrix approximates to the following expression(6)C^bn=Cnn′Cbn=I‐φ×Cbn,where φ denotes the vector 00φ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 δKD 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
(7)δKD=δKDb+δKDm,δK̇Db=0,δK̇Dm=−1τδKDm+wDm,where δKDb denotes the random constant, δKDm is the noise with a correlation time τ, and wDm is a Gaussian noise.


### 2.2.2. Error Model for Dead Reckoning

Defining the velocity measurement error of OD in the body frame isδvDb, and then the velocity in the navigation frame can modeled as
(8)v^Dn=C^bnvDb+δvDb.Substituting (6) into (8) yields
(9)v^Dn=vDn−φ×vDn+CbnδvDb,and then the velocity error in navigation frame can be obtained
(10)δvDn=v^Dn−vDn=−φ×vDn+CbnδvDb.Substituting (1) into (10) produces
(11)δvDn=CbnvDb×φ+CbnvDb⋅δKD.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
(12)δL̇=1RM+hδvDNn−vDNnRM+h2δh,δλ̇=secLRM+hδvDEn+vDEn⋅secL⋅tanLRM+hδL+vDEn⋅secLRM+h2δh,δḣ=δvDUn.Denotingδp=δL,δλ,δhT, rephrasing (12) into matrix form, and substituting (11) into it yield
(13)δṗ=AδvDn+Bδp=ACbnvDb×⋅φ+Bδp+ACbnvDb⋅δKD,where
(14)A=01RM+h0secLRN+h00001,B=00vDNnRM+h2vDEnsecLtanLRN+h0vDEnsecLRN+h2000.


## 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
(5)φ̇=0.Then, the attitude matrix approximates to the following expression(6)C^bn=Cnn′Cbn=I‐φ×Cbn,where φ denotes the vector 00φ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 δKD 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
(7)δKD=δKDb+δKDm,δK̇Db=0,δK̇Dm=−1τδKDm+wDm,where δKDb denotes the random constant, δKDm is the noise with a correlation time τ, and wDm is a Gaussian noise.


## 2.2.2. Error Model for Dead Reckoning

Defining the velocity measurement error of OD in the body frame isδvDb, and then the velocity in the navigation frame can modeled as
(8)v^Dn=C^bnvDb+δvDb.Substituting (6) into (8) yields
(9)v^Dn=vDn−φ×vDn+CbnδvDb,and then the velocity error in navigation frame can be obtained
(10)δvDn=v^Dn−vDn=−φ×vDn+CbnδvDb.Substituting (1) into (10) produces
(11)δvDn=CbnvDb×φ+CbnvDb⋅δKD.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
(12)δL̇=1RM+hδvDNn−vDNnRM+h2δh,δλ̇=secLRM+hδvDEn+vDEn⋅secL⋅tanLRM+hδL+vDEn⋅secLRM+h2δh,δḣ=δvDUn.Denotingδp=δL,δλ,δhT, rephrasing (12) into matrix form, and substituting (11) into it yield
(13)δṗ=AδvDn+Bδp=ACbnvDb×⋅φ+Bδp+ACbnvDb⋅δKD,where
(14)A=01RM+h0secLRN+h00001,B=00vDNnRM+h2vDEnsecLtanLRN+h0vDEnsecLRN+h2000.


## 3. 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 δKD, 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.


### 3.1. Fine Alignment Method Based on Single Landmark Point

As depicted in Figure1, 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,
(15)δΔSh=ΔSh×φU3+ΔShδKD,where φU is the up-direction attitude error angle, and ΔSh is the horizontal projection of the true trajectory, φU3=0,0,φUT. 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.Figure 1
The similarity between DR trajectory and true trajectory.Then,φU can be easily derived based on the triangle geometry as follows:
(16)φU=arccosΔS~hΔShΔShΔS~h≈ΔS~hΔShΔShΔS~h.Because the horizontal attitude angel error is usually pretty small,φU approximates to the azimuth angle error, that is, δψ≈φU.Next, the scale factor error of OD can be achieved by using the true distance and the DR distance as follows:(17)δKD=ΔS~hΔSh−1.


### 3.2. Iterative Fine Alignment Algorithm Based on the Results from Map Matching

According to the results of map matching experiments in [14], a couple of position points can be obtained through map matching algorithm. These position points are uniformly distributed while the circular error probability is about 3 m. Thus, these map matching position points are utilized as a group of landmark points to estimate the azimuth angle error φ and the scale factor error δKD.As shown in Figure2, Pi is the ith truth position, and P~i is the output of DR. Pi′ or Pi′′ 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 Pi while other map-mapped points will lead to a larger azimuth than. Pi. 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
(18)δψi′=δψ+εi,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
(19)∑i=1nεi=0.Figure 2
Distribution of the azimuth error angle.Next, the following equation based onn measurements can be obtained
(20)δψ¯′=1n∑i=1nδψi′=1n∑i=1nδψ+εi=δψ¯.Surprisingly, the estimation mean of the azimuth angle error based onn map-mapped points is equal to the result based on one accurate landmark point.As a result, the estimation algorithm based on then map-mapped points can be obtained
(21)δψ≈φU=1n∑i=1nΔS~ihΔSihΔSihΔS~ih,(22)δKD≈1n∑i=1nΔS~ihΔSih−1.Till now, the estimation forδψ and δKD 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 δKD according to (21) and (22) when map-mapped points are available.
(iii)
Useδψ to modify the initial attitude matrix and use δKD 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 δKD again.
(v)
Turn to (iii).


## 3.1. Fine Alignment Method Based on Single Landmark Point

As depicted in Figure1, 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,
(15)δΔSh=ΔSh×φU3+ΔShδKD,where φU is the up-direction attitude error angle, and ΔSh is the horizontal projection of the true trajectory, φU3=0,0,φUT. 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.Figure 1
The similarity between DR trajectory and true trajectory.Then,φU can be easily derived based on the triangle geometry as follows:
(16)φU=arccosΔS~hΔShΔShΔS~h≈ΔS~hΔShΔShΔS~h.Because the horizontal attitude angel error is usually pretty small,φU approximates to the azimuth angle error, that is, δψ≈φU.Next, the scale factor error of OD can be achieved by using the true distance and the DR distance as follows:(17)δKD=ΔS~hΔSh−1.


## 3.2. Iterative Fine Alignment Algorithm Based on the Results from Map Matching

According to the results of map matching experiments in [14], a couple of position points can be obtained through map matching algorithm. These position points are uniformly distributed while the circular error probability is about 3 m. Thus, these map matching position points are utilized as a group of landmark points to estimate the azimuth angle error φ and the scale factor error δKD.As shown in Figure2, Pi is the ith truth position, and P~i is the output of DR. Pi′ or Pi′′ 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 Pi while other map-mapped points will lead to a larger azimuth than. Pi. 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
(18)δψi′=δψ+εi,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
(19)∑i=1nεi=0.Figure 2
Distribution of the azimuth error angle.Next, the following equation based onn measurements can be obtained
(20)δψ¯′=1n∑i=1nδψi′=1n∑i=1nδψ+εi=δψ¯.Surprisingly, the estimation mean of the azimuth angle error based onn map-mapped points is equal to the result based on one accurate landmark point.As a result, the estimation algorithm based on then map-mapped points can be obtained
(21)δψ≈φU=1n∑i=1nΔS~ihΔSihΔSihΔS~ih,(22)δKD≈1n∑i=1nΔS~ihΔSih−1.Till now, the estimation forδψ and δKD 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 δKD according to (21) and (22) when map-mapped points are available.
(iii)
Useδψ to modify the initial attitude matrix and use δKD 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 δKD again.
(v)
Turn to (iii).


## 4. 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 Table1. 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.Table 1
The motions of the vehicle.
Phase
Motion
Start time (s)
Period (s)
Angular acceleration (°/s)
Acceleration (m/s2)
Initial speed (m/s)
1
Station keeping
0
10
0
0
0
2
Uniform acceleration motion
10
10
0
2
0
3
Uniform motion
20
30
0
0
20
4
Turn left
50
20
−3
0
20
5
Uniform motion
70
100
0
0
20
6
Turn right
170
10
4
0
20
7
Uniform motion
180
90
0
0
20
8
Turn left
270
30
−0.5
0
20
9
Turn right
300
300
0.09
0
20Figure 3
Nominal trajectory of the vehicle.Figure 4
Nominal azimuth of the vehicle.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 is2×10−4g whilst the intensity of the random noise is 1×10−4g. 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 Table2, 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.Table 2
Simulation cases.
Cases
Position uncertainty
(1) Alignment with one landmark point
0 m
(2) Alignment with one landmark point
2 m
(3) Alignment with landmark points from MM
2 m
(4) Iterative alignment with landmark points from MM
2 mThe alignment results for case 1 are shown in Figure5. 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.Figure 5
Attitude angle error for case 1.The results for case 2 are presented in Figure6, where estimations for δK~D and δψ are 0.0021 and 25.898′, respectively. The estimation error of δK~D is about 22%. The after-correction 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.Figure 6
Attitude angle error for case 2.As shown in Figure7 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.Figure 7
Attitude angle error for case 3.As shown in Figure8 are the results for case 4. The algorithm executed in this case is presented at the end of Subsection 3.2. Firstly, do the same estimation as case 3 and then use the estimation to correct the initial azimuth of SINS and scale factor of OD. After that, use the corrected initial parameters to redo the DR process. Next, estimate δK~D and δψ again when it comes to the epoch 500 s. The final azimuth error is about 0.809′ (no more than 50″).Figure 8
Attitude angle error for case 4.Further, by comparing Figures5–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 angle. Above all, estimation error of the cases can be summarized in Table 3.Table 3
Estimation error forδψ and δKD.
δψ (angle min)
δKD
Case 1
4.634
26.7%
Case 2
4.905
22%
Case 3
4.680
23%
Case 4
0.809
0.31%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.


## 5. Conclusions

This paper developed a novel iterative fine alignment for in-motion 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.



---

*Source: 7368018-2018-07-31.xml*