# Initial Alignment Error On-Line Identification Based on Adaptive Particle Swarm Optimization Algorithm

Authors: Weilin Guo; Yong Xian; Bing Li; Leliang Ren
Journal: Mathematical Problems in Engineering
 (2018)

---

## Abstract

To solve the problem of high accuracy initial alignment of strap-down inertial navigation system (SINS) for ballistic missile, an on-line identification method of initial alignment error based on adaptive particle swarm optimization (PSO) is proposed. Firstly, a complete navigation model of SINS is established to provide the accurate model basis for subsequent numerical optimization calculation. Then setting the initial alignment error as the optimization parameter and regarding the minimum deviation between SINS and GPS output as the objective function, the error parameter optimization model is designed. At the same time, the mutation idea of genetic algorithm (GA) is introduced into the PSO; thus the adaptive PSO is adopted to identify the initial alignment error on-line. The simulation results show that it is feasible to solve the initial alignment error identification problem of SINS by intelligent optimization algorithm. Compared with the standard PSO algorithm and the GA, the adaptive PSO algorithm has the fastest convergence speed and the highest convergence precision, and the initial pitch error and the initial yaw error precision are within10′′ and the initial azimuth error precision is within 25′′. The navigation accuracy of SINS is improved effectively. Finally, the feasibility of the adaptive PSO algorithm to identify the initial alignment error is further validated based on the test data.


---

## Body

## 1. Introduction

Initial alignment for strap-down inertial navigation system (SINS) plays an important role in the navigation operation of the ballistic missile. The main purpose of initial alignment is to establish the initial attitude matrix, and the quality of initial alignment will affect the navigation accuracy of SINS directly [1] and thus affect the missile firing accuracy ultimately. Therefore, improving the initial alignment accuracy of SINS is of great significance to improve the performance of ballistic missile weapon.The propagation process of the initial alignment error of SINS is a complex nonlinear problem. The previous solution is to linearize the nonlinear problem, and the filtering algorithms based on Kalman filter are widely adopted [2–6]. A fast SINS initial alignment scheme based on the disturbance observer and Kalman filter is proposed to estimate the misalignment angles in [4], and an adaptive extended Kalman filter algorithm combined with innovation-based adaptive estimation is proposed in [7], while these filtering algorithms often have some disadvantages, such as the difficulty of model establishing, poor observability of parameters, and long alignment time [8]. Consequently, this paper rejects the traditional research method based on analytic simplification, linearization and filtering, attempting to convert the initial alignment problem of SINS into parameter optimization identification problem. The complete nonlinear optimization model is established, and the intelligent optimization algorithm is used to realize the on-line identification of the initial alignment error of SINS.To solve the initial alignment problem of inertial system, the application of genetic algorithm (GA) in the initial alignment of SINS on the static base is studied based on the intelligent optimization algorithm [9, 10]. The precision of the initial alignment error is about 2′ in [9], and the alignment accuracy needs to be improved. At the same time, the GA has the disadvantages of large computational capacity, low efficiency, and complicated coding, while the particle swarm optimization (PSO) is simple in structure, fast in convergence, and easy to implement and has the advantage of dealing with complex systems. The transfer alignment between the master inertial sensor and the slave inertial sensor is realized by PSO, and the influence of the maneuver on alignment accuracy is analyzed in [11]. The PSO is applied into the parameter optimization of compass alignment circuit in SINS, and the performance of strap-down gyrocompass initial alignment is improved in [12]. In view of the above analysis and research results, the PSO algorithm is considered to solve the initial alignment problem of the ballistic missile SINS.In this paper, a complete SINS navigation model is established, and then the error parameter optimization model is constructed based on the minimum deviation between the position parameters outputted by SINS and the position parameters measured by GPS. The mutation idea of GA is introduced into PSO, and the inertial weight and learning factors are improved to obtain adaptive PSO. Finally, the standard PSO, GA, and adaptive PSO are adopted to identify the initial alignment error with the flight software of a certain type of ballistic missile. At the same time, the test data are used to inspect the identification effect of intelligent optimization algorithm.The rest of this paper is organized as follows. A navigation model of SINS, including initial alignment error model and error compensation model, is established in the second section. In the third section, an error parameter optimization model is constructed and the adaptive PSO is designed for the ballistic missile SINS. In the fourth section, the simulation for identification of initial alignment error is given to demonstrate the feasibility of the intelligent optimization algorithm. Finally, we conclude in the fifth section.


## 2. Establishment of SINS Navigation Model

The main coordinate frames used in this paper different from other references are defined as follows: the body coordinate of the ballistic missile is the orthogonal reference frame aligned with the inertial measurement unit (IMU) axes, and the origin locates the mass of the ballistic missile, the x-axis along the longitudinal direction forward, opposite the direction of gravity, the y-axis is perpendicular to the longitudinal direction upward, the z-axis along the transversal direction right, completing a right-handed system. Launch inertial coordinate (inertial coordinate) is a coordinate whose origin is the launch point, the x-axis points to target in the local level of launch point, and the y–axis is perpendicular to the launch point’s local level (upward) and constitutes the right-handed Cartesian coordinates with the axes of x, z. The inertial coordinate is used as the navigation frame.


### 2.1. Initial Alignment Error Model

