Mathematical Problems in Engineering

Volume 2015 (2015), Article ID 903054, 6 pages

http://dx.doi.org/10.1155/2015/903054

## SINS/CNS Nonlinear Integrated Navigation Algorithm for Hypersonic Vehicle

^{1}College of Aerospace Engineering, Nanjing University of Aeronautics and Astronautics, Jiangsu, Nanjing 210016, China^{2}Navigation Research Center, Nanjing University of Aeronautics and Astronautics, Jiangsu, Nanjing 210016, China

Received 27 August 2014; Accepted 1 November 2014

Academic Editor: Bin Jiang

Copyright © 2015 Yong-jun Yu et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

Celestial Navigation System (CNS) has characteristics of accurate orientation and strong autonomy and has been widely used in Hypersonic Vehicle. Since the CNS location and orientation mainly depend upon the inertial reference that contains errors caused by gyro drifts and other error factors, traditional Strap-down Inertial Navigation System (SINS)/CNS positioning algorithm setting the position error between SINS and CNS as measurement is not effective. The model of altitude azimuth, platform error angles, and horizontal position is designed, and the SINS/CNS tightly integrated algorithm is designed, in which CNS altitude azimuth is set as measurement information. GPF (Gaussian particle filter) is introduced to solve the problem of nonlinear filtering. The results of simulation show that the precision of SINS/CNS algorithm which reaches 130 m using three stars is improved effectively.

#### 1. Introduction

Hypersonic Vehicle (HV) which refers to a vehicle flying at Mach 5 or above has already been the research focus in aeronautic and aerospace fields with its great strategic military application values [1, 2].

Although Hypersonic Vehicle has many advantages, such as large flight envelope, high maneuverability, and well penetrability, the dynamic model of an HV is fast time varying and highly nonlinear because of its Mach numbers [3]. Large-scale variations of altitude and velocity lead to uncertainties in the aerodynamic parameters [4, 5]. As a result, HV is a highly nonlinear and uncertain system. Consequently, it is difficult to measure or estimate the dynamic state and characteristics of the vehicle [6]. Autonomous navigation system with high accuracy and reliability has been a major constraint on the improvement in performance of HV.

In recent years, owing to the development of microelectronics and computer technology, as well as the accuracy improvement of Charge Coupled Device (CCD), Charge Inject Device (CID) star trackers, and inertial components, the Strap-down Inertial Navigation System (SINS) and Celestial Navigation System (CNS) are widely used in in aircrafts [7].

The celestial navigation method is a kind of autonomous navigation technology which can determine the vehicle’s position and attitude [8]. Since Celestial Navigation System (CNS) has characteristics of accurate orientation and strong autonomy, it has become an important component of integrated navigation system of HV [9].

The conventional celestial navigation utilizes the inertial navigation platform technology to realize the vertical vector and compute the vehicle’s navigation information by measuring the relative position changes between the vertical vector and the celestial vector. The navigation accuracy of this method depends largely on the accuracy of horizontal reference and celestial sensor measurements [10]. CNS usually gets the inertial horizon reference by inertial navigation platform. Considering that the strap-down type replacing the platform type has been the development trend of INS, it has become extremely difficult to improve the accuracy of the inertial horizon references due to the impact of INS core instruments (gyros and accelerometers) error [11].

In traditional SINS/CNS integrated mode, CNS utilizes the position and attitude information of INS to calculate celestial positions and heading attitude and then realize periodic correction of the INS drifts. This mode can damp the divergence of INS position errors; however, since the CNS location and orientation mainly depend upon the inertial reference that contains errors caused by gyro drifts and other error factors, this postcorrection method is not effective.

A number of classical approaches, the Kalman filter (KF), extended Kalman filter (EKF), unscented Kalman filter (UKF), particle filtering (PF), and so forth, have been proposed to the information fusion. Kalman filtering is commonly used algorithms for information integration. A prerequisite for using Kalman filter is that the system dynamics and noise statistics are known [12]. But considering the HV is a highly nonlinear system, the dynamic characteristics of the HV and external environment make these premise conditions often not met.

PF can effectively solve the problem of nonlinear filtering [13, 14]. However, its limitation is obvious.(1)PF occasionally has the particle impoverishment (PI) problem that results from resampling process [15].(2)The number of particles will increase at a rapid rate along with the increase of the system dimensions.

The Gaussian PF avoids the PI problem that is the disadvantageous feature of PF in the estimation of a static parameter. Furthermore, resampling process is not required in the GPF algorithm. Therefore, its computational complexity (CC) is significantly reduced compared to particle filtering.

