IEEE/CAA Journal of Automatica Sinica  2018, Vol. 5 Issue(6): 1113-1120 PDF
A Filtering Approach Based on MMAE for a SINS/CNS Integrated Navigation System
Fangfang Zhao1, Cuiqiao Chen1, Wei He2, Shuzhi Sam Ge3
1. School of Computer Science and Engineering, and Center for Robotics, University of Electronic Science and Technology of China, Chengdu 611731, China;
2. School of Automation and Electrical Engineering, University of Science and Technology of Beijing, Beijing 100083, China;
3. Social Robotics Laboratory, Interactive Digital Media Institute, and the Department of Electrical and Computer Engineering, National University of Singapore, Singapore 117576, Singapore
Abstract: This paper explores multiple model adaptive estimation (MMAE) method, and with it, proposes a novel filtering algorithm. The proposed algorithm is an improved Kalman filter-multiple model adaptive estimation unscented Kalman filter (MMAE-UKF) rather than conventional Kalman filter methods, like the extended Kalman filter (EKF) and the unscented Kalman filter (UKF). UKF is used as a subfilter to obtain the system state estimate in the MMAE method. Single model filter has poor adaptability with uncertain or unknown system parameters, which the improved filtering method can overcome. Meanwhile, this algorithm is used for integrated navigation system of strapdown inertial navigation system (SINS) and celestial navigation system (CNS) by a ballistic missile's motion. The simulation results indicate that the proposed filtering algorithm has better navigation precision, can achieve optimal estimation of system state, and can be more flexible at the cost of increased computational burden.
Ⅰ. INTRODUCTION

For this navigation system, filtering algorithm is a significant factor affecting navigation performance, which is frequently used to estimate state and filter measurement. In the information fusion, Kalman filter (KF) is a basic algorithm aiming at acquiring the attitude, velocity, and position information of the target. Improved filtering methods, such as EKF and UKF, have been developed based on the standard Kalman filter [4].

It is difficult to gain the optimal navigation estimation solely relying on KF due to the nonlinearity of the missile's model, and both the EKF and UKF are suitable for nonlinear system [5]. EKF works by utilizing a first-order Taylor series expansion to approximate linear system, then processing equations according to linear equations. However, linearization may cause the filter's divergence and poor performance when the system has strong nonlinearity. Julier and Uhlman provide the theory of UKF, which effectively solves the problem related to EKF based on the assumption that the state variables' distribution approximates a Gaussian distribution [6], [7]. Essentially, UKF adopts an unscented transformation (UT) strategy and provides a higher order moment of function without the need to linearize the nonlinear equations or compute the Jacobian matrix, as required in EKF. Therefore, UKF has no higher-order terms truncation error and can achieve higher precision compared with EKF for nonlinear system [8]. Under the hypothesis that the process and measurement noise of EKF and UKF are a Gaussian distribution with zero-mean, we end up with the problem of filter tuning. This problem is time-consuming and taxing. Fortunately, there are many available tools to aid the filter design. A popular approach to resolve this problem is to adopt an adaptive filter.

Mehra offered four distinct categories such as Bayes estimate, covariance matching, correlation, and maximum likelihood (ML) for the adaptive filter. The procedures used to implement the adaptive Kalman filter are classified into three main methods: innovation-based adaptive estimator (IAE), multiple model adaptive estimate (MMAE), and adaptive fading Kalman filter (AFKF) in [9]. MMAE is an important aspect in multi-model adaptive control, which is appropriate for systems with uncertain parameters or variable structure [10], [11]. The central idea of multiple model (MM) was put forward by Magill in 1956. This algorithm has become a mainstream approach to study estimation problems in hybrid systems. Meanwhile, the interactive multiple model (IMM) algorithm and static multiple model algorithm are most widely used in what. In MMAE method, the state estimation of the system is obtained by the weighted sum of each subfilter's estimation with the likelihood function of unknown elements. Thus, this method can adapt to the systems with uncertain or unknown parameters [12].

The contributions of this paper are to propose the MMAE-UKF method and apply this proposed filtering approach to a SINS/CNS combined navigation system. At the same time, we carry out a simulation with motion of ballistic missile and make comparisons among EKF, MMAE-EKF, UKF, and MMAE-UKF algorithms. Simulation results are provided to indicate that MMAE-UKF can obtain the highest navigation precision.