The initial alignment errors of SINS, including initial pitch angle errorΔφ0, initial yaw angle error Δψ0, and initial azimuth error Δγ0, are caused by vertical degree, installation error, and aiming error of the ballistic missile. The initial attitude matrix A between the body coordinate and the inertial coordinate can be described as follows by using the quaternion (q0,q1,q2,q3):(1)A=q12+q02-q22-q322q1q2-q0q32q0q2+q1q32q1q2+q0q3q02+q22-q12-q322q2q3-q0q12q1q3-q0q22q0q1+q2q3q02+q32-q12-q22The initial values of the quaternion(q0,q1,q2,q3) are(2)q0=q00-γ02q20q1=q10+γ02q30q2=q20+γ02q00q3=q30-γ02q10where(3)q00=221-Δφ02q10=-ψ02q00q20=ψ02q30q30=221+Δφ02From (2) and (3), we can see that the initial alignment error will affect the initial values of the quaternion and thus affect the calculation precision of the initial attitude matrix A.


### 2.2. Error Compensation Model of SINS

During the flight course of ballistic missile, the inertial measurement unit (IMU) of SINS, including gyroscope and accelerometer, can measure apparent acceleration and angular velocity in real time and output the data in pulse form. The pulse outputs of the accelerometer and the gyroscope under a navigation cycle are(ΔNwx1,ΔNwy1,ΔNwz1) and (ΔNbx1,ΔNby1,ΔNbz1), respectively, in which the pulse number remains the same as the actual missile, and the outputs are integers.After the IMU sends the pulse signals to the onboard computer, the error compensation calculation is completed by the onboard computer in real time. The equations of the error compensation calculation of the apparent velocity increment and the angular increment under a navigation cycle in body coordinate are shown in (4) and (5), respectively [13]:(4)
Δ
W
x
b
0
Δ
W
y
b
0
Δ
W
z
b
0
=
Δ
N
w
x
1
-
K
0
x
K
1
x
Δ
N
w
y
1
-
K
0
y
K
1
y
Δ
N
w
z
1
-
K
0
z
K
1
z
Δ
W
x
b
Δ
W
y
b
Δ
W
z
b
=
Δ
W
x
b
0
Δ
W
y
b
0
Δ
W
z
b
0
-
0
E
x
y
Δ
W
x
10
E
x
z
Δ
W
x
10
(5)
Δ
θ
x
b
0
Δ
θ
y
b
0
Δ
θ
z
b
0
=
Δ
N
b
x
1
K
x
Δ
N
b
y
1
K
y
Δ
N
b
z
1
K
z
-
D
0
x
D
0
y
D
0
z
Δ
θ
x
b
Δ
θ
y
b
Δ
θ
z
b
=
Δ
θ
x
b
0
Δ
θ
y
b
0
Δ
θ
z
b
0
-
0
E
y
x
E
z
x
E
x
y
0
E
z
y
E
x
z
E
y
z
0
Δ
θ
x
b
0
Δ
θ
y
b
0
Δ
θ
z
b
0
-
D
1
x
D
2
x
D
3
x
D
1
y
D
2
y
D
3
y
D
1
z
D
2
z
D
3
z
Δ
W
x
1
Δ
W
y
1
Δ
W
z
1where Ki,K0i,K1i,D0i,D1i,D2i,D3i, and Eij are the tool error coefficient of IMU which are calibrated in the missile technical site. ΔWib0 and Δθib0 are the calculated intermediate values of the apparent velocity and angle increment, respectively. ΔWib and Δθib are the apparent velocity and the angle increment in body coordinate after the error compensation, respectively, where i,j=x,y,z represents the three directions of the x, y, and z axes.


### 2.3. Calculation of Velocity and Position in Inertial Coordinate

According to the error compensation model of SINS, the apparent velocity increment in body coordinate can be calculated. The apparent velocity increment in body coordinate is converted to inertial coordinate, and it can be presented as(6)ΔWxaΔWyaΔWza=AΔWxbΔWybΔWzbwhere ΔWxa, ΔWya, and ΔWza are the projections of apparent velocity increment in inertial coordinate and the matrix A is calculated by (1), where the calculation equation of the quaternion is as follows:(7)q0q1q2q3j=q0-q1-q2-q3q1q0-q3q2q2q3q0-q1q3-q2q1q0j-11-18Δθj212-148Δθj2Δθxb12-148Δθj2Δθyb12-148Δθj2ΔθzbjwhereΔθj=Δθxb2+Δθyb2+Δθzb2. According to (7), the quaternion is calculated by the recursion method which based on the value of the previous moment, recursion to get the quaternion of the current moment. The initial value of the quaternion can be calculated by (2).By integrating the apparent velocity increment in inertial coordinate, the recursive value of the velocity and position in inertial coordinate at any moment of the missile can be obtained, and the calculation equation is as follows:(8)vxavyavzaj=vxavyavzaj-1+ΔWxaΔWyaΔWzaj+gxagyagzajΔT2+gxagyagzaj-1ΔT2(9)xayazaj=xayazaj-1+vxavyavzaj-1ΔT+ΔWxaΔWyaΔWzaj+gxagyagzaj-1ΔTΔT2where vxa,vya,vza are the projections of velocity in inertial coordinate; xa,ya,za are the projections of position in inertial coordinate; gxa,gya,gza are the projections of gravity acceleration in inertial coordinate, which can be computed by the ellipsoid gravity acceleration model; ΔT is the navigation cycle.


## 2.1. Initial Alignment Error Model

