In-flight alignment method of navigation system based on microelectromechanical systems sensor measurement

Abstract

Inertial navigation systems based on microelectromechanical systems (MEMS) sensors offer advantages that include small size, light weight, low power consumption, strong environmental adaptability, and low cost. These advantages make these sensors particularly suitable for application to precision-guided munitions, which commonly have poor launching environments, strict volume and power consumption requirements, and high cost sensitivity. In this article, the key technologies required for missile-borne integrated navigation systems and the solutions for the problems with each of these technologies are analyzed. An attitude angle estimation method for satellite-assisted MEMS measurement information is proposed that solves the in-flight alignment problem. A high-precision combination of satellite positioning and microinertial navigation is realized through design of a new integrated algorithm framework. The experimental results show that the proposed methods can effectively solve the current problems in guided ammunition navigation.

Introduction

Modern warfare requires weapons and military equipment to have characteristics that include a high degree of informatization, high guidance precision, and a high combat efficiency-to-cost ratio. Informatization and guidance have become the main trends in the development of traditional ammunition for the new era. When a weapon’s hit precision is increased by one time, it is almost equivalent to increasing the amount of ammunition by 8 times, which thus reduces the cost of ammunition consumption and the logistic support burden. The development of guided ammunition requires increased levels of precision and reliability for missile-borne navigation systems. These navigation systems are required to provide comprehensive and accurate navigation and positioning information, to support all-weather operations, to have strong autonomy, and to provide good antijamming performance. Microinertial sensors offer the advantages of small size, light weight, high-impact resistance, low power consumption, and low cost. When combined with satellite positioning systems, microinertial sensors not only have the advantages of the full autonomy of the inertial navigation system, but also have the high-precision positioning characteristics provided by the satellite system with zero accumulation of position errors; this combination can compensate for the shortcomings of the inertial navigation system and the single positioning mode of the satellite system, provide higher navigation accuracy and enhance the antiinterference capabilities of guided munitions.^1,2 However, while the development of microinertial navigation devices has progressed very rapidly and can meet the application requirements of guided munitions, the cost of these microinertial devices increases with increasing performance, the consumption of guided munitions is high, and the device cost is sensitive. If this cost can be reduced through algorithm optimization, it may provide a good system solution.^3,4

A microelectromechanical systems (MEMS) sensor is a device and a system with characteristic dimensions ranging toward the micron level that is manufactured using microelectromechanical processing technology and has the capacity to convert sensations into electrical signals. Information technology is an important and powerful resource for the development of human society in the 21st century. Sensor technology, as an information acquisition technology, has become one of the three main pillars of modern information technology. MEMS sensor technology represents an important development direction for modern sensor technology. In the late 1980s, the rapid development of MEMS technology greatly accelerated the development of both microsensor and microactuator technologies.

Microsensors based on MEMS technology are characterized by their small size, light weight, low cost, low power consumption, high reliability, easy mass production, ease of integration, and multiple functionalization capabilities. The MEMS microsensor is thus an indispensable technology for the development of a variety of automated devices and modern weapons equipment. These sensors have a wide range of potential applications and are regarded as highly important technology throughout the developed world.

Current MEMS-integrated navigation systems for guided munitions applications are represented by the HG19xx and IGSxx series that were developed jointly by Honeywell and Draper Laboratory. The design goal was to design, develop, and produce a sensor that is able to withstand an overload of more than 20,000 g, has launch accuracy of better than 0.5°/h and a volume of less than 12.9 cm³, and has an antiinterference capability. This goal can be achieved and can be enhanced by the combination of the MEMS system with global positioning system (GPS) technology.

After three development stages, the HG1920, HG1930, and HG1940 inertial measurement units and the IGS200, IGS202, IGS204, and IGS250 integrated navigation system product series have been gradually produced; examples are shown in Figure 1. However, these devices all cost more than US$1000 each.

Figure 1.

Honeywell microelectromechanical systems sensor products.

The relationship between cost and performance is illustrated in Figure 2.

Figure 2.

Relationship between cost and performance.

MEMS sensors have brought considerable change to military applications of navigation technology because of their good reliability and relatively low cost, but they still need to be improved in terms of the following aspects:

• The market potential in this field has not yet been fully realized.

• Further investment is needed to continue the research.

• Performance needs to be improved further, and combined with smaller device size and lower cost.

Gyroscope chips, such as the ITG-3200 three-axis gyroscope chip produced by InvenSense, have a range of approximately 2000°/s, can resist impacts of 10,000 g, have a temperature compensation function, and have interintegrated circuit (IIC) digital output interfaces.

Acceleration sensing chips, such as the ADO ADXL345 three-axis accelerometer chip, have a large configurable range of 16 g, can operate properly in temperatures ranging from 40 °C to 85 °C, can resist impacts of 10,000 g, and can be used with serial peripheral interface (SPI)/IIC configurations for their digital output interfaces. Low-cost triaxial gyroscope and accelerometer chips are shown in Figure 3.

Figure 3.

Low-cost triaxial gyroscope and accelerometer chips.

Literatures^5–9 introduce the integrated navigation algorithm under the condition of large misalignment angle. However, when the accuracy of the MEMS sensors are relatively poor, the convergence speed of the algorithm is very slow. The convergence curves for the ammunition’s attitude are shown in Figure 4.

Figure 4.

Three attitude misalignment angle convergence curves for the ammunition.

From the simulated curves, it can be seen that while the pitch, roll, and heading misalignment angles do have a convergence trend, the convergence speed is low.

This result is obviously not practical. Therefore, in the case of a common projectile, which has an approximately parabolic maneuvering mode, the effectiveness of the estimation of the roll and the other attitude angles is poor.