The organization of the remainder of this paper is shown below. Multiple model adaptive estimation method is presented in Section Ⅱ. Section Ⅲ describes the MMAE-UKF algorithm in detail. In Section Ⅳ, the ballistic missile motion and integrated navigation is specified. Then, Section Ⅴ gives the simulation conditions and results. In the end, conclusions are drawn in Section Ⅵ.

This section primarily researches the multiple model adaptive estimate algorithm, which is composed of a set of parallel filters and a hypothesis testing. In this filter bank, each filter has a specific model of the system which is expressed by independent vector parameters. Then, each filter forms estimated value of the current system state according to the input vector and self-model independently. Making use of the current state estimate to calculate predicted measurement, the residual error is obtained through subtraction of predicted measurement value from true value [13]. Then, based on the true vector parameters and measurement, hypothesis testing applies the residual error to compute the conditional probability of each model, and the obtained probability is applied to measure the estimation accuracy of each filter state. In the end, we obtain the optimal estimate of the system state with the state estimation of each model multiplied by the corresponding weights [14]. The MMAE architecture is demonstrated in Fig. 1.

A. Bayesian Estimate

Multiple model adaptive estimation is a recursive estimator, which utilizes a group of filters with unknown elements. The elements are the parameters of process noise covariance matrix, indicated by the vector ${\pmb p}$. A group of distributed values are produced from the probability distribution function about ${\pmb p}$, given by $p({\pmb p})$, and ${\{{{\pmb p}}^{ (l)}; l = 1, 2, \ldots, M}\}$ where $M$ is the sub-filter's number. The aim of the filter estimate is to decide conditional probability of the $l${th} element ${{\pmb p}}^{ (l)}$, which is provided the current-time measurement $\widetilde{{\pmb y}} _{k}$. The application of Bayesian law is shown as follows [15]:

 \begin{align} p\left({\pmb p}^{(l)}|\widetilde{{\pmb Y}}_{k}\right)= \dfrac{p\left(\widetilde{{\pmb Y}}_{k}|{\pmb p}^{(l)}\right) p\left({\pmb p}^{(l)}\right)}{\sum\limits_{j=1}^{M}p\left(\widetilde{{\pmb Y}}_{k}|{\pmb p}^{(j)}\right) p\left({\pmb p}^{(j)}\right)} \end{align} (1)

where $\widetilde{{\pmb Y}}_{k}$ indicates the set of ${\{\widetilde{{\pmb y}}_{0}, \widetilde{{\pmb y}}_{1}, \ldots, \widetilde{{\pmb y}}_{k}}\}$.

And the posteriori probability density function calculated by

 \begin{align}\label{eq.2} p\left({\pmb p}^{(l)}|\widetilde{{\pmb Y}}_{k}\right) &= \frac{p\left(\widetilde{{\pmb y}}_{k}, {\pmb p}^{(l)}|\widetilde{{\pmb Y}}_{k-1}\right)} {p\left(\widetilde{{\pmb y}}_{k}|\widetilde{{\pmb Y}}_{k-1}\right)} \nonumber \\[2mm] &= \frac{p(\widetilde{{\pmb Y}}_{k}| \widehat{{\pmb x}}^{-(l)}_{k})p\left({\pmb p}^{(l)}|\widetilde{{\pmb Y}}_{k-1}\right)} {\sum\limits_{j=1}^{M}\left[p\left(\widetilde{{\pmb Y}}_{k}|\widehat{{\pmb x}}^{-(j)}_{k}\right) p\left({\pmb p}^{(j)}|\widetilde{{\pmb Y}}_{k-1}\right)\right]} \end{align} (2)

where $p(\widetilde{{\pmb Y}}_{k}, {\pmb p}^{(l)}|\widetilde{{\pmb y}}_{k-1})$ is denoted by $p(\widetilde{{\pmb y}}_{k}|\widehat{{\pmb x}}^{-(l)}_{k})$ in the recursion. It is noteworthy that the denominator of (2) is simply a normalization component to guarantee that $p({\pmb p}^{(l)}|\widetilde{{\pmb Y}}_{k})$ is a probability density function. Thus, the recursion formula is thrown into a group of weight $\varpi^{(l)}_{k}$, which is defined as

 \begin{align}\label{eq.3} \varpi^{(l)}_{k} \,&=\varpi^{(l)}_{k-1}p\left(\widetilde{{\pmb y}}_{k}| \widehat{{\pmb x}}^{-(l)}_{k}\right) \nonumber\\[2mm] \varpi^{(l)}_{k}&\leftarrow \frac{\varpi^{(l)}_{k}}{\sum\limits_{j=1}^{M} \varpi^{(j)}_{k}} \end{align} (3)