The initial alignment errors of SINS, including initial pitch angle errorΔφ0, initial yaw angle error Δψ0, and initial azimuth error Δγ0, are caused by vertical degree, installation error, and aiming error of the ballistic missile. The initial attitude matrix A between the body coordinate and the inertial coordinate can be described as follows by using the quaternion (q0,q1,q2,q3):(1)A=q12+q02-q22-q322q1q2-q0q32q0q2+q1q32q1q2+q0q3q02+q22-q12-q322q2q3-q0q12q1q3-q0q22q0q1+q2q3q02+q32-q12-q22The initial values of the quaternion(q0,q1,q2,q3) are(2)q0=q00-γ02q20q1=q10+γ02q30q2=q20+γ02q00q3=q30-γ02q10where(3)q00=221-Δφ02q10=-ψ02q00q20=ψ02q30q30=221+Δφ02From (2) and (3), we can see that the initial alignment error will affect the initial values of the quaternion and thus affect the calculation precision of the initial attitude matrix A.


## 2.2. Error Compensation Model of SINS

During the flight course of ballistic missile, the inertial measurement unit (IMU) of SINS, including gyroscope and accelerometer, can measure apparent acceleration and angular velocity in real time and output the data in pulse form. The pulse outputs of the accelerometer and the gyroscope under a navigation cycle are(ΔNwx1,ΔNwy1,ΔNwz1) and (ΔNbx1,ΔNby1,ΔNbz1), respectively, in which the pulse number remains the same as the actual missile, and the outputs are integers.After the IMU sends the pulse signals to the onboard computer, the error compensation calculation is completed by the onboard computer in real time. The equations of the error compensation calculation of the apparent velocity increment and the angular increment under a navigation cycle in body coordinate are shown in (4) and (5), respectively [13]:(4)
Δ
W
x
b
0
Δ
W
y
b
0
Δ
W
z
b
0
=
Δ
N
w
x
1
-
K
0
x
K
1
x
Δ
N
w
y
1
-
K
0
y
K
1
y
Δ
N
w
z
1
-
K
0
z
K
1
z
Δ
W
x
b
Δ
W
y
b
Δ
W
z
b
=
Δ
W
x
b
0
Δ
W
y
b
0
Δ
W
z
b
0
-
0
E
x
y
Δ
W
x
10
E
x
z
Δ
W
x
10
(5)
Δ
θ
x
b
0
Δ
θ
y
b
0
Δ
θ
z
b
0
=
Δ
N
b
x
1
K
x
Δ
N
b
y
1
K
y
Δ
N
b
z
1
K
z
-
D
0
x
D
0
y
D
0
z
Δ
θ
x
b
Δ
θ
y
b
Δ
θ
z
b
=
Δ
θ
x
b
0
Δ
θ
y
b
0
Δ
θ
z
b
0
-
0
E
y
x
E
z
x
E
x
y
0
E
z
y
E
x
z
E
y
z
0
Δ
θ
x
b
0
Δ
θ
y
b
0
Δ
θ
z
b
0
-
D
1
x
D
2
x
D
3
x
D
1
y
D
2
y
D
3
y
D
1
z
D
2
z
D
3
z
Δ
W
x
1
Δ
W
y
1
Δ
W
z
1where Ki,K0i,K1i,D0i,D1i,D2i,D3i, and Eij are the tool error coefficient of IMU which are calibrated in the missile technical site. ΔWib0 and Δθib0 are the calculated intermediate values of the apparent velocity and angle increment, respectively. ΔWib and Δθib are the apparent velocity and the angle increment in body coordinate after the error compensation, respectively, where i,j=x,y,z represents the three directions of the x, y, and z axes.


## 2.3. Calculation of Velocity and Position in Inertial Coordinate

According to the error compensation model of SINS, the apparent velocity increment in body coordinate can be calculated. The apparent velocity increment in body coordinate is converted to inertial coordinate, and it can be presented as(6)ΔWxaΔWyaΔWza=AΔWxbΔWybΔWzbwhere ΔWxa, ΔWya, and ΔWza are the projections of apparent velocity increment in inertial coordinate and the matrix A is calculated by (1), where the calculation equation of the quaternion is as follows:(7)q0q1q2q3j=q0-q1-q2-q3q1q0-q3q2q2q3q0-q1q3-q2q1q0j-11-18Δθj212-148Δθj2Δθxb12-148Δθj2Δθyb12-148Δθj2ΔθzbjwhereΔθj=Δθxb2+Δθyb2+Δθzb2. According to (7), the quaternion is calculated by the recursion method which based on the value of the previous moment, recursion to get the quaternion of the current moment. The initial value of the quaternion can be calculated by (2).By integrating the apparent velocity increment in inertial coordinate, the recursive value of the velocity and position in inertial coordinate at any moment of the missile can be obtained, and the calculation equation is as follows:(8)vxavyavzaj=vxavyavzaj-1+ΔWxaΔWyaΔWzaj+gxagyagzajΔT2+gxagyagzaj-1ΔT2(9)xayazaj=xayazaj-1+vxavyavzaj-1ΔT+ΔWxaΔWyaΔWzaj+gxagyagzaj-1ΔTΔT2where vxa,vya,vza are the projections of velocity in inertial coordinate; xa,ya,za are the projections of position in inertial coordinate; gxa,gya,gza are the projections of gravity acceleration in inertial coordinate, which can be computed by the ellipsoid gravity acceleration model; ΔT is the navigation cycle.


## 3. Error Parameter Optimization Model and Algorithm Design

### 3.1. Establishment of Error Parameter Optimization Model

#### 3.1.1. Select the Optimization Variable

Set the initial alignment error of SINS as the optimization parameter; that is,(10)X=x1,x2,x3T=Δφ0,ψ0,γ0T


#### 3.1.2. Determine the Objective Function