At this stage, an idea is proposed that introduction of the attitude angle calculation model will allow the attitude angle to be calculated directly from the measured data from the satellite and the MEMS sensors. The three misalignment angles can also be regarded as observation variables, and the gyroscope drift can be calculated based on the divergence velocities of the three misalignment angle curves, thus improving the degree of observability of the gyroscope bias.

The purpose of this study is to improve the algorithm that makes it possible to develop ballistic MEMS-integrated navigation systems using similarly low-cost sensor devices.

Key technology analysis and solutions

As a result of the small diameters, poor launch environments and strong roll-based maneuverability of guided munitions, higher requirements and new challenges are proposed for the development of inertial systems for guided ammunition applications.

Given the characteristics described above, the design of the inertial system for guided ammunition should be focused on the design of an anti-high-overload structure, in-flight initial self-alignment of attitude, integrated MEMS/global navigation satellite system (GNSS) navigation, and error calibration and compensation technology for the micro-inertial devices when operating under highly dynamic conditions. Anti-high-overload technology is realized through shock absorption design, encapsulation, and other technical means, which are not described in detail in this work. This article focuses on the description of the navigation algorithm for use in highly dynamic environments.

Large misalignment angle model for integrated navigation

At present, the precision of low-cost MEMS inertial devices is very poor. In general, the precision of a typical silicon micro-gyroscope ranges from tens to hundreds of degrees per hour. Therefore, the in-flight alignment conducted by the miniature inertial measurement unit (MIMU) using GPS information is usually performed under large misalignment angle conditions.

Rapid and accurate in-flight alignment at this large misalignment angle will greatly improve the reaction speed and the operational accuracy of guided munitions, which is of major significance for modern applications. Most previous alignment methods are based on a linear error equation under the assumption that the error angle is small, and the error angle is then estimated using a Kalman filter. When the error angle is large, the traditional linear small error equation cannot be used to describe the system characteristics, which means that a nonlinear error propagation equation and a nonlinear filtering algorithm are required.

The quaternion error equation that was proposed in Yu et al. ⁷ is applicable in the case where the three attitude error angles can all have any value.

In Wei and Zhang’s literature, ¹⁰ the quaternion error equation and extended Kalman filter (EKF) technology have been used to provide effective improvements in the alignment accuracy of a strapdown inertial navigation system with a high-precision inertial device at a large misalignment angle. Lu et al. ¹¹ proposes an attitude estimation algorithm when large external acceleration occurs, which can effectively improve the performance of the algorithm.

1. Attitude quaternion error equation

   Here, an additive quaternion is used to establish a quaternion error equation.

   Define, $\delta \mathit{Q}=\hat{\mathit{Q}}-\mathit{Q}=[\begin{array}{cccc}\delta q_0\hfill & \delta q_1\hfill & \delta q_2\hfill & \delta q_3\hfill \end{array}{]}^{\mathrm{T}}$ ,where, $\delta \mathit{Q}$ is the additive error quaternion, $\hat{\mathit{Q}}$ is the calculated quaternion, and $\mathit{Q}$ is the real quaternion; ${\hat{\mathbf{\omega }}}_{\mathrm{ib}}^b={\mathbf{\omega }}_{\mathrm{ib}}^b+\delta {\mathbf{\omega }}_{\mathrm{ib}}^b$ , ${\mathbf{\omega }}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}{\mathbf{\omega }}_x\hfill & {\mathbf{\omega }}_y\hfill & {\mathbf{\omega }}_z\hfill \end{array}\right]}^T$ is the angular velocity of the $b$ -system relative to the $n$ -system, ${\hat{\mathbf{\omega }}}_{\mathrm{ib}}^b$ is gyroscope measurements, $\delta {\mathbf{\omega }}_{\mathrm{ib}}^b$ is measurement error; ${\hat{\mathbf{\omega }}}_{\mathrm{in}}^n={\mathbf{\omega }}_{\mathrm{in}}^n+\delta {\hat{\mathbf{\omega }}}_{\mathrm{in}}^n$ , ${\mathbf{\omega }}_{\mathrm{in}}^n=\left[\begin{array}{ccc}{\mathbf{\omega }}_E\hfill & {\mathbf{\omega }}_N\hfill & {\mathbf{\omega }}_U\hfill \end{array}\right]$ is the projection of the angular velocity of the $n$ -system relative to the $i$ -coordinate system on the $n$ -coordinate system, ${\hat{\mathbf{\omega }}}_{\mathrm{in}}^n$ is the angular velocity of the $n$ -system relative to the $i$ -system obtained using the position and velocity calculations; Quaternion matrix

   $$\mathit{M}\mathit{Q}=\left[\begin{array}{cccc}{q}_0\hfill & -q_1\hfill & -q_2\hfill & -q_3\hfill \\ \multicolumn{1}{c}{q_1}& q_0\hfill & -q_3\hfill & q_2\hfill \\ \multicolumn{1}{c}{q_2}& q_3\hfill & q_0\hfill & -q_1\hfill \\ \multicolumn{1}{c}{q_3}& -q_2\hfill & q_1\hfill & q_0\hfill \end{array}\right]$$

   Quaternion transformation matrix

   $$\mathit{M}\prime \mathit{Q}=\left[\begin{array}{cccc}{q}_0\hfill & -q_1\hfill & -q_2\hfill & -q_3\hfill \\ \multicolumn{1}{c}{q_1}& q_0\hfill & q_3\hfill & -q_2\hfill \\ \multicolumn{1}{c}{q_2}& -q_3\hfill & q_0\hfill & q_1\hfill \\ \multicolumn{1}{c}{q_3}& q_2\hfill & -q_1\hfill & q_0\hfill \end{array}\right]$$

   The specific expression is

   $$\begin{array}{c}\multicolumn{1}{c}{\delta \stackrel{·}{\mathit{Q}}=\frac{1}{2}[\mathit{M}\prime \left({\mathbf{\omega }}_{\mathrm{ib}}^b\right)-\mathit{M}\left({\mathbf{\omega }}_{\mathrm{in}}^n\right)]\delta \mathit{Q}+\frac{1}{2}[\mathit{M}\prime \left(\delta {\mathbf{\omega }}_{\mathrm{ib}}^b\right)-\mathit{M}\left(\delta {\mathbf{\omega }}_{\mathrm{in}}^n\right)]\mathit{Q}}\\ =\frac{1}{2}[\mathit{M}\prime \left({\mathbf{\omega }}_{\mathrm{ib}}^b\right)-\mathit{M}\left({\mathbf{\omega }}_{\mathrm{in}}^n\right)]\delta \mathit{Q}+\frac{1}{2}\mathit{M}\left(\mathit{Q}\right){\mathbf{\epsilon }}^b-\frac{1}{2}\mathit{M}\prime \left(\mathit{Q}\right)\delta {\mathbf{\omega }}_{\mathrm{in}}^n\hfill \end{array}$$

   (1)