where $\varpi^{(l)}_{k}= p({\pmb p}^{(l)}| \widetilde{{\pmb y}}_{k})$. At time $t_{0}$, the weight can be initialized as $\varpi^{(l)}_{0} = 1 / M$.

The conditional mean estimation is the sum of parallel filters estimations by weighting as below:

 \begin{align} \widehat{{\pmb x}}^{-}_{k} = \sum\limits_{l=1}^{M}\varpi^{(l)}_{k}\widehat{ {\pmb x}}^{-(l)}_{k}. \end{align} (4)

Meanwhile, the state estimations' error covariance is calculated through

 \begin{align} {pmb P}^{-}_{k} = \sum\limits_{l=1}^{M}\varpi^{(l)}_{k} \left(\widehat{{\pmb x}}^{-(l)}_{k} - \widehat{{\pmb x}}^{-}_{k}\right) \left(\widehat{{\pmb x}}^{-(l)}_{k} - \widehat{{\pmb x}}^{-}_{k}\right)^{T}. \end{align} (5)
B. Likelihood Equation

The likelihood function for the residual of measurement is constructed in this part. Primarily, the residual error is given by

 \begin{align} \epsilon_{i} \equiv \left[ \begin{array}{c} {\pmb e}_{k}\\ {\pmb e}_{k-1}\\ \vdots\\ {\pmb e}_{k-i} \end{array}\right] \end{align} (6)

where ${\pmb e}_{k} =\widetilde{{\pmb y}}_{k} - {{\pmb H}}_{k}\widehat{{\pmb x}}_{k}$. The likelihood equation that is correlated with $\epsilon_{i}$ is denoted as

 \begin{align} L_{i} = \frac{1}{\left[\det(2\pi {{\pmb C}}_{i})\right]^{\frac{1}{2}}} \exp \left(-\frac{1}{2}\epsilon_{i} ^{T}{{\pmb C}}_{i}^{-1} \epsilon_{i}\right) \end{align} (7)

where ${{\pmb C}}_{i} \equiv E\left\{\epsilon_{i}\epsilon_{i} ^{T}\right\}$ and $E\{\cdot\}$ denotes expectation.

When $i = 0$, $\epsilon_{0}={\pmb e}_{k}$, ${{\pmb C}}_{0} = {{\pmb H}}_{k}{{\pmb P}}_{k}^{-}{{\pmb H}}_{k}^{T}+{{\pmb R}}_{k}$. Therefore, the likelihood equation can be expressed by

 \begin{align} L_{0} = &\ \frac{1}{\left\{\det\left[2\pi({{\pmb H}}_{k} {{\pmb P}}_{k}^{-}{{\pmb H}}_{k}^{T} + {{\pmb R}}_{k})\right]\right\}^{\frac{1}{2}}} \nonumber\\[2mm] &\, \times \exp \left[-\frac{1}{2}{\pmb e}_{k}^{T}(H_{k}{{\pmb P}}_{k}^{-}H_{k}^{T} + {{\pmb R}}_{k})^{-1}{\pmb e}_{k}\right]. \end{align} (8)

This part presents the adaptive laws. The current measurements are just applied to update law given by (3) in standard MMAE method. Thus, the update law on account of the likelihood function is obtained through

 \begin{align} & \varpi^{(l)}_{k} = \varpi^{(l)}_{k-1}L^{(l)}_{0} \nonumber\\[2mm] & \varpi^{(l)}_{k} \leftarrow \frac{\varpi^{(l)}_{k}} {\sum\limits_{j=1}^{M}\varpi^{(j)}_{k}} \end{align} (9)

