An Improved Coarse Alignment Algorithm for Odometer-Aided SINS Based on the Optimization Design Method.

An improved coarse alignment (ICA) algorithm is proposed in this paper with a focus on improving alignment accuracy of odometer-aided strapdown inertial navigation system (SINS) under variable velocity and variable acceleration condition. In the proposed algorithm, the outputs of inertial sensors and odometer in a sampling interval are linearized rather than assumed to be a constant, which improves the accuracy of the vector observations and the precision of coarse alignment. Simulation and field test results illustrate that, under variable velocity and variable acceleration condition, the proposed algorithm can obtain a better alignment performance than conventional coarse alignment method.

1. Introduction

[-15]Strapdown inertial navigation system (SINS) can autonomously, continuously and comprehensively provide the position, velocity, and attitude of the carrier [1,2,3,4]. The performance of SINS depends on the accuracy and rapidity of the initial alignment process, which can be divided into coarse alignment and fine alignment [5,6]. Coarse alignment is important since it provides a rapidly alignment result for the fine alignment. The existing algorithms of the coarse alignment mainly include: analytic coarse alignment [7,8,9], inertial frame coarse alignment (IFCA) [10,11], and coarse alignment based on Davenport’s q method [12,13,14,15,16]. The analytic coarse alignment can only be used on static base. In order to solve moving base coarse alignment problem, some IFCA algorithms have been proposed. In [13], Li et al. proposes a fast SINS initial alignment method based on IFCA with the aid of external reference velocity. However, this algorithm has the problem of large random errors. In [15,17,18], the optimization-based alignment (OBA) method with the aid of external reference information provided by Global Navigation Satellite System (GNSS) is proposed. The OBA algorithm obtains optimal attitude matrix through the q method to reduce random errors of attitude angles, however, these algorithms in [15,17] are not suitable for odometer-aided SINS, since the odometer can only provide the velocity in body frame. To solve this problem, the OBA algorithm with the aid of external velocity provided by odometer is reported in [19,20,21]. However, the outputs of inertial sensors and odometer in one sampling interval are assumed to be a constant, which may result in errors of coarse alignment when the velocity and acceleration change.

In order to improve the alignment performance under changeable velocity and acceleration, an improved coarse alignment (ICA) algorithm for SINS aided by odometer is proposed in this paper. The main contribution is that the outputs of inertial sensors and odometer that are assumed to be a constant in a sampling interval in [15,17,19] are replaced by their linear approximations to improve the accuracy of the integral formulae of vector observations. Experimental results show that the proposed ICA algorithm has better accuracy than the OBA algorithm and the traditional IFCA algorithm especially when the velocity and acceleration change, which is more suitable for coarse alignment of odometer-aided SINS.

This paper is organized as follows. Section 2 introduces the OBA algorithm for odometer-aided SINS. Section 3 derives the proposed ICA algorithm with linear approximation of inertial sensors and odometer. Simulation results and field test results are reported in Section 4. Finally, conclusions are drawn in Section 5.

2. OBA Algorithm for Odometer-Aided SINS

The coarse alignment aims at determining a coordinate transformation matrix in a short time, where n denotes the local level navigation frame and b denotes the body frame, respectively. In this paper, we denote by i the inertial frame and e the earth frame. The kinematic equations of navigation are known as [1,15,19] where denotes the ground velocity in the navigation frame, is the earth rotation angular velocity, denotes the angular rate of the navigation fame with respect to the earth frame, and is the specific force measured by accelerometers. is the body angular rate measured by gyroscope, and represents the angular rate of the body frame with respect to the navigation frame. Moreover, denotes the local gravity acceleration, and denotes the cross product matrix. According to the chain rule, the attitude transformation matrix can be decomposed as where and denote the change of body frame and navigation frame from time 0 to , respectively. They are calculated by the following differential equations where denotes the angular rate of the navigation frame with respect to the inertial frame.

After obtaining the attitude matrixes and by Equations (4) and (5), the key problem to determine is to obtain the constant matrix . The specific force Equation (2) is used as the measurement equation in the OBA algorithm with the aid of velocity in navigation frame, such as GNSS, and it can be rewritten as follows if the velocity is provided in body frame, such as the odometer

Substituting (3) into (6) and integrating the specific force equation from time interval 0 to t, we have

Equation (7) can also be rewritten in a compact form as where

The next step is to calculate the vector observations and . In [15,17,19], the output of accelerometer , the output of gyroscope and the output of odometer are all assumed to be constants in a sampling interval, which may result in errors in when velocity and acceleration are changing. Next we will propose an ICA algorithm by linearizing these outputs in sampling intervals.

3. ICA Algorithm for Odometer-Aided SINS

In the following, T represents the time period of the update interval , , and the current time is , where M is the sampling number. Ignoring the change of gravity acceleration , the vector observation is written as

Since the angular rate of navigation frame with respect to inertial frame changes slowly, can be approximated as [15] where denotes the navigation frame rotation vector from to , and is approximated as