Regard the minimum deviation between the position parameters outputted by SINS and the position parameters measured by GPS as the objective function, namely,(11)JX=min∑i=1Numδxi2+δyi2+δzi2δxi=xINSi-xGPSiδyi=yINSi-yGPSiδzi=zINSi-zGPSiwhere xINS(i), yINS(i), and zINS(i) are the position parameters outputted by SINS at the navigation cycle i, which can be calculated by (9); xGPS(i), yGPS(i), and zGPS(i) are the position parameters measured by GPS at the navigation cyclei; δx(i), δy(i), and δz(i) are the position deviations. Num is the number of navigation cycle used to optimize alignment, and Num·ΔT≤Ts, Ts is the total simulation test time.


### 3.2. Design of Adaptive Particle Swarm Optimization Algorithm

PSO is an intelligent optimization algorithm for finding optimal region of search spaces through the interaction of individuals in a swarm [14], and it has been widely applied in the fields of aeronautics and astronautics because of its advantages such as fast convergence, simple structure, and strong versatility [15–17]. However, the standard PSO algorithm has the disadvantage of premature convergence and low efficiency in optimization iteration. Consequently, the standard PSO is improved to the adaptive PSO, and the improved strategy is as follows:( 1 ) The mutation idea is introduced into PSO algorithm based on GA algorithm. Mutation operation is an important means to increase population diversity in GA algorithm, which can expand search space and avoid falling into local optimization. Therefore, the mutation idea is introduced into PSO algorithm, and through the mutation operation of particles, the population can jump out of the current local optimal position in foraging process and search for a larger space range. Thus, the global search ability is enhanced to overcome the shortcoming of premature convergence of PSO.( 2 ) The learning factors and inertia weight of PSO are designed as dynamic adjustment form to improve the convergence speed and overcome the disadvantage of low efficiency of PSO in the late optimization. By dynamically adjusting the values of the learning factors and inertia weight of PSO algorithm, it is ensured that PSO has strong global search ability at the initial stage of optimization and a fast search speed at the later stage of optimization.The basic optimization flow of initial alignment error parameter by adaptive PSO is shown in Figure1, and the specific steps for optimization calculations are as follows.Figure 1
Calculation flow chart of adaptive PSO.Step 1 (initialize the population).
Set particle population sizeN, maximum iterations M, maximum position xmax, and minimum position xmin=-xmax. The initial position and the initial velocity of the particles are randomly generated, and each particles fitness value of initialization population is calculated to determine the individual best and the global best of the particles.Step 2 (update particle swarm velocity and position).
The equation for calculating the velocity and position of particles is as follows:(12)vijk+1=wkvijk+r1c1kPijk-xijk+r2c2kPgjk-xijkxijk+1=xijk+vijk+1where xij(k) and vij(k) (j=1,2,3) are the position and velocity of thejth dimension of particlei at iterationk; w(k) is the inertia weight; r1 and r2 are the random numbers distributed in the range [0,1]; c1(k) and c2(k) are the learning factors; Pij(k) and Pgj(k) are the individual best and the global best at iterationk, respectively.
In order to enhance the global exploration ability in the early stage of optimization and improve the convergence speed at the later stage of optimization, the inertia weight is designed to be the dynamic adjustment mode. The inertial weight has a large value at the beginning, and the weight decreases with the increase of the number of iterations. Consequently, the inertia weightw(k) can be designed as(13)wk=wmax-kM2wmax-wmin
Learning factorsc1(k) and c2(k) are the factors which can control degree of self-learning and group learning of particles, respectively. The particle swarm needs a large self-learning ability to enhance the global search effect in the early stage of optimization, and it needs a large group learning ability to speed up the convergence speed in the late optimization. Therefore, with the increase of the number of iterations, learning factors c1(k) continues to decrease and learning factors c2(k) increases gradually and the learning factors can be described as(14)c1k=c1max-kM2c1max-c1minc2k=c2min+kM2c2max-c2minStep 3.
Calculate fitness value of population, and update individual best and global best. According to (11), the fitness value f(xi(k+1)) of particles is calculated at iteration k+1 and compared with the previous fitness value f(Pi(k)) of individual best. If f(xi(k+1))<f(Pi(k)), the individual best is updated, namely, Pi(k+1)=xi(k+1). Similarly, comparing the fitness value of the individual bestf(Pi(k+1)) at iteration k+1 with the previous fitness value of the global best f(Pg(k)), if f(Pi(k+1))<f(Pg(k)), the global best of particles is updated, namely, Pg(k+1)=Pi(k+1).Step 4.
Mutation operation and update the individual best and the global best again.
(A) Select Mutation Particles. The fitness values of particle swarm at iteration k+1 were sorted by descending order, and the numbers Nm of particles arranged in front and with large fitness value were selected as the mutation objects.
(B) Mutation Operation. First, generate the random number r, and then compare the sizes of the random number r and rm, where rm is the mutation probability of particle. If the generated random number r is less than rm, the mutation operation is performed as follows:(15)xijk=2r-1xmaxj-xminjwhere r is the random number distributed in the range [0,1] and λ is the mutation coefficient. Otherwise, go to Step 5 for the next calculation.
(C) Update Particle’s Best. For the new particles generated by mutation, update the individual best and the global best again.Step 5 (end condition).
If reaching maximum iterations, namely,k>M, or the search results satisfy the accuracy requirement, the calculation is stopped and the optimal parameter of initial alignment error is output. Otherwise, return Step 2 for the next generation calculation.


## 3.1. Establishment of Error Parameter Optimization Model