since $p(\widetilde{{\pmb y}}_{k}| \widehat{{\pmb x}}^{-(l)}_{k}) = L^{(l)}_{0}$, and

 \begin{align} L_{0}^{(l)} = &\ \frac{1}{\left\{\det\left[2\pi({{\pmb H}}_{k} {{\pmb P}}_{k}^{-(l)}{{\pmb H}}_{k}^{T} + {{\pmb R}}_{k})\right]\right\}^{\frac{1}{2}}} \nonumber\\[2mm] &\, \times \exp \left[-\frac{1}{2}{\pmb e}_{k}^{(l)T}({{\pmb H}}_{k} {{\pmb P}}_{k}^{-(l)}{{\pmb H}}_{k}^{T} + {{\pmb R}}_{k})^{-1}{\pmb e}_{k}^{(l)}\right]. \end{align} (10)
Ⅲ. MMAE-UKF ALGORITHM

In what follows, we consider the form of the discrete nonlinear system model

 \begin{align}\label{eq.11} \begin{cases} {{\pmb x}}_{k} = f({{\pmb x}}_{k-1}, {{\pmb u}}_{k-1}, {{\pmb w}}_{k-1})\\[1mm] {{\pmb z}}_{k} = h({{\pmb x}}_{k}, {{\pmb v}}_k) \end{cases} \end{align} (11)

where ${{\pmb x}}_{k} \in \mathbb{R}^{n}$ denotes state vector of system, ${{\pmb u}}_{k}\in\mathbb{R}^{m}$ is input of the known control, ${{\pmb z}}_{k} \in\mathbb{R}^{q}$ is the measurement, ${{\pmb w}}_{k}\sim {\rm N}(0, {{\pmb Q}}_{k})$ is the noise sequence of process, and ${{\pmb v}}_{k}$ $\sim$ ${\rm N}(0, {{\pmb R}}_{k})$ is noise of measurement. The vectors ${{\pmb w}}_{k}$ and ${{\pmb v}}_{k}$ are mutually independent and zero-mean additive noise sequence with covariance matrices ${{\pmb Q}}_{k}$ and ${{\pmb R}}_{k}$, respectively. The statistical properties of initial condition ${{\pmb x}}_{0}$ of (11) are denoted by the mean and variance [16].

In order to resolve this problem of nonlinearity and non-Gaussian, the UKF algorithm based on unscented transformation (UT) is provided to improve the EKF algorithm. UT adopts sigma symmetric sampling method, which needs $2n$ $+$ $1$ sigma points. The core of UKF algorithm is to select a group of sigma points near the $\hat{{{\pmb X}}}(k|k)$, $\hat{{{\pmb X}}}(k|k)$ and ${{\pmb P}}(k|k)$ indicate the mean and covariance of the sigma points, respectively. Under the hypothesis that the dimension of the state variables is $n \times 1$, $2n+1$ sample points are obtained

 \begin{align} &\chi_{0, k} = \hat{{{\pmb x}}}_k \end{align} (12)
 $\chi_{i, k} = \hat{{{\pmb x}}}_k +\sqrt{(\lambda+n)}(\sqrt{{{\pmb P}}(k|k)})_i, \\ \qquad\qquad\qquad\qquad\qquad\quad i=1, 2, \ldots, n$ (13)
 $\chi_{i, k} =\hat{{{\pmb x}}}_k -\sqrt{(\lambda+n)}(\sqrt{({{\pmb P}}(k|k)})_i, \\ \qquad\qquad\qquad\qquad\qquad\quad i=n+1, 2, \ldots, 2n$ (14)

where $\lambda=\alpha^{2}(n+\kappa)-n$ is an adjustment factor and $\alpha>0$ is a scale factor.

First, the basic UKF algorithm is detailed as follows:

1) Initialize:

 \begin{align*} \hat{{{\pmb x}}}_{0}=E[{{\pmb x}}_{0}], \quad {{\pmb P}}_{0}=E[({{\pmb x}}_{0}-\hat{{{\pmb x}}}_{0})({{\pmb x}}_{0}-\hat{{{\pmb x}}}_{0})^{T}]. \end{align*}

2) Calculate sigma points:

 \begin{align} \chi_{k-1} = [\chi_{0, k-1} ~~\chi_{1, k-1}~~ \ldots ~~\chi_{2n, k-1}]. \end{align} (15)

3) Time update:

a) Sigma points spread by the system equation are

 $\chi_{k|k-1} = \ f(\chi_{k-1}, k-1)$ (16)
 $\hat{{{\pmb x}}}_{k|k-1} = \ \sum\limits^{2n}_{i=0} w_{i}^{(m)}\chi_{i, k|k-1}$ (17)
 ${{\pmb P}}_{k|k-1} = \ \sum\limits^{2n}_{i=0}w_{i}^{(c)}[\chi_{i, k|k-1}-\hat{{{\pmb x}}}_{k|k-1}]\nonumber\\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\ \left[\chi_{i, k|k-1}-\hat{{{\pmb x}}}_{k|k-1}\right]^{T}+{{\pmb Q}}_{k}.$ (18)