2. Velocity error equation

   Define ${\hat{\mathit{f}}}^b={\mathit{f}}^b+{\nabla }^b$ , ${\hat{\mathit{V}}}_e^n={\mathit{V}}_e^n+\delta \mathit{V}$ , ${\hat{\mathbf{\omega }}}_{\mathrm{en}}^n={\mathbf{\omega }}_{\mathrm{en}}^n+\delta {\mathbf{\omega }}_{\mathrm{en}}^n$ , $\Delta {\mathit{C}}_b^n={\hat{\mathit{C}}}_b^n-{\mathit{C}}_b^n$ , where ${\nabla }^b$ is the constant drift of the accelerometer, ${\mathit{f}}^b$ is the specific force, and ${\hat{\mathit{f}}}^b$ is the measured value of the accelerometer; ${\mathit{V}}_e^n=[\begin{array}{ccc}{V}_E\hfill & V_N\hfill & V_U\hfill \end{array}]$ indicates ground speed, ${\hat{\mathit{V}}}_e^n$ represents the ground speed calculation value, $\delta \mathit{V}=[\begin{array}{cc}\delta V_E\delta V_N\hfill & \delta V_U]\hfill \end{array}$ represents the ground speed error value; ${\mathit{C}}_b^n$ is attitude matrix, ${\hat{\mathit{C}}}_b^n$ is the calculated value of the attitude matrix, $\Delta {\mathit{C}}_b^n$ is the attitude matrix calculation error; ${\mathbf{\omega }}_{\mathrm{en}}^n$ is the projection of the angular velocity of the $n$ -system relative to the $e$ -system on the $n$ -system; $\delta {\mathbf{\omega }}_{\mathrm{en}}^n$ is the calculation error; ${\mathit{g}}^n$ is the projection of gravity acceleration in the n-system, $\delta {\mathit{g}}^n$ is gravity acceleration error

   $$\begin{array}{c}\delta \stackrel{.}{V}=\left(C_b^n+\Delta C_b^n\right){\widehat{f}}^b-C_b^n{f}^b-\left[\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)+\left(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n\right)\right]\times {\widehat{V}}_e^n\\ +\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times V_e^n+\delta g^n\\ =\Delta C_b^n{f}^b+C_b^n{\nabla }^b-\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times \delta V-\left(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n\right)\times V_e^n+\delta g^n\end{array}$$

   (2)

   Here, we introduce $\mathit{U}(\mathit{Q})$ and $\mathit{Y}(\mathit{Q})$ , where

   $$\mathit{U}\left(\mathit{Q}\right)=\left[\begin{array}{cccc}{q}_1\hfill & q_0\hfill & -q_3\hfill & q_2\hfill \\ \multicolumn{1}{c}{q_2}& q_3\hfill & q_0\hfill & -q_1\hfill \\ \multicolumn{1}{c}{q_3}& -q_2\hfill & q_1\hfill & q_0\hfill \end{array}\right]$$

   (3)
   $$\mathit{Y}\left(\mathit{Q}\right)=\left[\begin{array}{cccc}{q}_1\hfill & q_0\hfill & q_3\hfill & -q_2\hfill \\ \multicolumn{1}{c}{q_2}& -q_3\hfill & q_0\hfill & q_1\hfill \\ \multicolumn{1}{c}{q_3}& q_2\hfill & -q_1\hfill & q_0\hfill \end{array}\right]$$

   (4)

   $\mathit{Q}$ is the real quaternion, and

   $$\mathit{Q}={\left[\begin{array}{cccc}{q}_0\hfill & q_1\hfill & q_2\hfill & q_3\hfill \end{array}\right]}^{\mathrm{T}}$$

   (5)

   By ignoring the influence of $\delta {\mathit{g}}^n$ , the nonlinear velocity error equation is obtained as follows

   $$\begin{array}{c}\multicolumn{1}{c}{\delta \stackrel{·}{\mathit{V}}=[2\mathit{U}\left(\mathit{Q}\right){\mathit{Y}}^{\mathrm{T}}\left(\delta \mathit{Q}\right)+\mathit{U}\left(\delta \mathit{Q}\right){\mathit{Y}}^{\mathrm{T}}\left(\delta \mathit{Q}\right)]{\mathit{f}}^b}\\ +{\mathit{C}}_b^n{\nabla }^b-(2{\mathbf{\omega }}_{\mathrm{ie}}^n+{\mathbf{\omega }}_{\mathrm{en}}^n)\times \delta \mathit{V}\hfill \\ -(2\delta {\mathbf{\omega }}_{\mathrm{ie}}^n+\delta {\mathbf{\omega }}_{\mathrm{en}}^n)\times \mathit{V}\hfill \end{array}$$

   (6)

   Among these terms, $\mathit{U}(\delta \mathit{Q}){\mathit{Y}}^T(\delta \mathit{Q})$ represents the nonlinear part of the equation.