### 3.1.1. Select the Optimization Variable

Set the initial alignment error of SINS as the optimization parameter; that is,(10)X=x1,x2,x3T=Δφ0,ψ0,γ0T


### 3.1.2. Determine the Objective Function

Regard the minimum deviation between the position parameters outputted by SINS and the position parameters measured by GPS as the objective function, namely,(11)JX=min∑i=1Numδxi2+δyi2+δzi2δxi=xINSi-xGPSiδyi=yINSi-yGPSiδzi=zINSi-zGPSiwhere xINS(i), yINS(i), and zINS(i) are the position parameters outputted by SINS at the navigation cycle i, which can be calculated by (9); xGPS(i), yGPS(i), and zGPS(i) are the position parameters measured by GPS at the navigation cyclei; δx(i), δy(i), and δz(i) are the position deviations. Num is the number of navigation cycle used to optimize alignment, and Num·ΔT≤Ts, Ts is the total simulation test time.


## 3.1.1. Select the Optimization Variable

Set the initial alignment error of SINS as the optimization parameter; that is,(10)X=x1,x2,x3T=Δφ0,ψ0,γ0T


## 3.1.2. Determine the Objective Function

Regard the minimum deviation between the position parameters outputted by SINS and the position parameters measured by GPS as the objective function, namely,(11)JX=min∑i=1Numδxi2+δyi2+δzi2δxi=xINSi-xGPSiδyi=yINSi-yGPSiδzi=zINSi-zGPSiwhere xINS(i), yINS(i), and zINS(i) are the position parameters outputted by SINS at the navigation cycle i, which can be calculated by (9); xGPS(i), yGPS(i), and zGPS(i) are the position parameters measured by GPS at the navigation cyclei; δx(i), δy(i), and δz(i) are the position deviations. Num is the number of navigation cycle used to optimize alignment, and Num·ΔT≤Ts, Ts is the total simulation test time.


## 3.2. Design of Adaptive Particle Swarm Optimization Algorithm

PSO is an intelligent optimization algorithm for finding optimal region of search spaces through the interaction of individuals in a swarm [14], and it has been widely applied in the fields of aeronautics and astronautics because of its advantages such as fast convergence, simple structure, and strong versatility [15–17]. However, the standard PSO algorithm has the disadvantage of premature convergence and low efficiency in optimization iteration. Consequently, the standard PSO is improved to the adaptive PSO, and the improved strategy is as follows:( 1 ) The mutation idea is introduced into PSO algorithm based on GA algorithm. Mutation operation is an important means to increase population diversity in GA algorithm, which can expand search space and avoid falling into local optimization. Therefore, the mutation idea is introduced into PSO algorithm, and through the mutation operation of particles, the population can jump out of the current local optimal position in foraging process and search for a larger space range. Thus, the global search ability is enhanced to overcome the shortcoming of premature convergence of PSO.( 2 ) The learning factors and inertia weight of PSO are designed as dynamic adjustment form to improve the convergence speed and overcome the disadvantage of low efficiency of PSO in the late optimization. By dynamically adjusting the values of the learning factors and inertia weight of PSO algorithm, it is ensured that PSO has strong global search ability at the initial stage of optimization and a fast search speed at the later stage of optimization.The basic optimization flow of initial alignment error parameter by adaptive PSO is shown in Figure1, and the specific steps for optimization calculations are as follows.Figure 1
Calculation flow chart of adaptive PSO.Step 1 (initialize the population).
Set particle population sizeN, maximum iterations M, maximum position xmax, and minimum position xmin=-xmax. The initial position and the initial velocity of the particles are randomly generated, and each particles fitness value of initialization population is calculated to determine the individual best and the global best of the particles.Step 2 (update particle swarm velocity and position).
The equation for calculating the velocity and position of particles is as follows:(12)vijk+1=wkvijk+r1c1kPijk-xijk+r2c2kPgjk-xijkxijk+1=xijk+vijk+1where xij(k) and vij(k) (j=1,2,3) are the position and velocity of thejth dimension of particlei at iterationk; w(k) is the inertia weight; r1 and r2 are the random numbers distributed in the range [0,1]; c1(k) and c2(k) are the learning factors; Pij(k) and Pgj(k) are the individual best and the global best at iterationk, respectively.
In order to enhance the global exploration ability in the early stage of optimization and improve the convergence speed at the later stage of optimization, the inertia weight is designed to be the dynamic adjustment mode. The inertial weight has a large value at the beginning, and the weight decreases with the increase of the number of iterations. Consequently, the inertia weightw(k) can be designed as(13)wk=wmax-kM2wmax-wmin
Learning factorsc1(k) and c2(k) are the factors which can control degree of self-learning and group learning of particles, respectively. The particle swarm needs a large self-learning ability to enhance the global search effect in the early stage of optimization, and it needs a large group learning ability to speed up the convergence speed in the late optimization. Therefore, with the increase of the number of iterations, learning factors c1(k) continues to decrease and learning factors c2(k) increases gradually and the learning factors can be described as(14)c1k=c1max-kM2c1max-c1minc2k=c2min+kM2c2max-c2minStep 3.
Calculate fitness value of population, and update individual best and global best. According to (11), the fitness value f(xi(k+1)) of particles is calculated at iteration k+1 and compared with the previous fitness value f(Pi(k)) of individual best. If f(xi(k+1))<f(Pi(k)), the individual best is updated, namely, Pi(k+1)=xi(k+1). Similarly, comparing the fitness value of the individual bestf(Pi(k+1)) at iteration k+1 with the previous fitness value of the global best f(Pg(k)), if f(Pi(k+1))<f(Pg(k)), the global best of particles is updated, namely, Pg(k+1)=Pi(k+1).Step 4.
Mutation operation and update the individual best and the global best again.
(A) Select Mutation Particles. The fitness values of particle swarm at iteration k+1 were sorted by descending order, and the numbers Nm of particles arranged in front and with large fitness value were selected as the mutation objects.
(B) Mutation Operation. First, generate the random number r, and then compare the sizes of the random number r and rm, where rm is the mutation probability of particle. If the generated random number r is less than rm, the mutation operation is performed as follows:(15)xijk=2r-1xmaxj-xminjwhere r is the random number distributed in the range [0,1] and λ is the mutation coefficient. Otherwise, go to Step 5 for the next calculation.
(C) Update Particle’s Best. For the new particles generated by mutation, update the individual best and the global best again.Step 5 (end condition).
If reaching maximum iterations, namely,k>M, or the search results satisfy the accuracy requirement, the calculation is stopped and the optimal parameter of initial alignment error is output. Otherwise, return Step 2 for the next generation calculation.