b) Sigma points propagated by the measurement equation are

 ${{\pmb z}}_{k|k-1} = h(\chi_{k|k-1}, k)$ (19)
 $\hat{{{\pmb z}}}_{k|k-1} = \sum\limits^{2n}_{i=0} w_{i}^{(m)}{{\pmb z}}_{i, k|k-1}.$ (20)

4) Measurement update:

a) Estimate covariance of the prediction measure

 ${{\pmb P}}_{\hat{z}_{k}\hat{z}_{k}} = \ \sum\limits^{2n}_{i=0} w_{i}^{(c)}\left[{{\pmb z}}_{i, k|k-1}-\hat{{{\pmb z}}}_{k|k-1}\right]\\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\; \, \times \left[{{\pmb z}}_{i, k|k-1}-\hat{{{\pmb z}}}_{k|k-1}\right]^{T}+{{\pmb R}}_{k}$ (21)
 ${{\pmb P}}_{\hat{x}_{k}\hat{z}_{k}} = \ \sum\limits^{2n}_{i=0} w_{i}^{(c)}\left[\chi_{i, k|k-1}-\hat{{{\pmb x}}}_{k|k-1}\right]\\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\; \, \times\left[{{\pmb z}}_{i, k|k-1}-\hat{{{\pmb z}}}_{k|k-1}\right]^{T}$ (22)

where $w_{0}^{(m)}={\lambda}/{(n+\lambda)}$, $w_{0}^{(c)}={\lambda}/{(n+\lambda)}+(1-\alpha^{2}+\beta)$, $w_{i}^{(m)}=w_{i}^{(c)}={1}{(2(n+\lambda))}$, the function of $\beta \geq$ 0 is to change $w_{0}^{(c)}$ and adjusting the value of $\beta$ can improve the accuracy of variance.

b) Compute filter gain and covariance correction

 ${{\pmb K}}_{k} = {{\pmb P}}_{\hat{x}_{k}\hat{z}_{k}}{{\pmb P}}_{\hat{z}_{k}\hat{z}_{k}}^{-1}$ (23)
 $\hat{{{\pmb x}}}_{k} = \hat{{{\pmb x}}}_{k|k-1} + {{\pmb K}}_{k}(\textbf{z}_{k}-\hat{\textbf{z}}_{k|k-1})$ (24)
 ${{\pmb P}}_{k} = {{\pmb P}}_{k|k-1} - {{\pmb K}}_{k}{{\pmb P}}_{\hat{z}_{k}\hat{z}_{k}} {{\pmb K}}_{k}^{T}.$ (25)

Then, the MMAE-UKF algorithm is summarized as follows:

1) Initialization:

 \begin{align*} \hat{{{\pmb x}}}_{0}=E[{{\pmb x}}_{0}], ~~~{{\pmb P}}_{0}=E\left[({{\pmb x}}_{0}-\hat{{{\pmb x}}}_{0})({{\pmb x}}_{0}-\hat{{{\pmb x}}}_{0})^{T}\right]. \end{align*}

2) Estimate with UKF:

a) Calculation of sigma points;

b) Time update;

c) Measurement update.

3) Calculate likelihood equation using multiple model adaptive estimation

 \begin{align} L_{0}^{(l)} = &\ \frac{1}{\left\{\det\left[2\pi({{\pmb H}}_{k} \hat{{{\pmb P}}}_{k}^{(l)}{{\pmb H}}_{k}^{T} + {{\pmb R}}_{k})\right]\right\}^{\frac{1}{2}}} \nonumber\\[2mm] &\, \times \exp \left[-\frac{1}{2}{\pmb e}_{k}^{(l)T}({{\pmb H}}_{k} \hat{{{\pmb P}}}_{k}^{(l)}{{\pmb H}}_{k}^{T} + {{\pmb R}}_{k})^{-1}{\pmb e}_{k}^{(l)}\right]. \end{align} (26)