Substituting Equations (12) and (13) into Equation (11) yields

The first integral term in Equation (9) can be written as

In order to improve the accuracy of the vector observations, we make a linear approximation for the outputs and of SINS. Suppose that changes linearly during the period of and can be approximated as

Then and can be calculated by using Equations (17) and (18)

Similarly, can be approximated as a linear form where

Substituting Equations (16) and (19) into Equation (15), we obtain

The output of the odometer contained in the second integral in (9) can also be approximated as

Then and can be calculated through Equations (24) and (25)

The term is calculated as follows

Substituting Equations (19), (23) and (26) into Equation (9), the second integral term in (9) can be approximated as

The vector observation is abbreviated as

Then the discrete measurement equation can be rewritten in a compact form

After obtaining the vector observations, the measurement Equation (29) can be solved by the method. The constant matrix can be formulated by its corresponding quaternion , subject to where s is scalar part and is the vector part. The vector observations can be written in the form of quaternion

It is convenient to make Equation (29) equivalent to , where ∘ denotes the quaternion multiplication and is the conjugate quaternion of . The quaternion multiplication matrix is defined as

Then we have the equivalent measurement equation [12,15]

Thus, the attitude quaternion can be determined by solving the following optimization problem where

It can be proved that J in Equation (35) will be minimized if is chosen to be the eigenvector corresponding to the smallest eigenvalue of , which is the optimal quaternion related to [15,18]. The block diagram of the ICA algorithm is shown in Figure 1. In Figure 1, LA denotes the process of linear approximation for the outputs of inertial sensors and odometer, AUb and AUn denote update processes of attitude matrix and attitude matrix , respectively, and the q method is used to calculate the attitude matrix . According to the chain rule, the attitude matrix can be updated in real time by Equation (3). In conclusion, the attitude matrix can be calculated through the ICA algorithm in real time by Equations (3)–(5) and (29).

Figure 1

The block diagram of the proposed ICA algorithm.

4. Simulation and Test

4.1. Simulation Results

We first give simulation results under variable velocity and acceleration condition for coarse alignment of odometer-aided SINS using the proposed ICA algorithm. In order to verify the performance of the ICA algorithm, the in-motion model with severe maneuvering was built. The swing motion are set as sine functions where , and are the amplitudes and set as , , and , respectively. , and are swing periods, which are set as , and , respectively. , and are initial phases and they are random values in the range from 0 radian to radian, , and are initial attitude angles and they are random values in the range from to . The initial parameters of the simulation are shown in Table 1. As is shown in Table 1, the random error of the gyroscope and accelerometer are set as Gaussian white noise, and the standard deviation of the gyroscope noise and accelerometer noise are and , respectively.

Table 1

Initial parameters.

Parameter Name	Parameter Value
Initial longitude λ	120^∘E
Initial latitude L	30^∘N
Fiber-optic gyroscope drift	0.005^∘/h
Fiber-optic gyroscope noise	0.05^∘/h/HZ
Accelerometer bias	10^−4g
Accelerometer noise	10^−4g/Hz
odometer noise	0.02m/s

In order to simulate the condition of variable velocity and variable acceleration, the velocity of the vehicle is set as a sine-cosine function in the navigation frame, and the velocity of the vehicle is shown in Figure 2.

Figure 2

The true velocity profile of the simulation test.

The first simulation lasts about 150 s under the condition of variable velocity and variable acceleration, and the simulation results of the OBA algorithm and the proposed ICA algorithm are shown in Figure 3. Figure 3 shows that the errors of the roll angle and pitch angle are all less than after 10 s, and the heading error obtained by the proposed ICA algorithm is less than after 80 s. Compared with the OBA algorithm, the heading angle calculated by the ICA algorithm can obtain more accurate results with faster convergence speed.

Figure 3

The attitude angle error of the simulation.

To further show the advantage of the proposed algorithm, 50 coarse alignment trials are performed. Each coarse alignment lasts 100 s, and the final attitude angles are chosen as the alignment results. Figure 4 gives the results of the three kinds of typical coarse alignment algorithms. The black line denotes the results of the proposed ICA algorithm, the green line denotes the results of the OBA algorithm aided by odometer [19], and the red line denotes the results of the traditional IFCA method (TIFCA) aided by odometer [13]. The statistics of the attitude angle errors through three algorithms are shown in Table 2.

Figure 4

The attitude angle errors of 50 coarse alignments. The abscissa denotes the order of the alignments, and the ordinate denotes the errors of the pitch angle, roll angle and heading angle, respectively.

Table 2

The simulation result of 50 coarse alignments.

Attitude Angle	Method	Maximum Error	Mean Error	Standard Deviation
Pitch	TIFCA	0.0157^∘	0.0002^∘	0.0066^∘
	OBA	−0.0145^∘	−0.0021^∘	0.0068^∘
	ICA	0.0103^∘	−0.00006^∘	0.0055^∘
Roll	TIFCA	−0.0333^∘	0.0018^∘	0.0161^∘
	OBA	0.0156^∘	−0.0015^∘	0.0087^∘
	ICA	0.0140^∘	0.0016^∘	0.0067^∘