## 4. Simulation Experiment and Result Analysis

The simulation study is presented to confirm the feasibility of initial alignment error on-line identification based on adaptive PSO in this section. Firstly, the simulation conditions are set up, then the simulation experiment and results are analyzed. At last, the test data is introduced.


### 4.1. Simulation Condition Settings

The simulation conditions of the initial alignment error optimization model for SINS are set as follows:( 1 ) Setting initial pitch angle error is 90′′, initial yaw angle error is -90′′ and initial azimuth error is -150′′. The navigation cycle ΔT is 0.1 s, the number of navigation cycles Num is 500, and the total simulation test time Ts is 50 s.( 2 ) Tool error of SINS. The constant and random drifts of gyro are chosen as 0.02°/h and 0.01°/h/Hz, respectively. The constant and random drifts of accelerators are chosen as 100ug and 50ug/Hz, respectively.( 3 ) GPS navigation error. The position error is 5.0 m, and the velocity error is 0.1 m/s.( 4 ) Simulation conditions of PSO algorithm. The maximum iterations M is 100, the population size N is 40, and the maximum position xmax=300300500. The inertia weights wmax and wmin are 0.8 and 0.4, respectively. The learning factors c1max and c2max are all 3.0, and the learning factors c1min and c2min are all 0.5. The mutation numbers of particles Nm are 10, the mutation probability rm is 0.2, and the mutation coefficient λ is 0.5.


### 4.2. Simulation Results and Analyses

Based on the above error parameter optimization model and taking the flight software of a certain ballistic missile as the simulation experiment environment, the standard PSO algorithm, the GA, and the adaptive PSO algorithm are adopted to optimize the initial alignment error parameter of SINS, and the convergence process of the initial pitch angle error, the initial yaw angle error, and the initial azimuth angle error are shown in Figures2–4, and the fitness convergence diagrams of the three algorithms are shown in Figure 5. The optimization results of initial alignment error are shown in Table 1. It shows the accuracy and convergence speed of three different algorithms.Table 1
Optimization results of initial alignment error.
Optimization algorithm Parameter Initial alignment error Calculation time/(s) Δ φ 0/(′′) Δ ψ 0/(′′) Δ γ 0/(′′) GA optimization Optimization value 92.42 -108.67 -122.62 3.85 Residual error 2.42 -18.67 27.38 Standard PSO Optimization value 91.03 -102.35 -124.53 3.46 Residual error 1.03 -12.35 25.47 Adaptive PSO Optimization value 90.51 -99.29 -127.56 3.48 Residual error 0.51 -9.29 22.44Figure 2
Convergence process of initial pitch angle error.Figure 3
Convergence process of initial yaw angle error.Figure 4
Convergence process of initial azimuth angle error.Figure 5
Fitness convergence diagram.It is shown from Table1 that the GA, the standard PSO, and the adaptive PSO can be used to optimize the initial alignment error parameter; the maximum residual error of the initial alignment error of three algorithms is not more than 30′′, the optimization calculation time is less than 4.0 s, and the convergence speed is fast. It shows that it is feasible and effective in identifying the initial alignment error of SINS by using intelligent optimization algorithm.From Figures2–5, we can see that the adaptive PSO has a fastest convergence speed than the GA and the standard PSO. It is shown from Table 1 that the residual errors of initial pitch angle and the initial yaw angle calculated by the adaptive PSO are less than 10′′, and the residual error of the initial azimuth is less than 25′′, which shows that the adaptive PSO can improve the convergence accuracy of the initial alignment error.The initial alignment error parameter calculated by the adaptive PSO is compensated, and then the navigation parameters of the SINS are recalculated. The deviation between the navigation parameters obtained by error compensation and the actual navigation parameters of the missile is called the optimization residual. By simulation, the SINS position error, GPS position error and optimization residual of position are shown in Figure6, and the SINS velocity error, GPS velocity error, and optimization residual of velocity are shown in Figure 7. The root mean square (RMS) statistic results of navigation parameter error are listed in Table 2.Table 2
RMS statistical results of navigation parameter errors.
Error type Navigation parameter Pure SINS error Compensation error Position error (m) x a 6.9748 0.1730 y a 2.4885 0.3683 z a 10.7109 0.0960 Velocity error (m/s) v a x 0.3748 0.0147 v a y 0.1790 0.0306 v a z 0.6336 0.0044Figure 6
Position error simulation curve.Figure 7
Velocity error simulation curve.From Figures6 and 7, we can find out that the optimization residuals of position and velocity are not only far less than SINS, but also significantly less than GPS navigation system. It is shown from Table 2 that the RMS position errors of SINS are within 10 m, and the RMS velocity errors of SINS are within 1 m/s while the RMS position errors are less than 1 m, and the RMS velocity errors are less than 0.1 m/s, after the initial alignment error is identified and compensated by the adaptive PSO algorithm. Obviously, compared with SINS, the RMS errors of the navigation parameters compensated by the adaptive PSO are reduced by 10 times, and the navigation accuracy is greatly improved. The simulation results indicate that the adaptive PSO can effectively identify the initial alignment error and improve the navigation accuracy of SINS.