Evaluate weights and normalize the importance weight to be a normalization constant

 $\varpi^{(l)}_{k} = \, \varpi^{(l)}_{k-1}L^{(l)}_{0}$ (27)
 $\varpi^{(l)}_{k} \leftarrow \frac{\varpi^{(l)}_{k}} {\sum_{j=1}^{M}\varpi^{(j)}_{k}}$ (28)

for $l=1, 2, \ldots, M$.

 $\hat{{\pmb x}}_{k} = \sum\limits_{l=1}^{M}\varpi^{(l)}_{k}\hat{{{\pmb x}}}^{(l)}_{k}$ (29)

4) Output

 ${\pmb p}_{k} = \sum\limits_{l=1}^{M}\varpi^{(l)}_{k}(\hat{{{\pmb x}}}^{(l)}_{k} - \hat{{\pmb x}}_{k})(\hat{{{\pmb x}}}^{(l)}_{k} - \hat{{\pmb x}}_{k})^{T}.$ (30)
Ⅳ. State and Measurement Model A. Dynamics of Ballistic Missile

The movement of the missile can be divided into three phases, including the vertical rising part of the boost stage, the turning part of the boost stage and the final flameout stage [17]. Under the assumption that ballistic missile is a particle and the earth is a fixed sphere, then the nonlinear dynamics model of the missile is shown as

 \begin{align}\begin{cases} \dot{v} = \frac{P-X_{d}}{m}-g\sin\theta \\[1mm] \dot{x} = v\cos\theta \\ \dot{y} = v\sin\theta \end{cases} \end{align} (31)

where $P$ is thrust force, $X_{d}$ is air drag, and $g$ is gravitational acceleration. And, $g=g_{0}({R_{m}}/{r})^{2}$, $r=\sqrt{x^{2}+(R_{m}+y)^{2}}$.

 \begin{align}\label{eq31} \theta = \begin{cases} \theta_{0},&t \leq t_{1} \\[1mm] \frac{(\theta_{0}-\theta_{k})(t_{2}-t)^{2}}{(t_{2}-t_{1})^{2}+ \theta_{k}},&t_{1} < t \leq t_{2} \\[1mm] \theta_{k},&t_{2} < t \leq t_{k}. \end{cases} \end{align} (32)

where $\theta$ is the angle of pitch about the launching point under the gravity coordinate system, $\theta_{0}$ is angle of pitch when launching, and $\theta_{k}$ shows the angle of pitch at the end of the boost stage of turning.

To avert the coordinate transform as a result of the earth's rotation, the gravity coordinate system at the launch site is turned into inertial coordinate system. Then, the position of the ballistic missile is obtained by making use of the velocity's first-order integration under the inertial coordinate system at the launch site, and the derivative of velocity is taken to obtain the absolute acceleration of ballistic missile. The accelerometer's error is modeled as the first-order Markov process [18]. Because of the relative equation (33) between absolute acceleration and apparent acceleration $\alpha_{ib}$, we can gain the accelerometer output through the conversion of $\alpha$ into the coordinate system of the ballistic missile and the addition of the first-order Markov process. Meanwhile, the gyroscope output is obtained through adding the Gaussian noise to attitude angular velocity in the missile coordinate system.

 \begin{align}\label{eq.32} \dot{v}=\alpha + g. \end{align} (33)

The accelerometer output is:

 \begin{align} \alpha i_{ib}= {{\pmb B}}^{b}_{ib}(\alpha_{ib})={{\pmb B}}^{b}_{ib}(\dot{v}-g) \end{align} (34)

where ${{\pmb B}}^{b}_{ib}$ is the transition matrix of relevant coordinate.

After the first-order calculation, the gyroscope gains the attitude angular rate denoted by $\dot{\varphi}$, $\dot{\psi}$, $\dot{\gamma}$, then the velocity of attitude angle is given by

 \begin{align} wi_{ib}=\left[ \begin{array}{ccc} -\sin\psi&0&1 \\ \sin\gamma \cos\psi&\cos\gamma&0 \\ \cos\gamma \sin\psi &-\sin\gamma&0 \end{array} \right] \left[ \begin{array}{c} \dot{\varphi} \\ \dot{\psi} \\ \dot{\gamma} \end{array} \right] \end{align} (35)

where $\varphi$, $\psi$ and $\gamma$ express the angle of pitch, yaw, and roll, respectively.