Heading	TIFCA	−5.391^∘	0.2396^∘	2.3105^∘
	OBA	4.984^∘	0.5555^∘	2.8391^∘
	ICA	−1.67^∘	−0.1393^∘	0.6220^∘

Figure 4 and Table 2 show that the errors of the level attitude angle, pitch angle and roll angle, are all less than . However, the TIFCA algorithm and the OBA algorithm have a poor performance on heading alignment, and we can see from Table 2 that the heading error of the two algorithms are more than and standard deviation is greater than . The heading error of the proposed ICA algorithm is less than at 100 s, and the standard deviation reduces to . The standard deviation of the heading angel error adopting ICA algorithm is obviously smaller than the other two algorithms. Both pitch and roll angles can be accurate and quickly obtained by all algorithms, and there is no big difference in the results. The difficulty of coarse alignment is the estimation of the heading angle, especially on variable velocity and variable acceleration condition. As is shown in simulation results, compared with the TIFCA algorithm and OBA algorithm, the proposed algorithm has a better alignment performance under the condition of variable velocity and variable acceleration.

4.2. Test Results

In order to verify the validity of the proposed ICA algorithm in practice, we carried out a field test to verify the performance of the ICA algorithm. As is shown in Figure 5, the self-made fiber-optic-SINS (FSINS) and photonics inertial navigation system (PHINS) are installed together on a reference platform inside the car, and the GPS antenna is installed outside on the top of the car. The position of the experiment is and . The initial position for each coarse alignment is provided by GPS, and the ground velocity in the body frame is provided by the odometer. The specific force and the body angler rate are provided by FSINS, and the FSINS is equipped with gyroscopes (drift , noise ) and accelerometers (bias , noise ). The attitude angles provided by the GPS/PHINS integrated navigation system are used as the attitude reference, and the specifications of GPS/PHINS integrated navigation system are listed in Table 3. The car was moving severely with the change of velocity and acceleration. We carried out four coarse alignments, and each alignment data segment lasts 100 s with the feature of variable velocity and variable acceleration. Figure 6 shows the velocity changes of four segments.

Figure 5

The FSINS and PHINS for the experiment.

Table 3

Specifications of the GPS/PHINS integrated navigation system.

Index	Accuracy
Heading, Roll, Pitch resolution	0.001^∘
Heading, Roll, Pitch dynamic accuracy	0.01^∘
Saturation of the speed	41.66m/s
Speed accuracy	0.1m/s
The position error of GPS receiver	≤10 m

Figure 6

Velocity profile for four segments.

The coarse alignment results of three algorithms are listed in Table 4, Table 5 and Table 6, in which final attitude angle errors of four coarse alignments are defined as the differences between final attitude angles of three algorithms and the outputs of the reference. As is shown in Table 4 and Table 5, the level attitude errors of three algorithms all reduce to values less than in , which fulfill the accuracy requirement of coarse alignment. Table 6 shows the final heading angle errors of four segments. As is shown in Table 6, the heading angle errors of the four segments obtained by the proposed ICA algorithm are all smaller than the other two algorithms. In conclusion, under the variable velocity and variable acceleration condition, the ICA algorithm has a better performance in coarse alignment of odometer-aided SINS, and the test results coincide with the simulation results.

Table 4

The final pitch angle error of four segments.

Data	TIFCA	OBA	ICA
Segment 1	0.008^∘	−0.006^∘	−0.006^∘
Segment 2	0.009^∘	−0.007^∘	−0.003^∘
Segment 3	−0.004^∘	0.004^∘	0.003^∘
Segment 4	0.013^∘	0.013^∘	0.011^∘

Table 5

The final roll angle error of four segments.

Data	TIFCA	OBA	ICA
Segment 1	−0.035^∘	−0.033^∘	−0.035^∘
Segment 2	−0.012^∘	−0.011^∘	−0.014^∘
Segment 3	−0.015^∘	−0.008^∘	−0.009^∘
Segment 4	0.036^∘	0.038^∘	0.038^∘

Table 6

The final heading angle error of four segments.

Data	TIFCA	OBA	ICA
Segment 1	−1.117^∘	−0.677^∘	−0.065^∘
Segment 2	1.376^∘	1.024^∘	0.210^∘
Segment 3	−2.029^∘	−1.698^∘	−0.685^∘
Segment 4	−1.498^∘	−2.354^∘	0.944^∘

5. Conclusions

To overcome the performance degradation of the existing TIFCA and OBA algorithms under variable velocity and variable acceleration conditions, a novel improved coarse alignment algorithm for odometer-aided SINS is proposed in this paper. By constructing the vector observations with a linear approximation of sensors’ outputs, the proposed algorithm is able to obtain better accuracy than existing TIFCA and OBA algorithms. Simulation results and field tests verified the performance that the proposed algorithm can obtain smaller heading angle errors, which is more suitable for odometer-aided SINS coarse alignment.