3. Position error equation

   $$\{\begin{array}{c}\delta \stackrel{·}{L}=\frac{\delta V_N}{R_M+h}-\delta h\frac{V_N}{{(R_M+h)}^2}\hfill \\ \delta \stackrel{·}{\lambda }=\frac{\delta V_E}{R_N+h}\sec L+\delta L\frac{V_E}{R_N+h}\tan L\sec L-\delta h\frac{V_E\sec L}{{(R_N+h)}^2}\hfill \\ \delta \stackrel{·}{h}=\delta V_U\hfill \end{array}$$

   (7)

   $L$ represents the latitude, $\lambda$ represents the longitude, $h$ represents the height, and $\delta L$ , $\delta \lambda$ , $\delta h$ represent the latitude, longitude, and altitude errors, respectively. $R_M$ is meridian radius of curvature and $R_N$ is radius of curvature of prime vertical circle.

4. Inertial device error equation

The system only works for a short time of a few minutes, so the zero bias drift is very low and can be neglected.

In this article, the gyroscope drift is modeled as a combination of a random constant and white noise. The gyroscope’s constant zero bias is taken to be the system state, and the equation for the gyroscope’s zero bias error is as follows

$$\{\begin{array}{c}{\stackrel{·}{\epsilon }}_x=0\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\epsilon }}_{\mathrm{y}}=0}\\ \multicolumn{1}{c}{{\stackrel{·}{\epsilon }}_{\mathrm{z}}=0}\end{array}$$

(8)

The accelerometer drift is also modeled as a combination of the random constant and white noise. The accelerometer at a constant zero bias taken to be the system state, and the accelerometer’s zero bias error equation is given as follows

$$\{\begin{array}{c}{\stackrel{·}{\nabla }}_x=0\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\nabla }}_y=0}\\ \multicolumn{1}{c}{{\stackrel{·}{\nabla }}_z=0}\end{array}$$

(9)

By combining the error equations for the quaternion, the velocity, the position, and the inertial device that were derived above, the system state equation can be obtained as follows

$$\left[\begin{array}{c}{\stackrel{·}{\mathit{X}}}_a\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\mathit{X}}}_e}\end{array}\right]=\left[\begin{array}{cc}{\mathit{F}}_N\hfill & {\mathit{F}}_S\hfill \\ \multicolumn{1}{c}{0_{6\times 10}}& 0_{6\times 6}\hfill \end{array}\right]\left[\begin{array}{c}{\mathit{X}}_a\hfill \\ \multicolumn{1}{c}{{\mathit{X}}_e}\end{array}\right]+q(\mathit{X},t)+\mathit{G}\prime W$$

(10)

Here, $\mathit{X}\prime ={\left[\begin{array}{cc}{\mathit{X}}_a\hfill & {\mathit{X}}_e\hfill \end{array}\right]}^{\mathrm{T}}$ , in which

$$\begin{array}{c}{\stackrel{·}{\mathit{X}}}_a={\left[\begin{array}{cccccccccc}\delta L\hfill & \delta \lambda \hfill & \delta h\hfill & \delta v_E\hfill & \delta v_N\hfill & \delta v_U\hfill & \delta q_0\hfill & \delta q_1\hfill & \delta q_2\hfill & \delta q_3\hfill \end{array}\right]}^{\mathrm{T}}\hfill \\ {\stackrel{·}{\mathit{X}}}_e={\left[\begin{array}{cccccc}{\nabla }_x\hfill & {\nabla }_y\hfill & {\nabla }_z\hfill & {\epsilon }_x\hfill & {\epsilon }_y\hfill & {\epsilon }_z\hfill \end{array}\right]}^{\mathrm{T}}\hfill \end{array}$$

(11)

The system noise vector is composed of the gyro zero bias and the accelerometer zero bias, which is given as

$$\mathit{W}=[\begin{array}{cccccc}{w}_{\nabla x}\hfill & w_{\nabla y}\hfill & w_{\nabla z}\hfill & w_{\epsilon x}\hfill & w_{\epsilon y}\hfill & w_{\epsilon z}\hfill \end{array}{]}^{\mathrm{T}}$$

(12)

The difference between the position and the velocity calculated using the strapdown inertial navigation system and the position and velocity measured using GPS is taken to be the measurement quantity. The measurement equation is thus given as follows

$$\mathit{Z}\prime \left(t\right)=\mathit{H}\prime \left(t\right)\mathit{X}\prime \left(t\right)+\mathit{V}\left(t\right)$$

(13)

Note that the observation vector is

$$\mathit{Z}\prime =\left[\begin{array}{c}\Delta P\hfill \\ \multicolumn{1}{c}{\Delta V}\\ \multicolumn{1}{c}{\Delta Q}\end{array}\right]$$

(14)

in which $\Delta P=\left[\begin{array}{c}{L}_{\mathrm{IE}}-L_{\mathrm{GE}}\hfill \\ \multicolumn{1}{c}{{\lambda }_{\mathrm{IN}}-{\lambda }_{\mathrm{GN}}}\\ \multicolumn{1}{c}{h_{\mathrm{IU}}-h_{\mathrm{GU}}}\end{array}\right]$ , $\Delta V=\left[\begin{array}{c}{v}_{\mathrm{IE}}-v_{\mathrm{GE}}\hfill \\ \multicolumn{1}{c}{v_{\mathrm{IN}}-v_{\mathrm{GN}}}\\ \multicolumn{1}{c}{v_{\mathrm{IU}}-v_{\mathrm{GU}}}\end{array}\right]$