Based on this assumption that the system has additive noise, the mathematical model can be indicated by

 ${{\pmb x}}_{k}= f({{\pmb x}}_{k-1})+{{\pmb w}}_{k}$ (36)
 ${{\pmb y}}_{k}= h({{\pmb x}}_{k})+{{\pmb v}}_{k}$ (37)

${{\pmb x}}_{k}$ shows the system state, ${{\pmb z}}_{k}$ shows the vector of measurement, ${{\pmb w}}_{k}$ denotes noise of process that is assumed to be the sequence of Gaussian noise with covariance $Q_{k}$, and ${{\pmb v}}_{k}$ is noise of measurement that is a zero-mean additive noise sequence with covariance $R_{k}$.

High accuracy and high availability of navigation information are necessary for many nonlinear systems. The integrated navigation system can make the most of each subsystem's information, and further enhances the precision of navigation information. Thus, we combine SINS with CNS to build integrated navigation system that utilizes SINS as the main body. Next, we correct the attitude angle, velocity, and position information in SINS by the information obtained from CNS [19]. Employing improved filter technology, the errors of accelerometer, gyro or other navigation parameters are obtained in the system of combined navigation.

The SINS/CNS integrated navigation system uses the scheme of strap-down that is connecting the carrier with star sensor, which has the advantages of flexible application, low cost, and a simple structure, but the disadvantage of requiring a larger field angle due to the star sensors only working in a dynamic environment [20]. Fig. 2 shows the work process of integrated navigation system. In view of specific force and angular speed that is obtained from missile trajectory generator, the SINS can get the inertial instrument output. Then, the SINS attitude information turns up. Meanwhile, this CNS subsystem obtains the attitude of vehicle through the star sensors' measured information and directly outputs the angular error information. After filtering the angular error, we gain the SINS's attitude-angle error estimate and the final output of this integrated navigation system.

Integrated navigation systems are more accurate than simple system of inertial navigation because the fixed star's orientation in the inertial space is essentially constant. Though the precession of the earth's polar axis, parallax, and the star sensors' aberration cause small changes in the direction of the stars, they result in an attitude error of less than 1$^{\circ}$. So, the star sensor is equivalent to gyro without drifting, and we can apply the astronomical measurements of inertial equipment error.

In this integrated navigation system, the CNS usually applies the star sensors to seek starry sky, capturing the stars, recognize star charts, and then drawing the fixed stars' imaging location. After image recognition, we eventually calculate the attitude matrix of star sensor to gain the attitude information [21]. The star sensors output the attitude angle's observed value under the inertial coordinate system. In addition, ${{\pmb M}}$ indicates the transition matrix of attitude between the inertial coordinate system and the missile coordinate system.

 \begin{align} {{\pmb M}}=\left[ \begin{array}{ccc} 0&\cos\varphi &-\cos\psi \cos\varphi \\[1mm] 0&-\sin\varphi&\cos\psi \cos\varphi \\[1mm] 1&0&\sin\psi \end{array} \right]. \end{align} (38)

And then, the attitude angle is indicated by

 \begin{align} (\psi, \varphi, \gamma)=F(x, y, \alpha, \beta, f) \end{align} (39)

where ($x, y$) denotes the fixed star's imaging position in the star sensors, ($\alpha, \beta$) show the star's right ascension and declination, respectively, and $f$ shows the focal length of star sensor.

Ⅴ. SIMULATION

In this part, we carry out simulations for SINS/CNS integrated navigation system of the missile to verify the filtering performance of this improved algorithm. The performance of four filtering methods (EKF, MMAE-EKF, UKF, and MMAE-UKF) are also analyzed and compared.

A. Simulation Conditions

In general, we assume a set of time-varying parameters of process noise covariance, but in practice, we do not know these values of the system in advance. Therefore, due to the unpredictability of parameters, we need to adopt a method to generate an optimal random distribution. Here, we select two methods and contrast the simulation result of these two methods that are producing random numbers. The first method is linear congruence method to generate uniform distribution; the second is a Hammersley [22] quasi-random number generation method. Fig. 3 shows a contrast between the first method and Hammersley quasi-random method, for randomly producing 250 elements between $[0, \, 1]$ which represents the range of generating distribution of random number sequences. From the figure, we see clearly that the Hammersley quasi-random method gives a better distribution of elements than uniform distribution, so we select the second method.

 Download: larger image Fig. 3 Comparison of uniform distribution and Hammersley sequence