### 4.3. Test Data Validation

In order to verify the effectiveness of adaptive PSO algorithm to identify initial alignment error, the data collected from the test are analyzed. Among them, the gyroscope constant drift and the accelerometer constant bias are about 0.01°/h and 100μg, respectively; SINS data update frequency is 10.0HZ; GPS data update frequency is 1.0Hz. The actual measured output values of SINS and GPS were collected and recorded during the test. The deviation between the position and the speed of SINS and GPS output is called the position error and the velocity error of SINS, then the position error and the velocity error curve of SINS are shown in Figures 8 and 9, respectively.Figure 8
Position error curve of SINS.Figure 9
Velocity error curve of SINS.Based on the above test data, the adaptive PSO algorithm is used to optimize the initial alignment error parameters, and the results of the optimization parameters are shown in Table3.Table 3
Results of optimization parameters.
Optimization parameter Lowerbound Upperbound Optimization value Δ φ 0/(′′) -90.00 90.00 90.00 Δ ψ 0/(′′) -90.00 90.00 73.46 Δ γ 0/(′′) -180.00 180.00 -104.04The initial alignment error calculated by the adaptive PSO is compensated, and then the navigation parameters of SINS are recalculated. After calculation and compensation, the position and velocity error curves are shown in Figures10 and 11 respectively, and the error RMS statistic results are shown in Table 4.Table 4
RMS statistical results of errors.
Error type Navigation parameter Initialerror Optimization error Position error (m) x a 6.2535 5.4109 y a 10.3145 8.2356 z a 8.6935 7.5713 Velocity error (m/s) v a x 0.4784 0.2770 v a y 0.7956 0.6541 v a z 0.2769 0.2540Figure 10
Position optimization error curve.Figure 11
Velocity optimization error curve.From Figures10 and 11, we can see that the optimization errors of the position and velocity are less than the initial errors of the position and velocity of the test data, after the initial alignment error is compensated. It is shown from Table 4 that the RMS errors of the position and velocity of optimization calculation are less than the RMS errors of the test data. Therefore, it is proved that the adaptive PSO algorithm is effective in identifying the initial alignment error and can improve the navigation accuracy of missile flight.


## 4.1. Simulation Condition Settings

The simulation conditions of the initial alignment error optimization model for SINS are set as follows:( 1 ) Setting initial pitch angle error is 90′′, initial yaw angle error is -90′′ and initial azimuth error is -150′′. The navigation cycle ΔT is 0.1 s, the number of navigation cycles Num is 500, and the total simulation test time Ts is 50 s.( 2 ) Tool error of SINS. The constant and random drifts of gyro are chosen as 0.02°/h and 0.01°/h/Hz, respectively. The constant and random drifts of accelerators are chosen as 100ug and 50ug/Hz, respectively.( 3 ) GPS navigation error. The position error is 5.0 m, and the velocity error is 0.1 m/s.( 4 ) Simulation conditions of PSO algorithm. The maximum iterations M is 100, the population size N is 40, and the maximum position xmax=300300500. The inertia weights wmax and wmin are 0.8 and 0.4, respectively. The learning factors c1max and c2max are all 3.0, and the learning factors c1min and c2min are all 0.5. The mutation numbers of particles Nm are 10, the mutation probability rm is 0.2, and the mutation coefficient λ is 0.5.


## 4.2. Simulation Results and Analyses