Three attitude angles estimated by attitude angle filtering algorithm are converted to attitude quaternion $\hat{\mathrm{Q}}$ , which is subtracted from attitude quaternion Q calculated by inertial navigation system, and the observation of attitude error quaternion $\delta \mathrm{Q}$ is obtained.

The measured noise vector $\mathit{V}$ combines the position measurement noise and the velocity measurement noise. The measurement matrix is thus given as

$$\mathit{H}\prime =\left[{\mathit{I}}_{10\times 10}\right]$$

(15)

Unscented Kalman filtering technology

The EKF is widely used in nonlinear filtering applications because it offers advantages such as a simple method and easy implementation. However, an EKF filter linearizes the nonlinear function, that is, it ignores the influence of the Taylor expansion of the nonlinear function on the higher-order terms; therefore, when the system is highly nonlinear, the EKF estimation accuracy drops dramatically and can even diverge.

At present, a particle filter (PF) estimation method based on an approximate probability density distribution is quite popular for nonlinear filtering because it is not necessary to understand the specific expansion formula of the nonlinear function in detail.^12–14 The probability density distribution can be solved directly by solving the nonlinear function of the particles. The unscented Kalman filter (UKF) can be regarded as a type of PF. The UKF uses a deterministic sampling strategy to approximate nonlinear distributions. When compared with general PF filtering, the UKF contains fewer particles, which has the major advantage of requiring small amounts of computation and is thus more practical.

The UKF algorithm is a Kalman filter algorithm based on the unscented transform (UT).^15,16 The UT transform method is used to obtain the mean value for a one-step prediction and the mean square error for one-step prediction of states. The steps of the UKF algorithm are given as follows:

• Initialization

$${\hat{\mathit{x}}}_0=E\left[{\mathit{x}}_0\right]$$

(16)

$${\mathit{P}}_0=E\left[({\mathit{x}}_0-{\hat{\mathit{x}}}_0){({\mathit{x}}_0-{\hat{\mathit{x}}}_0)}^{\mathrm{T}}\right]$$

(17)

• Computational sampling (sigma) point:

The 2n + 1 sigma points of the state are selected (where n is the dimension of the state)