In this simulation, the filtering methods include the EKF, MMAE-EKF, UKF, and MMAE-UKF algorithms. Table Ⅰ shows the initial simulation parameters.

Table Ⅰ
THE INITIAL SIMULATION PARAMETERS

In the system of integrated navigation, the true values of the parameters $wgt_{0}$ and $wga_{0}$ about process noise covariance are set as 9.6963E-05 and 4.8902E-04, respectively. It may be noted that the parameters $wgt$ and $wga$ in the EKF and UKF simulation process are chosen as $wgt_{0}$ and $wga_{0}$. Since the $6\times 6$ covariance matrix of process noise ${{\pmb Q}}_k$ is parameterized by $wgt$ and $wga$, indicated as

 \begin{align} {{\pmb Q}}_{k}=\left[ \begin{array}{cc} wgt^{2}\left[ \begin{array}{ccc} 1&0&0 \\ 0&1&0 \\ 0&0&1 \end{array} \right] &O_{3\times3} \\ O_{3\times3}&wga^{2}\left[ \begin{array}{ccc} 1&0&0 \\ 0&1&0 \\ 0&0&1 \end{array} \right] \\ \end{array} \right]. \end{align} (40)

To highlight the advantage of filtering algorithm in this actual time-varying system, the Hammersley point set is used to distribute the parameters for each sub filter when setting the filter's parameters. In MMAE, the filters' number is 10.

For the parameter setting of system, we assume the set of parameters for the process noise covariance matrix changes over time. That is to say, the total flight time of this ballistic missile is 300 seconds; during the first 100 seconds, the parameters of process noise covariance $wgt$ and $wga$ are half of $wgt_{0}$ and $wga_{0}$, respectively. From 101 seconds to 200 seconds, the parameters $wgt$ and $wga$ are $wgt_{0}$ and $wga_{0}$, respectively. During the last 100 seconds, the parameters $wgt$ and $wga$ are twice $wgt_{0}$ and $wga_{0}$, respectively.

B. Simulation Result

This part provides simulation result with filtering performance of navigation system by the simulations, and includes some comparisons of the four filtering algorithms. From Figs. 4-6, we can compare and analyze the error of yaw angle, pitch angle, and roll angle of the different filtering methods. According to the simulation results, the MMAE method indicates better performance in aspect of accuracy than the traditional EKF and UKF algorithms in information fusion. Furthermore, the MMAE method with UKF has obviously higher accuracy than the one with EKF algorithm in the state estimation process, and the MMAE-UKF algorithm gains the minimal attitude angle error.

To demonstrate the advantages and disadvantages of these four algorithm in navigation system more clearly, we assess the attitude error navigation performance of these filtering methods by the root mean square error (RMSE). The RMSE is defined by

 \begin{align} RMSE =\sqrt{\frac{1}{n}\sum\limits_{k=1}^{n}(x_{k}-\hat{x}_{k})^{2}} \end{align} (41)

where $n$ shows the number of available experiment data and $(x_{k} -\hat{x}_{k})$ is error of true value and estimation value.

Table Ⅱ shows the simulation result with root mean square error of three estimated attitude angle for different algorithms. Thus, we can see that the optimal estimate of the following four algorithms is to employ this improved MMAE method with unscented Kalman filter.

Table Ⅱ
PERFORMANCE OF DIFFERENT FILTERING ALGORITHMS
Ⅵ. CONCLUSION

This paper has derived a new filtering approach based on multiple model adaptive estimate method, which is suited for nonlinear system-Ballistic Missile Motion. In this method, we design UKF as sub filter in MMAE rather than EKF to obtain the estimate of system state. To indicate the navigation performance of the improved filtering algorithm more clearly, we carry out the simulations, and the behaviors of EKF, MMAE-EKF, UKF, and MMAE-UKF have also been compared and analyzed in the SINS/CNS integrated navigation system. Based on the simulation results, compared with conventional EKF and UKF methods, MMAE-EKF and MMAE-UKF algorithms obtain better navigation performance. MMAE-UKF method can gain the highest navigation accuracy at the cost of the highest computational cost, and the improved methods can meet the demand of the ballistic missile's movement. With a view that integrated navigation system has some uncertain factors, filtering methods with adaptive parameters are to be studied and some real experiments are to be done in the future.

REFERENCES