This paper will carry out the research on the SINS/CNS integrated navigation algorithm for the HV. In order to improve the accuracy and reliability of SINS/CNS integrated systems, the scheme and algorithm of airborne SINS/CNS integrated navigation based on celestial angle observation have been presented. The theory of SINS/CNS integrated navigation system based on celestial altitude angle observation information has been discussed adequately; a model with celestial altitude angle, platform error angles, and horizontal position is deduced. Meanwhile, a new SINS/CNS tightly integrated localization algorithm using Gaussian particle filter (GPF) is presented, which makes full use of SINS and CNS navigation information to achieve higher accuracy of the SINS/CNS integration.

#### 2. SINS/CNS Tightly Integrated Navigation Model

##### 2.1. The Model of CNS Measurement

The basic principle of astronomical positioning is given bywhere is the observed altitude angle of the navigational stars in Local-Level Frame. , are longitude and latitude; , are the declination and Greenwich hour angle.

According to (1), using longitude and latitude information from SINS, the computed altitude angle in Local-Level Frame is received aswhere , are latitude and longitude from SINS, and

By using first order approximation of Taylor expansion, (3) can be expressed as

According to (1) and (4), the relation between computed altitude angle and observed altitude angle can be expressed as follows:where

Since CNS gets the inertial horizon reference by SINS, we can not get the observed altitude angle in Local-Level Frame in (1) but the observed altitude angle in Computational Frame.

The celestial vector in Local-Level Frame can be set as

The celestial vector in Computational-Level Frame can be set as

The relationship between Computational Frame and Local-Level Frame can be expressed as follows:where , and , , are east, north, and up platform angle error.

According to (5) and (10), the relationship between computed altitude angle in Local-Level Frame and the observed altitude angle in Computational Frame can be expressed as follows:

##### 2.2. Equation of State of SINS/CNS Integrated Algorithm

The navigation frame is chosen as east-north-upward geographic frame. The -- direction of the body frame is coincident with the Right-Front-Upward direction of the vehicle body.

The state equations are described as where is the state variables vector, and, , are single-order Markov errors of three gyros, , , are single-order Markov errors of three accelerometers, and

is the system matrix, is the system noise matrix, and .

The attitude equation in the form of a quaternion is written aswhere , the vehicle body angular rate measured by gyros in the body frame,

, and is the projection of the earth rotation rate in the navigation frame. is angular rate of the navigation frame with respect to the earth frame, expressed in the navigation frame:

The definition of the quaternion error between real quaternion and calculated quaternion iswhere , is calculated quaternion, and is real quaternion.

The dynamic vector equation of quaternion error can be presented aswhere , and

Dynamic vector equation of velocity errors can be expressed aswhere , and represents the specific force measured by the three accelerometers in the body frame.

Dynamic equation of position errors can be expressed aswhere , are the earth curvature radius.

From (11), the measurement equation with one navigational star can be set as

If there is more than one navigational star, the measurement equation can be set aswhere is the matrix that reflects the relations between and , is the noise vector of the measurement information, and

According to (12) and (24), the system equations are described as

#### 3. SINS/CNS Gaussian Particle Filter Algorithm

The GPF approximates the filtering and predictive densities by Gaussian distributions in a PF framework. The GPF recursively updates only the posterior mean and the covariance of the parameter of interest. The basic idea of PF is to represent a density by generated samples and their associated weights. The following shows the approximation of a filtering density by particles and their weights.

The equations of nonlinear system model are defined as follows:

GPF can be described as the following two steps.

*Measurement Update*. Generate the sample particles following where is the importance sampling function, , and is the particle number.

And the weights can be approximated by the following:where is the filtering density, and , are obtained by sample mean and covariance of the particles generated in the previous time update step.

Then compute the , following

*Time Update*. Generate and compute :

Then compute the , following

According to (26), the state equation is nonlinear and the measurement equation is linear; the measurement update of GPF can be estimated by Kalman filter. Thus, (29) and (30) can be simplified as follows:

The GPF flow chart for (26) is given as follows.

*Step 1. *Consider

*Step 2. *Consider

*Step 3. *Consider

*Step 4. *Consider

#### 4. Simulation and Analysis

The proposed algorithm is testified by the designed track. The drift of equivalent gyros in inertial navigation system is set to 0.1°/h, the drift of equivalent accelerometer is 10^{−4} g, and the period of SINS algorithm is 5 ms. The navigation stars are Alioth, Arcturus, and Dubhe. The error of star sensor is 10 arc-second, and the period of CNS is 1.0 s; the period of the GPF is 1.0 s.

The track is set as showed in Figure 1. The longitude is set as showed in Figure 2.