$$\{\begin{array}{c}{\mathbf{\chi }}_{k-1}^0={\hat{\mathit{x}}}_{k-1},i=0\hfill \\ \multicolumn{1}{c}{{\mathbf{\chi }}_{k-1}^0={\hat{\mathit{x}}}_{k-1}+\sqrt{n+\kappa }{\left(\sqrt{{\mathit{P}}_{k-1}}\right)}_i,i=1,\dots ,n}\\ \multicolumn{1}{c}{{\mathbf{\chi }}_{k-1}^i={\hat{\mathit{x}}}_{k-1}-\sqrt{n+\kappa }{\left(\sqrt{{\mathit{P}}_{k-1}}\right)}_i,i=n+1,\dots ,2n}\end{array}$$

(18)

In the case where the system state variables ${\mathit{x}}_k$ have more than 10 dimensions, ${\mathit{P}}_{k-1}$ will be a multidimensional matrix. A Cholesky decomposition should be performed at this time, as follows.

When ${\mathit{P}}_{k-1}={\mathit{A}}^{\mathrm{T}}\mathit{A}$ , $(\sqrt{{\mathit{P}}_{k-1}}{)}_i$ , the ith row of $\mathit{A}$ is selected; when ${\mathit{P}}_{k-1}=\mathit{A}{\mathit{A}}^{\mathrm{T}}$ , $(\sqrt{{\mathit{P}}_{k-1}}{)}_i$ , the ith column of $\mathit{A}$ is taken.

• Time update:

The posterior mean and the variance of the state are then calculated using UT technology

$${\mathbf{\chi }}_{\frac{k}{k-1}}^i=\mathit{f}\left({\mathbf{\chi }}_{k-1}^i\right)$$

(19)

$${\hat{\mathit{x}}}_k^{-}=\sum _{i=0}^{2n}{\mathit{W}}_i^m{\mathbf{\chi }}_{\frac{k}{k-1}}^i$$

(20)

$${\mathit{P}}_{x,k}^{-}=\sum _{i=0}^{2n}{\mathit{W}}_i^c[{\mathbf{\chi }}_{\frac{k}{k-1}}^i-{\hat{\mathit{x}}}_k^{-}]{[{\mathbf{\chi }}_{k-1}^i-{\hat{\mathit{x}}}_k^{-}]}^{\mathrm{T}}+{\mathit{Q}}_k$$

(21)

$${\mathit{z}}_{\frac{k}{k-1}}^i=\mathit{h}\left({\mathbf{\chi }}_{\frac{k}{k-1}}^i\right)$$

(22)

$${\hat{\mathit{z}}}_k^{-}=\sum _{i=0}^{2n}{\mathit{W}}_i^m{\mathit{z}}_{\frac{k}{k-1}}^i$$

(23)

• Measurement update:

The measurements are updated using standard Kalman filtering, but the formulas used for this purpose are different

$${\mathit{P}}_{z,k}=\sum _{i=0}^{2n}{\mathit{W}}_i^c[{\mathit{z}}_{\frac{k}{k-1}}^i-{\hat{\mathit{z}}}_k^{-}]{[{\mathit{z}}_{k-1}^i-{\hat{\mathit{z}}}_k^{-}]}^{\mathrm{T}}+{\mathit{R}}_k$$

(24)

$${\mathit{P}}_{\mathrm{xz},k}=\sum _{i=0}^{2n}{\mathit{W}}_i^c[{\mathbf{\chi }}_{\frac{k}{k-1}}^i-{\hat{\mathit{x}}}_k^{-}]{[{\mathit{z}}_{k-1}^i-{\hat{\mathit{z}}}_k^{-}]}^{\mathrm{T}}$$

(25)

$${\mathit{K}}_k={\mathit{P}}_{\mathrm{xz},k}{\mathit{P}}_{z,k}^{-}$$

(26)

$${\hat{\mathit{x}}}_k={\hat{\mathit{x}}}_k^{-}+\mathit{K}({\mathit{z}}_k-{\hat{\mathit{z}}}_k^{-})$$

(27)

$${\mathit{P}}_{x,k}={\mathit{P}}_{x,k}^{-}-\mathit{K}{\mathit{P}}_{z,k}{\mathit{K}}_k^{\mathrm{T}}$$

(28)

The UKF algorithm does not approximate the nonlinear function itself, but instead approximates the probability density distribution of the nonlinear function. Therefore, the algorithm is effective, regardless of the complexity of the system state equation, and its accuracy is much higher, and may reach the level of the three-level Taylor expansion. In terms of computational complexity, the real-time performance of the UKF algorithm is relatively poor, while its computational complexity is approximately 3 times that of the EKF, and a large part of the computational resources required for the UKF are focused on decomposition of the covariance matrix. If the decomposition method used for the $\sqrt{{\mathit{P}}_{k-1}}$ matrix can be improved to reduce the computational complexity to a major extent, then a significant improvement may be made in the real-time performance of the algorithm.

Observability analysis

Under the moving base condition, the piecewise constant system (PWCS) method is used to analyze the observability of the model. ¹⁷ The discrete linear time-varying system is set up as follows

$$\{\begin{array}{c}\mathit{X}(k+1)={\mathit{F}}_j\mathit{X}\left(k\right)+{\mathit{B}}_j\mathit{U}\left(K\right)\hfill \\ \multicolumn{1}{c}{{\mathit{Z}}_j\left(k\right)={\mathit{H}}_j\mathit{X}\left(k\right)}\end{array}$$

(29)

In the above formula, $\mathit{X}(k)\in {\mathit{R}}^n$ , ${\mathit{F}}_j\in {\mathit{R}}^{n\times n}$ , ${\mathit{B}}_j\in {\mathit{R}}^{n\times s}$ , $\mathit{U}(k)\in {\mathit{R}}^s$ , ${\mathit{Z}}_j(k)\in {\mathit{R}}^m$ , ${\mathit{H}}_j\in {\mathit{R}}^{m\times n}$ , and $j=1,2,...,r$ . For each time period $j$ , the matrices ${\mathit{F}}_j$ , ${\mathit{B}}_j$ , and ${\mathit{H}}_j$ are all constant.

The observability of the system state equation is independent of the input, and the observability matrix of the system in the $j$ th period can thus be obtained as follows

$${\mathit{Q}}_j={\left[\begin{array}{cccc}{\mathit{H}}_j\hfill & {\mathit{H}}_j{\mathit{F}}_j\hfill & \cdots \hfill & {\mathit{H}}_j{\mathit{F}}_j^{n-1}\hfill \end{array}\right]}^{\mathrm{T}}$$

(30)

A singular value decomposition is performed for the striped observability matrix (SOM) of the PWCS.

$${\mathit{Q}}_s\left(j\right)=\mathit{US}{\mathit{V}}^{\mathrm{T}}$$

(31)

Here, $\mathit{U}=\left[\begin{array}{cccc}{u}_1\hfill & u_2\hfill & ...\hfill & u_m\hfill \end{array}\right]$ and $\mathit{V}=\left[\begin{array}{cccc}{v}_1\hfill & v_2\hfill & ...\hfill & v_n\hfill \end{array}\right]$ are both orthogonal arrays, and

$${\mathit{S}}_{m\times n}=\left[\begin{array}{cc}{\mathbf{\Lambda }}_{r\times r}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill \end{array}\right]$$

(32)

Here, $\mathbf{\Lambda }=\mathrm{diag}(\begin{array}{cccc}{\sigma }_1\hfill & {\sigma }_2\hfill & \cdots \hfill & {\sigma }_r\hfill \end{array})$ is a diagonal matrix, and ${\sigma }_1\ge {\sigma }_2\ge \cdots \ge {\sigma }_r$ are the singular values of the matrix ${\mathit{Q}}_s(r)$ .

Assuming that the appearance measurement of the system is $\mathit{Z}$ , and that the initial value of the state is $\mathit{X}(0)$ , the following formula can be obtained

$$\mathit{Z}={\mathit{Q}}_{\mathit{s}}\left(r\right)\mathit{X}\left(0\right)$$

(33)

Equation (33) is then substituted into equation (31), and the following is obtained

$$\mathit{Z}=\sum _{i=1}^r{\sigma }_i\left({\nu }_i^t\mathit{X}\left(0\right)\right)u_i$$

(34)

Here, $u_i$ is column i of $U$ and ${\nu }_i$ is column i of V.

From the given appearance measurement $Z$ , a set of state vectors $\mathit{X}(0)$ can be obtained when ${\sigma }_i={\sigma }_{i+1}=\cdots ={\sigma }_r=0$ , as follows

$$\mathit{X}(0)=\sum _{i=1}^l\left(\frac{u_i^{\mathrm{T}}z}{{\sigma }_i}\right){\nu }_i+\sum _{i=l+1}^r{\alpha }_i{\nu }_i$$

(35)

Among these parameters, ${\alpha }_i$ is an arbitrary coefficient.

The expression for the degree of observability ¹⁸ is given as shown below

$$\{\begin{array}{c}{\eta }_k=\frac{{\sigma }_i}{{\sigma }_0},\mathrm{i}=1,2,\dots ,r\hfill \\ \multicolumn{1}{c}{{\sigma }_i~max\left[\frac{u_i^{\mathrm{T}}\mathit{Z}{v}_i}{{\sigma }_i}\right]}\end{array}$$

(36)

In the formula above, ${\eta }_k$ represents the degree of observability of the Kth state variable; ${\sigma }_0$ represents the singular values that correspond to the directly observed state; and ${\sigma }_i$ represents the singular value that corresponds to the maximum $k$ th component in the vector $[u_i^{\mathrm{T}}\mathit{Z}{v}_i/{\sigma }_i]$ . By analyzing each singular value using the procedure described above, the degree of observability of the system can be obtained at the current moment.

An observability analysis of the large misalignment error model is also carried out.

The additive quaternion is used to describe the attitude angle, so the three attitude angle errors are also reflected in the additive quaternion error. Here, the quaternion errors are expressed as $\delta q_0$ , $\delta q_1$ , $\delta q_2$ , and $\delta q_3$ . The position errors include the latitude error $\delta L$ , the longitude error $\delta \lambda$ , and the height error $\delta h$ . The velocity error includes $\delta V_E$ , $\delta V_N$ , and $\delta V_U$ . The guided projectiles adopt a conventional maneuvering mode, which is described as follows.

Attitude angle estimation method

Following satellite positioning, the pitch angle and the heading angle of the projectile axis can be calculated approximately using the measured values of the projectile velocity provided by the satellite positioning device (while ignoring the effects of the nutation and movement of the projectile)

$$\psi =\mathrm{atan}\frac{V_E}{V_N},\theta =\mathrm{atan}\frac{V_U}{\sqrt{V_E^2+V_N^2}}$$

(37)

In the formula, $\psi$ is the heading angle of the projectile axis and $\theta$ is the pitch angle of the projectile axis. $V_E$ , $V_N$ , and $V_U$ are the velocities of the projectile in the east, north, and sky directions, respectively.

By differentiating the heading angle and the pitch angle of the projectile axis, the lateral and pitch angular velocities of the projectile can be calculated. Then, using the geometric relationship of the gyroscope installation within the inertial navigation system, the angular velocity measured using two gyroscopes oriented perpendicular to the projectile axis can be deduced, and the mathematical relationship among the lateral and pitch angular velocities and the roll angle of the projectile can be calculated.

According to external ballistics theory, the outputs of the Y axis and Z axis angular velocities in a fixed coordinate system are given as follows ¹⁹

$${\omega }_y^b=\stackrel{·}{\psi }\cos \theta \cos \gamma +\stackrel{·}{\theta }\sin \gamma$$

(38)

$${\omega }_z^b=-\stackrel{·}{\psi }\cos \theta \sin \gamma +\stackrel{·}{\theta }\cos \gamma$$

(39)

$\stackrel{·}{\psi }$ is the yaw angular velocity, $\theta$ is the pitch angle, $\stackrel{·}{\theta }$ is the pitch angle velocity, and γ is the roll angle.

Let $R=\stackrel{·}{\psi }\cos \theta$ and $Q=\stackrel{·}{\theta }$ , and then the trigonometric functions of the roll angle sine and cosine of the projectile with the pitch angular velocity and the yaw angular velocity are obtained as follows ²⁰

$$\sin \gamma =\frac{Q{\omega }_y^b-R{\omega }_z^b}{Q^2+R^2},\cos \gamma =\frac{R{\omega }_y^b+Q{\omega }_z^b}{Q^2+R^2}$$

(40)

The sine and cosine functions of the roll angle of the projectile can also be expressed as

$$\sin \gamma =\sin (P_x·t+{\phi }_0),\cos \gamma =\mathrm{cosn}(P_x·t+{\phi }_0)$$

(41)

where ${\phi }_0$ is the initial phase of the roll angle.

The state variables for Kalman filtering are established as follows

$$X\left(t\right)=[P_x^b{P}_y^b{P}_z^b{\omega }_x^{\mathrm{bias}}{\omega }_y^{\mathrm{bias}}{\omega }_z^{\mathrm{bias}}\sin {\phi }_0\cos {\phi }_0]$$

(42)

Of these variables, $P_x^b,P_y^b,P_z^b$ are the estimated values of ${\omega }_x^b{\omega }_y^b{\omega }_z^b$ , respectively.

Therefore, the Kalman filtering measurement equation is established as shown below

$$\begin{array}{c}\multicolumn{1}{c}{\left[\begin{array}{c}{\omega }_x^b\hfill \\ \multicolumn{1}{c}{{\omega }_y^b}\\ \multicolumn{1}{c}{{\omega }_z^b}\\ \multicolumn{1}{c}{\sum ^{\frac{{\omega }_y^b}{n}}}\\ \multicolumn{1}{c}{\sum ^{\frac{{\omega }_z^b}{n}}}\\ \multicolumn{1}{c}{\sin \gamma \left(t\right)}\\ \multicolumn{1}{c}{\cos \gamma \left(t\right)}\end{array}\right]\left[\begin{array}{cccccccc}1\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& \frac{R}{2(Q^2+R^2)}\hfill & \frac{-Q}{2(Q^2+R^2)}\hfill & 0\hfill & 0\hfill & 0\hfill & 0.5\sin (P_x·t)\hfill & 0.5\cos (P_x·t)\hfill \\ \multicolumn{1}{c}{0}& \frac{Q}{2(Q^2+R^2)}\hfill & \frac{R}{2(Q^2+R^2)}\hfill & 0\hfill & 0\hfill & 0\hfill & 0.5\cos (P_x·t)\hfill & -0.5\sin (P_x·t)\hfill \end{array}\right]·\left[\begin{array}{c}{P}_x^b\hfill \\ \multicolumn{1}{c}{P_y^b}\\ \multicolumn{1}{c}{P_z^b}\\ \multicolumn{1}{c}{{\omega }_x^{\mathrm{bias}}}\\ \multicolumn{1}{c}{{\omega }_y^{\mathrm{bias}}}\\ \multicolumn{1}{c}{{\omega }_z^{\mathrm{bias}}}\\ \multicolumn{1}{c}{\sin {\phi }_0}\\ \multicolumn{1}{c}{\cos {\phi }_0}\end{array}\right]}\end{array}$$

(43)

The state equation of the Kalman filtering system is then established as follows

$$X(t+1)=\left[\begin{array}{cccccccc}1\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 1\hfill \end{array}\right]·X\left(t\right)$$

(44)

The filter state variables, the filter measurement equation and the state equation are then inserted into the Kalman filter iteration equation to estimate the sine and cosine functions of the projectile roll angle. The projectile roll angle can be obtained by calculating the arc tangent of the function value and judging the quadrant with the positive and negative signs of the roll angle sine and cosine function values

$$\gamma \left(t\right)=\mathrm{atan}\left(\frac{\sin \gamma \left(t\right)}{\cos \gamma \left(t\right)}\right)$$

(45)

Experimental verification

At first, numerical simulation is carried out by MATLAB.

A simplified block diagram of the algorithm is shown in Figure 5.

Figure 5.

Simplified block diagram of the algorithm.

The observability is related to the maneuverability. In some maneuvering cases, certain states are unobservable, but most states in the complete process can be observed; however, the magnitudes of these observabilities are different. First, the position and the velocity in the three directions are external measurements, so the observability degree is approximately 1. For the attitude, the data for a projectile without a gliding wing have the lowest quaternion error observability at approximately 0.5. The data for a projectile with a gliding wing have the relatively high degree of observability of roughly 1.1 to 1.2. The degree of observability of the quaternion for uniform speed and flat flight is slightly higher than that for the corresponding condition with the gliding wing at approximately 1.2 to 1.6. The degrees of observability of the accelerometer zero bias and the gyroscope zero bias are small and are of the order of 0.01 in magnitude. When the measurement noise is high and the system model is uncertain, the observable accelerometer zero bias and gyroscope zero bias information can easily be submerged in the noise, thus making the estimation process more difficult.

The projectile’s flight time in the air is 100 s, the initial launch pitch angle is 56°, the initial projectile velocity is 720 m/s, and its rotational speed is 5 r/s. GPS position accuracy: 2.5 m CEP (horizontal accuracy) and 5.0 m CEP (vertical accuracy); GPS velocity accuracy: 0.25 m/s CEP (horizontal accuracy) and 0.5 m/s CEP (vertical accuracy). The improved algorithm is simulated by MATLAB. Simultaneously, the rank of the SOM is calculated and extracted online. The observability of the 16 states of the system is simulated and saved, and a curve of the change in observability is drawn as shown in Figure 6. Figure 7 shows the height and pitch angle contrast curve between GPS and integrated navigation system. Figure 8 shows the attitude estimation error curves of the two algorithms.

Figure 6.

Observability curves for attitude quaternion. (a) Observability degree curve of $\Delta q_0$ . (b) Observability degree curve of $\Delta q_1$ . (c) Observability degree curve of $\Delta q_2$ . (d) Observability degree curve of $\Delta q_3$ .

Figure 7.

The height and pitch angle contrast curve between global positioning system and integrated navigation system.

Figure 8.

The attitude estimation error curves of the two algorithms.

And then, to verify the effectiveness of the algorithm, a projectile with an integrated navigation system based on low-precision MEMS sensors and a calibrated geomagnetic attitude angle measurement device was used for comparison.

The projectile was fired at a pitch angle of 60° with a muzzle velocity of 710 m/s and an initial roll angle velocity of approximately 25 r/s. Here, the gyro zero bias in the three directions was about 1°/s, the random drift was 0.5°/s, the accelerometer zero bias in the three directions was 500  $\mu \mathrm{g}$ , and the random drift was 200 µg.

In the program, the initial roll angle is set at 0°. Figure 9 shows the roll attitude angle error curve, and Figure 10 shows the zero bias identification curve. The position error and velocity error statistics are listed in Table 1.

Figure 9.

Error between roll angle output by the integrated navigation system and roll angle from the geomagnetic survey.

Figure 10.

X-axis gyroscope zero bias estimation curve.

Table 1.

Error statistics for position and velocity.

Items	Mean values	Mean square error	Minimum	Maximum
X (m)	2.62	3.97	–2.7	14.5
Y (m)	–1.17	1.46	–1.4	1.8
Z (m)	–0.16	0.35	–0.1	0.8
Vx (m/s)	–0.16	0.28	–0.1	0.5
Vy (m/s)	–0.22	0.26	–0.3	0.7
Vz (m/s)	–0.03	0.13	0.0	0.4

The experimental results show that the roll angle achieves alignment less than 3 s after satellite positioning and then maintains high alignment accuracy. The zero bias estimation of the x-axis gyroscope tends to become stable in approximately 45 s. The mean position error is less than 3 m, and the mean velocity error is less than 0.3 m/s. All indicators demonstrate the correctness and effectiveness of the proposed algorithm.

Concluding remarks

In this article, the feasibility of applying an integrated navigation system that was designed using low-precision MEMS sensors to guided munitions is analyzed. The attitude angle estimation method for the satellite-assisted MEMS measurement information is proposed and the in-flight alignment problem is solved. The design of a new integrated filtering algorithm framework allows the high-precision combination of satellite positioning and a microinertial navigation system to be realized, and the initial alignment time is reduced. The experimental results demonstrate that this method can effectively solve the navigation problems of guided munitions using low-cost MEMS sensors.