Based on the above error parameter optimization model and taking the flight software of a certain ballistic missile as the simulation experiment environment, the standard PSO algorithm, the GA, and the adaptive PSO algorithm are adopted to optimize the initial alignment error parameter of SINS, and the convergence process of the initial pitch angle error, the initial yaw angle error, and the initial azimuth angle error are shown in Figures2–4, and the fitness convergence diagrams of the three algorithms are shown in Figure 5. The optimization results of initial alignment error are shown in Table 1. It shows the accuracy and convergence speed of three different algorithms.Table 1
Optimization results of initial alignment error.
Optimization algorithm Parameter Initial alignment error Calculation time/(s) Δ φ 0/(′′) Δ ψ 0/(′′) Δ γ 0/(′′) GA optimization Optimization value 92.42 -108.67 -122.62 3.85 Residual error 2.42 -18.67 27.38 Standard PSO Optimization value 91.03 -102.35 -124.53 3.46 Residual error 1.03 -12.35 25.47 Adaptive PSO Optimization value 90.51 -99.29 -127.56 3.48 Residual error 0.51 -9.29 22.44Figure 2
Convergence process of initial pitch angle error.Figure 3
Convergence process of initial yaw angle error.Figure 4
Convergence process of initial azimuth angle error.Figure 5
Fitness convergence diagram.It is shown from Table1 that the GA, the standard PSO, and the adaptive PSO can be used to optimize the initial alignment error parameter; the maximum residual error of the initial alignment error of three algorithms is not more than 30′′, the optimization calculation time is less than 4.0 s, and the convergence speed is fast. It shows that it is feasible and effective in identifying the initial alignment error of SINS by using intelligent optimization algorithm.From Figures2–5, we can see that the adaptive PSO has a fastest convergence speed than the GA and the standard PSO. It is shown from Table 1 that the residual errors of initial pitch angle and the initial yaw angle calculated by the adaptive PSO are less than 10′′, and the residual error of the initial azimuth is less than 25′′, which shows that the adaptive PSO can improve the convergence accuracy of the initial alignment error.The initial alignment error parameter calculated by the adaptive PSO is compensated, and then the navigation parameters of the SINS are recalculated. The deviation between the navigation parameters obtained by error compensation and the actual navigation parameters of the missile is called the optimization residual. By simulation, the SINS position error, GPS position error and optimization residual of position are shown in Figure6, and the SINS velocity error, GPS velocity error, and optimization residual of velocity are shown in Figure 7. The root mean square (RMS) statistic results of navigation parameter error are listed in Table 2.Table 2
RMS statistical results of navigation parameter errors.
Error type Navigation parameter Pure SINS error Compensation error Position error (m) x a 6.9748 0.1730 y a 2.4885 0.3683 z a 10.7109 0.0960 Velocity error (m/s) v a x 0.3748 0.0147 v a y 0.1790 0.0306 v a z 0.6336 0.0044Figure 6
Position error simulation curve.Figure 7
Velocity error simulation curve.From Figures6 and 7, we can find out that the optimization residuals of position and velocity are not only far less than SINS, but also significantly less than GPS navigation system. It is shown from Table 2 that the RMS position errors of SINS are within 10 m, and the RMS velocity errors of SINS are within 1 m/s while the RMS position errors are less than 1 m, and the RMS velocity errors are less than 0.1 m/s, after the initial alignment error is identified and compensated by the adaptive PSO algorithm. Obviously, compared with SINS, the RMS errors of the navigation parameters compensated by the adaptive PSO are reduced by 10 times, and the navigation accuracy is greatly improved. The simulation results indicate that the adaptive PSO can effectively identify the initial alignment error and improve the navigation accuracy of SINS.


## 4.3. Test Data Validation

In order to verify the effectiveness of adaptive PSO algorithm to identify initial alignment error, the data collected from the test are analyzed. Among them, the gyroscope constant drift and the accelerometer constant bias are about 0.01°/h and 100μg, respectively; SINS data update frequency is 10.0HZ; GPS data update frequency is 1.0Hz. The actual measured output values of SINS and GPS were collected and recorded during the test. The deviation between the position and the speed of SINS and GPS output is called the position error and the velocity error of SINS, then the position error and the velocity error curve of SINS are shown in Figures 8 and 9, respectively.Figure 8
Position error curve of SINS.Figure 9
Velocity error curve of SINS.Based on the above test data, the adaptive PSO algorithm is used to optimize the initial alignment error parameters, and the results of the optimization parameters are shown in Table3.Table 3
Results of optimization parameters.
Optimization parameter Lowerbound Upperbound Optimization value Δ φ 0/(′′) -90.00 90.00 90.00 Δ ψ 0/(′′) -90.00 90.00 73.46 Δ γ 0/(′′) -180.00 180.00 -104.04The initial alignment error calculated by the adaptive PSO is compensated, and then the navigation parameters of SINS are recalculated. After calculation and compensation, the position and velocity error curves are shown in Figures10 and 11 respectively, and the error RMS statistic results are shown in Table 4.Table 4
RMS statistical results of errors.
Error type Navigation parameter Initialerror Optimization error Position error (m) x a 6.2535 5.4109 y a 10.3145 8.2356 z a 8.6935 7.5713 Velocity error (m/s) v a x 0.4784 0.2770 v a y 0.7956 0.6541 v a z 0.2769 0.2540Figure 10
Position optimization error curve.Figure 11
Velocity optimization error curve.From Figures10 and 11, we can see that the optimization errors of the position and velocity are less than the initial errors of the position and velocity of the test data, after the initial alignment error is compensated. It is shown from Table 4 that the RMS errors of the position and velocity of optimization calculation are less than the RMS errors of the test data. Therefore, it is proved that the adaptive PSO algorithm is effective in identifying the initial alignment error and can improve the navigation accuracy of missile flight.


## 5. Conclusion

The initial alignment error identification of ballistic missile SINS is studied in this paper. The real and complete navigation model of SINS is established, which provides an accurate model basis for the initial alignment error identification. At the same time, the error parameter optimization model is designed, and the initial alignment error is identified on-line by the intelligent optimization algorithm. What is more, the inertia weight and learning factors of PSO are designed as dynamic adjustment form to improve search speed and search accuracy, and the mutation operation of the GA is introduced into the PSO algorithm to jump out local optimal value and enhance global convergence ability.The simulation results show that the intelligent optimization algorithm is efficient to solve the problem of initial alignment error identification. Of course, the results show that the adaptive PSO algorithm has fastest search efficiency and highest convergence accuracy than the standard PSO algorithm and the GA, and the residuals of the initial pitch angle and the initial yaw angle are less than 10′′, and the residual of the initial azimuth is less than 25′′. Finally, the validity of the adaptive PSO algorithm to identify the initial alignment error is validated based on the test data. Therefore, the content of this paper has a certain reference value for the improvement of the initial alignment accuracy of the ballistic missile SINS.



---

*Source: 3486492-2018-12-31.xml*