A Novel Robust H_∞ Filter Based on Krein Space Theory in the SINS/CNS Attitude Reference System

Abstract

Owing to their numerous merits, such as compact, autonomous and independence, the strapdown inertial navigation system (SINS) and celestial navigation system (CNS) can be used in marine applications. What is more, due to the complementary navigation information obtained from two different kinds of sensors, the accuracy of the SINS/CNS integrated navigation system can be enhanced availably. Thus, the SINS/CNS system is widely used in the marine navigation field. However, the CNS is easily interfered with by the surroundings, which will lead to the output being discontinuous. Thus, the uncertainty problem caused by the lost measurement will reduce the system accuracy. In this paper, a robust $H_{\infty }$ filter based on the Krein space theory is proposed. The Krein space theory is introduced firstly, and then, the linear state and observation models of the SINS/CNS integrated navigation system are established reasonably. By taking the uncertainty problem into account, in this paper, a new robust $H_{\infty }$ filter is proposed to improve the robustness of the integrated system. At last, this new robust filter based on the Krein space theory is estimated by numerical simulations and actual experiments. Additionally, the simulation and experiment results and analysis show that the attitude errors can be reduced by utilizing the proposed robust filter effectively when the measurements are missing discontinuous. Compared to the traditional Kalman filter (KF) method, the accuracy of the SINS/CNS integrated system is improved, verifying the robustness and the availability of the proposed robust $H_{\infty }$ filter.

1. Introduction

In modern marine navigation, the strapdown inertial navigation system (SINS) is widely used due to its advantages of being more compact and autonomous, which can provide vehicle’s navigation information, including attitude, velocity and position [1,2,3]. However, in SINS systems, there are accumulated navigation errors caused by the inertial components inevitably. Additionally, this is a serious problem in long-term marine navigation. Thus, some other navigation systems, e.g., the Global Position System (GPS), the Doppler velocity log (DVL), the celestial navigation system (CNS), etc., are often integrated with it to improve the navigation accuracy of the whole system availably making use of the complementary navigation information obtained from different sensors [4,5,6,7,8].

In CNS, the accurate attitude is calculated based on the azimuth of a celestial body measured by the celestial sensor. Since the celestial body is used as the navigation information source, the CNS has numerous merits, such as high positioning and orienting accuracy, good stealthiness and independence. What is more, the navigation error is not accumulated over time. Therefore, the CNS is widely used to aid the SINS in astronautics, marine navigation and surveying fields, utilizing the SINS/CNS integrated navigation system. In [9], Xu and Fang proposed INS/CNS integration, using the INS error model and Kalman filter (KF) based on neural networks. Integrating INS with GPS and a star tracker was performed by [10] using a decentralized multisensor integration structure, in which the error compensation rate of integration was studied. Because of that traditional ground-based initial alignment methods cannot work well on the lunar surface, [8] proposed a new autonomous INS initial alignment method, which used star observations to help the INS estimate its attitude, gyroscopes drifts and accelerometer biases. Additionally, simulations proved the superiority of the new method.

However, the CNS easily interfered with by the surroundings, such as clouds, since stars are used as the beacon of the star sensor. For example, the number of stars, which is used for calculating the navigation information of the star sensor, is reduce under cloudy weather. Hence, the occluded star sensor will lead to the output being discontinuous [6,11,12]. Then, the system model will not be accurate under this condition. Furthermore, the uncertainty problem will be introduced into the integrated system, reducing the system accuracy significantly. Although the KF is the most commonly-used optimal estimation, it is hard to get the expected results when the system model is not accurate. Therefore, many methods are used to describe and compensate the uncertainty of the system [13,14,15,16]. In [14], a new robust Kalman filter was proposed that detects and bounds the influence of outliers in a discrete linear system, including those generated by thick-tailed noise distributions, such as impulsive noise. Taking the robust state estimation for uncertain descriptor systems into account, a robust filtering framework (RFF) was proposed to facilitate the robust filter design [16]. Hamza and Nebylov proposed a robust design of an INS/GNSS navigation system to solve the problem of state space models with non-Gaussian measurement noise based on parallel nonlinear filtering [13].

Although the robust Kalman filter based on the $H_2$ norm has a simple design form, it requests that the statistical characteristics of the system noise be already known, which is difficult to meet in practical applications. The robust $H_{\infty }$ filter has high stability, but its design form is complex, which is not suitable for actual applications, as well [15,17].

In recent years, due to its simple design, flexible structure and wide application, the Krein space theory has become a hot issue gradually [18,19,20]. Therefore, a robust $H_{\infty }$ filter for the SINS/CNS integrated navigation system is presented in this manuscript based on the Krein space theory. Taking the uncertainty problem into account, a robust $H_{\infty }$ filter in the Krein space frame is presented and derived. Even better, the novel filter not only achieves robustness against missing measurements using robust $H_{\infty }$ filtering, but also improves the system accuracy effectively due to the Krein space theory. Additionally, the results from simulations and experiments show that the presented robust filter is superior to the normal Kalman filter. The rest of this manuscript is organized as follows. The fundamentals of the Krein space theory and the linear error equations of the SINS/CNS integrated navigation system are introduced in Section 2. Additionally, the new robust filter is proposed in Section 3. Numerical simulations and experiments along with specific analysis are given in Section 4. Section 5 concludes this manuscript.

2. Linear Filter Based on the Krein Space Theory

In this section, some basic knowledge of the Krein space theory will be introduced in order to understand the theoretical derivation below easily. The system model of the SINS/CNS integrated system is established, as well, here.

2.1. Fundamentals of the Krein Space Theory

The Krein space is a non-classical functional space, which has attracted extensive attention. The kernel of the Krein space estimation theory is that the minimization of the quadratic cost function is translated into the Kalman filter problem just in the Krein space, rather than the random Hilbert space [19,20,21].

Now, consider the following Krein space state-space formal system:

$$\left\{\begin{array}{c}{\mathit{x}}_{j+1}=A_j{\mathit{x}}_j+B_j{\mathit{u}}_j\hfill \\ {\mathit{y}}_{j+1}=C_{j+1}{\mathit{x}}_{j+1}+D_{j+1}{\mathit{v}}_{j+1}\hfill \end{array}\right.$$

(1)

with covariance matrix:

$$〈\left(\begin{array}{c}{\mathit{x}}_0\\ {\mathit{u}}_j\\ {\mathit{v}}_{j+1}\end{array}\right),\left(\begin{array}{c}{\mathit{x}}_0\\ {\mathit{u}}_j\\ {\mathit{v}}_{j+1}\end{array}\right)〉=\left(\begin{array}{ccc}{P}_0& 0& 0\\ 0& Q_j& S_j\\ 0& S_j& R_{j+1}\end{array}\right)$$

(2)

wherein ${\mathit{x}}_{j+1}$ and ${\mathit{y}}_{j+1}$ are the state vector and the measurement vector at time $j+1$, respectively; $A_j$ and $C_{j+1}$ are specific known linear functions, the state-transition matrix and the measurement matrix; $B_j$ and $D_{j+1}$ are the coefficient matrices of the state noises; ${\mathit{u}}_j$ and ${\mathit{v}}_{j+1}$ are the state noise vector and observation noise vector with their autocorrelation and cross-correlation matrix of variance matrices $Q_j$, $R_{j+1}$ and $S_j$, respectively; $P_0$ is the initial state covariance matrix.

Thus, the quadratic cost function is chose as follows:

$$\mathit{J}={\mathit{x}}_0^T{\mathit{P}}_0^{-1}{\mathit{x}}_0+{\displaystyle \sum _{j=0}^N}{\mathit{u}}_j^T{\mathit{Q}}_j^{-1}{\mathit{u}}_j+{\displaystyle \sum _{j=0}^N}{\mathit{v}}_j^T{\mathit{R}}_j^{-1}{\mathit{v}}_j$$

(3)

We define the state transition matrix as:

$${\varphi }_{j,k}\stackrel{\Delta }{=}{A}_{j-1}\cdots A_k,j>k,{\varphi }_{j,j}=I$$

(4)

and the response matrix as:

$$h_{j,k}\stackrel{\Delta }{=}{C}_j{A}_{j-1}\cdots A_{k+1}{B}_k$$

(5)

Then, using:

$$\overline{\mathit{y}}\stackrel{\Delta }{=}col\left\{{\mathit{y}}_0,\cdots {\overline{\mathit{y}}}_N\right\}\overline{\mathit{y}}\stackrel{\Delta }{=}col\left\{{\mathit{y}}_0,\cdots {\overline{\mathit{y}}}_N\right\}\overline{\mathit{v}}\stackrel{\Delta }{=}col\left\{{\mathit{v}}_0,\cdots ,{\mathit{v}}_N\right\}$$

(6)

the Krein space state-space as Equation (1) satisfies this:

$$\overline{\mathit{y}}=\overline{\Theta }{\mathit{x}}_0+\overline{\mathrm{{\rm Y}}}\overline{\mathit{u}}+\overline{\Omega }\overline{\mathit{v}}=\left(\begin{array}{ccc}\overline{\Theta }& \overline{\mathrm{{\rm Y}}}& \overline{\Omega }\end{array}\right)\left(\begin{array}{c}{\mathit{x}}_0\\ \overline{\mathit{u}}\\ \overline{\mathit{v}}\end{array}\right)$$

(7)

wherein:

$$\Xi \stackrel{\Delta }{=}〈\left(\begin{array}{c}{\mathit{x}}_0\\ \overline{\mathit{u}}\\ \overline{\mathit{y}}\end{array}\right),\left(\begin{array}{c}{\mathit{x}}_0\\ \overline{\mathit{u}}\\ \overline{\mathit{y}}\end{array}\right)〉=\left(\begin{array}{ccc}I& 0& 0\\ 0& I& 0\\ \overline{\Theta }& \overline{\mathrm{{\rm Y}}}& \overline{\Omega }\end{array}\right)\left(\begin{array}{ccc}{\mathit{P}}_0& 0& 0\\ 0& \overline{\mathit{Q}}& \overline{\mathit{S}}\\ 0& {\overline{\mathit{S}}}^T& \overline{\mathit{R}}\end{array}\right){\left(\begin{array}{ccc}I& 0& 0\\ 0& I& 0\\ \overline{\Theta }& \overline{\mathrm{{\rm Y}}}& \overline{\Omega }\end{array}\right)}^T$$

(8)

and the symbols are defined as follows:

$$\begin{array}{c}\overline{Q}=Q_0\oplus \cdots \oplus Q_N\hfill \\ \overline{R}=R_0\oplus \cdots \oplus R_N\hfill \\ \overline{S}=S_0\oplus \cdots \oplus S_N\hfill \\ \overline{\Omega }=D_0\oplus \cdots \oplus D_N\hfill \end{array}$$

(9)

$$\overline{\Theta }=\left(\begin{array}{c}{C}_0\\ C_1{\varphi }_{1,0}\\ ⋮\\ C_N{\varphi }_{N,0}\end{array}\right),\overline{\mathrm{{\rm Y}}}=\left(\begin{array}{cccc}0& & & \\ h_{1,0}& 0& & \\ h_{2,0}& h_{2,1}& 0& \\ ⋮& \cdots & & \ddots \end{array}\right)$$

(10)

The projection error in Krein space to study is the following Gramian:

$$〈\mathbf{\ae }-\mathbf{k}\overline{\mathit{y}},\mathbf{\ae }-\mathbf{k}\overline{\mathit{y}}〉=\left(\begin{array}{cc}I& -k\end{array}\right)\Xi \left(\begin{array}{c}I\\ -k^T\end{array}\right)$$

(11)

where we suppose $\mathbf{\ae }=\mathbf{col}\left\{{\mathit{x}}_{\mathit{0}},\overline{\mathit{u}}\right\}$.

Unfortunately, the partial equivalence between Equations (3) and (11) cannot be found as easily as in [22,23]; thus, some more matrix algebra has to be used to translate Equation (3) into our desired format.

In [20], zero-valued polynomials were introduce to proof that the deterministic quadratic form as Equation (3) has the same stationary point to the error Gramian as Equation (11) in Krein space. On the basis of Theorem 1 and Lemma 2, it can be deduced easily. Therefore, it will not be elaborated any more in this paper.

As we all know, the KF method has a recursive form, which is simply and easily achieved. Therefore, the classical recursion form of KF is used in filters of the Krein space. Consider the discrete-time linear state-space model and its a priori knowledge, shown as Equations (1) and (2); the Riccati recursive steps are summarized as follows:

The estimated state vector:

$${\widehat{\mathit{x}}}_{j+1}={\mathit{A}}_j{\widehat{\mathit{x}}}_j+{\mathit{K}}_j\left({\mathit{y}}_j-{\mathit{B}}_j{\widehat{\mathit{x}}}_j\right)$$

(12)

The filtering gain:

$${\mathit{K}}_j=\left({\mathit{A}}_j{\mathit{P}}_j{\mathit{C}}_j^T+{\mathit{B}}_j{\mathit{S}}_j\right){\mathit{R}}_{e,j}^{-1}$$

(13)

wherein:

$${\mathit{R}}_{e,j}={\mathit{C}}_j{\mathit{P}}_j{\mathit{C}}_j^T+{\mathit{R}}_j$$

(14)

The estimated covariance matrix:

$${\mathit{P}}_{j+1}={\mathit{A}}_j{\mathit{P}}_j{\mathit{A}}_j^T+{\mathit{B}}_j{\mathit{Q}}_j{\mathit{B}}_j^T-{\mathit{K}}_j{\mathit{R}}_{e,j}{\mathit{K}}_j^T$$

(15)

2.2. SINS/CNS Integrated System Model

Although SINS has numerous advantages, the accumulated error caused by its inertial components limits its applications. The CNS can provide accurate attitude information based on the azimuth of the celestial body. Therefore, the SINS/CNS integrated navigation system is widely applied, since the navigation accuracy of the whole system can be enhanced significantly by using the complementary navigation information obtained from two different kinds of sensors. In the SINS/CNS integrated navigation system, the loosely-coupled scheme is used due to its simpleness and convenience. As we all know, the CNS is easy to interfere by the surroundings, and then, the CNS information will be invalid discontinuously. Taking this situation into account, the schematic diagram of the SINS/CNS integrated system is shown in Figure 1.

In this paper, we focus on marine navigation. As we all know, the vertical information (vertical acceleration, vertical velocity and vertical altitude) can be ignored for simplification reasonably and acceptably in surface navigation systems. Thus, only horizontal information is taken into account. Therefore, we choose the error equation of SINS as the state model, including horizontal velocity error equations, longitude and latitude error equations and attitude error equations.

(1) Velocity error equation:

$$\begin{array}{cc}\hfill & \delta {\dot{V}}_E=\frac{V_N}{R_n}tanL\delta V_E+\left(2{\omega }_{ie}sinL+\frac{V_E}{R_n}tanL\right)\delta V_N\hfill \\ & +{\phi }_U{f}_N+{\nabla }_E+\left(2{\omega }_{ie}cosL{V}_N+\frac{V_E{V}_N}{R_n}{sec}^{2}L\right)\delta L\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill \delta {\dot{V}}_N=& -\left(2{\omega }_{ie}sinL+\frac{2V_E}{R_n}tanL\right)\delta V_E-{\phi }_U{f}_E\hfill \\ & +{\nabla }_N-\left(2{\omega }_{ie}cosL{V}_E+\frac{V_E^2}{R_n}{sec}^{2}L\right)\delta L\hfill \end{array}$$

(17)

(2) Position error equation:

$$\delta \dot{L}=\frac{\delta V_N}{R_m}$$

(18)

$$\delta \dot{\lambda }=\frac{\delta V{}_E}{R_n}secL+\frac{V_E}{R_n}tanLsecL\delta L$$

(19)

(3) Attitude error equation

$$\begin{array}{cc}\hfill {\dot{\phi }}_E=& -\frac{\delta V_N}{R_m}+\left({\omega }_{ie}sinL+\frac{V_{E}tanL}{R_n}\right){\phi }_N\hfill \\ & -\left({\omega }_{ie}cosL+\frac{V_E}{R_n}\right){\phi }_U+{\epsilon }_E\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill {\dot{\phi }}_N=& -{\omega }_{ie}sinL\delta L+\frac{\delta V_E}{R_n}\hfill \\ & -\left({\omega }_{ie}sinL+\frac{V_{E}tanL}{R_n}\right){\phi }_E-\frac{V_N}{R_m}{\phi }_U+{\epsilon }_N\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill {\dot{\phi }}_U=& \left({\omega }_{ie}cosL+\frac{V_E}{R_n}{sec}^{2}L\right)\delta L+tanL\frac{\delta V_E}{R_n}\hfill \\ & +\left({\omega }_{ie}cosL+\frac{V_E}{R_n}\right){\phi }_E+\frac{V_N}{R_m}{\phi }_N+{\epsilon }_U\hfill \end{array}$$

(22)

wherein $V_E$ and $V_N$ are the east and north velocities and $\delta V_E$ and $\delta V_N$ are corresponding velocity errors; L and λ are the local latitude and longitude; while $R_m$ and $R_n$ are the Earth’s radii of the meridian circle and the prime vertical circle; $f_E$ and $f_N$ are the measured specific force by the east and north accelerometers; ${\phi }_E,{\phi }_N,{\phi }_U$ are the attitude angle errors; ${\epsilon }_x$, ${\epsilon }_y$ and ${\epsilon }_z$ are the gyro drifts of the x-, y- and z-axes; ${\nabla }_x$ and ${\nabla }_y$ are the accelerator biases of the x- and y-axes. Additionally, we also know this:

$$\left(\begin{array}{c}{\epsilon }_E\\ {\epsilon }_N\\ {\epsilon }_U\end{array}\right)=C_b^n\left(\begin{array}{c}{\epsilon }_x\\ {\epsilon }_y\\ {\epsilon }_z\end{array}\right)$$

(23)

wherein $C_b^n$ is the transformation matrix from the vehicle’s body coordinate system (b) to the navigation coordinate system (n), the size of which is $3\times 3$. A similar transform exists between ${\nabla }_E,{\nabla }_N$ and ${\nabla }_x,{\nabla }_y$.

From the above, the state vector $\mathit{x}$ of the SINS/CNS integrated system is defined as:

$$\mathit{x}\left(t\right)={\left[\begin{array}{cccccccccccc}\delta L& \delta \lambda & \delta V_E& \delta V_N& {\phi }_E& {\phi }_N& {\phi }_U& {\epsilon }_x& {\epsilon }_y& {\epsilon }_z& {\nabla }_x& {\nabla }_y\end{array}\right]}^T$$

(24)

The measurement is the attitude error between the calculated attitude of the SINS and the CNS. Therefore, the observation equation can be described as:

$$\mathit{z}\left(t\right)=\left[\begin{array}{c}{\phi }_E\\ {\phi }_N\\ {\phi }_U\end{array}\right]=H\left(t\right)\mathit{x}\left(t\right)+\mathit{v}\left(t\right)$$

(25)

where $\mathit{z}\left(t\right)$ denotes the measurement, while $\mathit{v}\left(t\right)$ is the measurement noise. The measurement matrix:

$$H\left(t\right)=\left[\begin{array}{ccc}{0}_{3\times 4}& I_{3\times 3}& 0_{3\times 5}\end{array}\right]$$

(26)

3. Robust ${\mathit{H}}_{\mathbf{\infty }}$ Filter Based on Krein Space Theory

In this section, we formulate the Krein space filter recursions for the $H_{\infty }$ filter problem. Taking the missing measurements into account, a novel robust $H_{\infty }$ filter based on the Krein space theory is proposed.

Considered the following discrete-time linear state-space system:

$$\left\{\begin{array}{c}{\mathit{x}}_{j+1}=\left(A_j+\Delta A_j\right){\mathit{x}}_j+B_j{\mathit{u}}_j\hfill \\ {\mathit{y}}_{j+1}=\left(C_{j+1}+\Delta C_{j+1}\right){\mathit{x}}_{j+1}+{\mathit{v}}_{j+1}\hfill \end{array}\right.$$

(27)

wherein ${\mathit{x}}_j$ and ${\mathit{y}}_{j+1}$ are the state vector and measurement vector, ${\mathit{u}}_j$ and ${\mathit{v}}_j$ are unknown system noises and $\Delta A_j$ and $C_j$ are uncertainty parameters. Generally, we assume that:

$$\left(\begin{array}{c}\Delta A_j\\ \Delta C_j\end{array}\right)=\left(\begin{array}{c}{F}_j\\ D_j\end{array}\right){\Delta }_j{E}_j$$

(28)

wherein $A_j,B_j,C_j,D_j,E_j$ and $F_j$ are already known matrices and ${\Delta }_j$ is a unknown matrix with:

$${\Delta }_j^T{\Delta }_j\le I$$

(29)

Given ${\xi }_j={\Delta }_j{E}_j{\mathit{x}}_j$, $s_j=E_j{\mathit{x}}_j$, the system uncertainty can be indicated by the following constraint condition:

$${∥{\xi }_j∥}^2\le {∥s_j∥}^2,j=0,1,\dots ,N$$

(30)

where $∥·∥$ denotes the standard $H_2$ norm.

In a general way, the estimated vector is the linear combination of the state vector. That means:

$${\mathit{z}}_j={\mathit{L}}_j{\mathit{x}}_j$$

(31)

wherein ${\mathit{L}}_j$ is a known coefficient matrix.

Defining that ${\widehat{\mathit{z}}}_j$ is the estimation of ${\mathit{z}}_j$ on the basis of the given observations $\left\{{\mathit{y}}_j\right\}$, the estimation error is:

$$e_j={\mathit{z}}_j-{\widehat{\mathit{z}}}_j$$

(32)

Thus, the estimation problem of the $H_{\infty }$ filter under the level parameter γ can be translated into solving the optimal solution of Equation (27):

$$J={∥T∥}_{\infty }^2={\displaystyle \underset{{\mathit{x}}_0,{\mathit{u}}_j,{\mathit{v}}_j}{sup}}\frac{{\displaystyle \sum _{j=0}^N}{e}_j^T{e}_j}{{{\mathit{x}}_0}^T{{\mathit{P}}_0}^{-1}{\mathit{x}}_0+{\displaystyle \sum _{j=0}^N}{\mathit{u}}_j^T{\mathit{Q}}_j^{-1}{\mathit{u}}_j+{\displaystyle \sum _{j=0}^N}{\mathit{v}}_j^T{\mathit{v}}_j}\le {\gamma }^2$$

(33)

From the above, we can see that the $H_{\infty }$ filter should be designed to ensure that the energy of estimation error $e_k$ is γ times less than the one of system noises. If Equation (33) can be guaranteed, the estimation error will be very small authentically. Substituting, the constraint of uncertain parameters shown as Equation (30) into Equation (33), we will obtain the robust $H_{\infty }$ filter problem:

$$\underset{J_{\infty }\left({\mathit{x}}_0,\mathit{u},\mathit{v}\right)}{\underbrace{{{\mathit{x}}_0}^T{{\mathit{P}}_0}^{-1}{\mathit{x}}_0+{\displaystyle \sum _{j=0}^N}{\mathit{u}}_j^T{\mathit{Q}}_j^{-1}{\mathit{u}}_j+{\displaystyle \sum _{j=0}^N}{\mathit{v}}_j^T{\mathit{v}}_j+{\displaystyle \sum _{j=0}^N}{∥{\xi }_j∥}^2-{\displaystyle \sum _{j=0}^N}{∥s_j∥}^2-\frac{1}{{\gamma }^2}{\displaystyle \sum _{j=0}^N}{e}_j^T{e}_j}}\le \epsilon$$

(34)

wherein ${\mathit{x}}_0$ and ${\mathit{P}}_0$ are the initial state vector and the initial state covariance matrix and ε is a constant.

As was mentioned in [20], Equation (34) generate an ellipsoid set of the estimated state, whose boundary is restrained by ε. Therefore, the estimation of the robust $H_{\infty }$ filter is under this constraint, as well. However, with uncertain parameters, the quadratic function is irreversible directly with the Gramian matrix of the noise error in the Krein space.

Therefore, based on Theorem 1 in [20], vectors in the Hilbert space are indicated by the vectors that have the same meanings in the Krein space. Then, the objective quadratic Equation (34) can be rewritten as:

$$J_{\infty }\left({\mathit{x}}_0,\mathit{u},\mathit{v}\right)={{\mathit{x}}_0}^T{{\mathit{P}}_0}^{-1}{\mathit{x}}_0+{\displaystyle \sum _{j=0}^N}\left\{{\mathit{u}}_j^T{\mathit{Q}}_j^{-1}{\mathit{u}}_j+{\mathit{v}}_j^T{\mathit{v}}_j+{\xi }_j^T{\xi }_j-s_j^T{s}_j-\frac{1}{{\gamma }^2}{e}_j^T{e}_j\right\}$$

(35)

Since that zero vector will not change the quadratic value, the following zero polynomial is introduced:

$$\begin{array}{c}0={\displaystyle \sum _{j=0}^N}\left\{2{\xi }_j^T{\mathit{D}}_j^T{\mathit{R}}_j^{-1}{\mathit{D}}_j{\xi }_j\right.+{\mathit{v}}_j^T{\mathit{R}}_j^{-1}{\mathit{D}}_j{\xi }_j+{\xi }_j^T{\mathit{D}}_j^T{\mathit{R}}_j^{-1}{\mathit{v}}_j\hfill \\ -\left.{\xi }_j^T{\mathit{D}}_j^T{\mathit{R}}_j^{-1}\left({\mathit{v}}_j+{\mathit{D}}_j{\xi }_j\right)-\left({{\mathit{v}}_j+{\mathit{D}}_j{\xi }_j}^T\right){\mathit{R}}_j^{-1}{\mathit{D}}_j{\xi }_j\right\}\hfill \end{array}$$

(36)

Substituting the above equation into Equation (35), we can get:

$$\begin{array}{ll}{J}_{\infty }\left({\mathit{x}}_0,\mathit{u},\mathit{v}\right)=& {{\mathit{x}}_0}^T{{\mathit{P}}_0}^{-1}{\mathit{x}}_0+{\displaystyle \sum _{j=0}^N}\left\{{\mathit{u}}_j^T{\mathit{Q}}_j^{-1}{\mathit{u}}_j-s_j^T{s}_j-\frac{1}{{\gamma }^2}{e}_j^T{e}_j\right.\\ & \left.+{\left(\begin{array}{c}{\xi }_j\\ {\mathit{v}}_j+{\mathit{D}}_j{\xi }_j\end{array}\right)}^T\left(\begin{array}{cc}I+{\mathit{D}}_j^T{\mathit{R}}_j^{-1}{\mathit{D}}_j& -{\mathit{D}}_j^T{\mathit{R}}_j^{-1}\\ -{\mathit{R}}_j^{-1}{\mathit{D}}_j& {\mathit{R}}_j^{-1}\end{array}\right)\left(\begin{array}{c}{\xi }_j\\ {\mathit{v}}_j+{\mathit{D}}_j{\xi }_j\end{array}\right)\right\}\end{array}$$

(37)

After matrix inversion, Equation (37) is equivalent to:

$$J_{\infty }\left({\mathit{x}}_0,\mathit{u},\mathit{v}\right)={{\mathit{x}}_0}^T{{\mathit{P}}_0}^{-1}{\mathit{x}}_0+{\displaystyle \sum _{j=0}^N}{\left(\begin{array}{c}{\mathit{u}}_j\\ {\mathit{D}}_j{\mathit{v}}_j\end{array}\right)}^T\left(\begin{array}{cc}{\mathit{Q}}_j& {\mathit{S}}_j{\mathit{D}}_j^T\\ {\mathit{D}}_j{\mathit{S}}_j^T& {\mathit{D}}_j{\mathit{R}}_j{\mathit{D}}_j^T\end{array}\right)\left(\begin{array}{c}{\mathit{u}}_j\\ {\mathit{D}}_j{\mathit{v}}_j\end{array}\right)$$

(38)

Based on the Equation (7) in the Krein space, the coordinate transformation is as follows:

$$\left(\begin{array}{c}{\mathit{x}}_0\\ \mathit{u}\\ \Omega \mathit{v}\end{array}\right)=\left(\begin{array}{ccc}I& 0& 0\\ 0& I& 0\\ -\Theta & -\mathrm{{\rm Y}}& I\end{array}\right)\left(\begin{array}{c}{\mathit{x}}_0\\ \mathit{u}\\ \mathit{y}\end{array}\right)$$

(39)

Then, the vector form of Equation (38) is:

$$J_{\infty }\left({\mathit{x}}_0,\mathit{u},\mathit{v}\right)={\left(\begin{array}{c}{\mathit{x}}_0\\ \mathit{u}\\ \Omega \mathit{v}\end{array}\right)}^T{\left(\begin{array}{ccc}{\mathit{P}}_0& 0& 0\\ 0& \mathit{Q}& \mathit{S}{\Omega }^T\\ 0& \Omega {\mathit{S}}^T& \Omega \mathit{R}{\Omega }^T\end{array}\right)}^{-1}\left(\begin{array}{c}{\mathit{x}}_0\\ \mathit{u}\\ \Omega \mathit{v}\end{array}\right)$$

(40)

Therefore, the quadratic function can be transformed as the following equation:

$$\begin{array}{c}\hfill J_{\infty }\left({\mathit{x}}_0,\mathit{u},\mathit{y}\right)={\left(\begin{array}{c}{\mathit{x}}_0\\ \mathit{u}\\ \mathit{y}\end{array}\right)}^T{\left\{\left(\begin{array}{ccc}I& 0& 0\\ 0& I& 0\\ \Theta & \mathrm{{\rm Y}}& I\end{array}\right)\left(\begin{array}{ccc}{\mathit{P}}_0& 0& 0\\ 0& \mathit{Q}& \mathit{S}{\Omega }^T\\ 0& \Omega {\mathit{S}}^T& \Omega \mathit{R}{\Omega }^T\end{array}\right){\left(\begin{array}{ccc}I& 0& 0\\ 0& I& 0\\ \Theta & \mathrm{{\rm Y}}& I\end{array}\right)}^T\right\}}^{-1}\left(\begin{array}{c}{\mathit{x}}_0\\ \mathit{u}\\ \mathit{y}\end{array}\right)\end{array}$$

(41)

Obviously, the weighted matrix of the quadratic function and the one of the error covariance matrix in the Krein space are just inverse. Therefore, we can design recursive steps of the $H_{\infty }$ filter in the Krein space.

Considering a state model in the Krein space represented as Equation (1), the recursive steps of the $H_{\infty }$ filter are summarized as follows:

$${\mathit{x}}_{j\left|j-1\right.}={\mathit{A}}_{j-1}{\mathit{x}}_{j-1\left|j-1\right.}$$

(42)

$$\begin{array}{c}{\mathit{P}}_{j\left|j\right.}={\mathit{A}}_{j-1}{\mathit{P}}_{j-1\left|j-1\right.}{{\mathit{A}}_{j-1}}^T+{\mathit{B}}_{j-1}{{\mathit{B}}_{j-1}}^T\hfill \\ -{\mathit{A}}_{j-1}{P}_{j-1\left|j-1\right.}\left[\begin{array}{cc}{C}_j^T& L_j^T\end{array}\right]{\left(R_e\right)}^{-1}\left[\begin{array}{c}{C}_j\\ L_j\end{array}\right]P_{j-1\left|j-1\right.}{{\mathit{A}}_{j-1}}^T\hfill \end{array}$$

(43)

$${\left(R_e\right)}^{-1}=\left[\begin{array}{cc}I& 0\\ 0& -{\gamma }^{2}I\end{array}\right]+\left[\begin{array}{c}{C}_j\\ L_j\end{array}\right]P_{j-1\left|j-1\right.}\left[\begin{array}{cc}{C}_j^T& L_j^T\end{array}\right]$$

(44)

$$K_j=P_{j\left|j\right.}{C}_j^T{\left(I+C_j{P}_{j\left|j\right.}{C}_j^T\right)}^{-1}$$

(45)

$${\mathit{x}}_{j\left|j\right.}={\mathit{x}}_{j\left|j-1\right.}+K_j\left({\mathit{y}}_j-C_j{\mathit{x}}_{j\left|j-1\right.}\right)$$

(46)

Compared to the KF, the proposed filter is much more robust. Actually, when $\gamma \to \infty$, the $H_{\infty }$ filter is just the KF exactly. Additionally, the less γ is, the stronger the robustness of the system. Using the above $H_{\infty }$ filter, the uncertainty of the system noise can be eliminated availably. Hence, taking void measurement into account, we introduce a new parameter α into the presented robust $H_{\infty }$ filter. Assume that the coefficient of the void measurement is indicated as $\alpha \left(1\ge \alpha \ge 0\right)$:

when $\alpha =1$, the CNS measurement is constantly available;

when $\alpha <1$, the CNS measurement is lost.

That means, the smaller α is, the more the measurement is invalid. Therefore, the proposed novel robust $H_{\infty }$ filter in the Krein space can be rewritten as the following reliability:

The predicted state is still as Equation (42), while the predicted measurement is:

$${\widehat{\mathit{y}}}_j=\alpha C_j{\mathit{x}}_{j\left|j-1\right.}$$

(47)

To derive the filtering steps conveniently, we define some intermediate variables:

$$C_j^{\infty }=\left[\begin{array}{c}\alpha C_j\\ L_k\end{array}\right]$$

(48)

$${\left(R_e^{\infty }\right)}^{-1}=C_j^{\infty }{P}_{j-1\left|j-1\right.}{\left(C_j^{\infty }\right)}^T+\left[\begin{array}{cc}\alpha \left(1-\alpha \right)C_j{C}_j^T& 0\\ 0& -{\gamma }^{2}I\end{array}\right]$$

(49)

$$K_e^{\infty }=P_{j-1\left|j-1\right.}\left(C_j^{\infty }\right){\left(R_e^{\infty }\right)}^{-1}$$

(50)

$$P_j^{\infty }=\left(I-K_e^{\infty }{C}_j^{\infty }\right)P_{j-1\left|j-1\right.}$$

(51)

Therefore, the estimated covariance matrix:

$$P_{j\left|j\right.}={\mathit{A}}_{j-1}{P}_j^{\infty }{{\mathit{A}}_{j-1}}^T+{\mathit{A}}_{j-1}{{\mathit{A}}_{j-1}}^T$$

(52)

The filtering gain is:

$$K_j=P_j^{\infty }{C}_j^T\left(I+\alpha \left(1-\alpha \right)I{C}_j{C}_j^T\right)$$

(53)

The estimated state:

$${\mathit{x}}_{j\left|j\right.}={\mathit{x}}_{j\left|j-1\right.}+K_j\left({\mathit{y}}_j-{\widehat{\mathit{y}}}_j\right)$$

(54)

In the SINS/CNS integrated system, the measurement of the CNS is invalid occasionally for the environmental disturbance. To solve various uncertainty problems, the robust $H_{\infty }$ filter introduces the $H_{\infty }$ norm, which is the robust design parameter. In this filter, the noise and the uncertainty are regarded as the limited energy random signal. Then, the filter can be designed based on the objective quadratic that the $H_{\infty }$ norm of the transfer function from the system interference to the estimated error is less than a given positive threshold.

4. Simulations and Experiments

To verify and estimate the performance of the proposed robust filter, simulations and experiments are performed in this section.

4.1. Simulations and Analysis

First of all, numerical simulations have been done. Suppose that the initial parameters of a marine vehicle are given and shown as

Table 1. Simulation parameters.

Parameters	Values
initial latitude	$L=40.{2631049}^{\circ }$
initial longitude	$\lambda =120.{886482}^{\circ }$
initial velocity	$v_x=v_y=0$
gravity acceleration	$g_0=9.7805$ m/s${}^2$
initial misalignment angles	${\varphi }_x={\varphi }_y={\varphi }_z={20}^{\prime }$
constant drifts of the gyroscopes	${\epsilon }_x={\epsilon }_y={\epsilon }_z=0.{01}^{\circ }$/h
random noise of the gyroscopes	$W_{gx}=W_{gy}=W_{gz}=0.{05}^{\circ }$/h
constant biases of the accelerometers	${\nabla }_x={\nabla }_y={10}^{-4}{g}_0$
random noise of the accelerometer	$W_{ax}=W_{ay}=5*{10}^{-5}{g}_0$
sampling frequency	98 Hz

:

According to Section 2.2, the state vector is composed of the position errors $\delta L$ and $\delta \lambda$, the velocity errors $\delta V_E,\delta V_N$, the misalignment angle errors ${\varphi }_E,{\varphi }_N,{\varphi }_U$, the gyroscope constant drifts ${\epsilon }_x,{\epsilon }_y,{\epsilon }_z$ and the accelerometer constant biases ${\nabla }_x,{\nabla }_y$. The measurement vector is the attitude error between the SINS and CNS. On the basis of Equations (16)–(26), the model of the SINS/CNS integrated system can be expressed clearly.

In order to estimate the performance of the proposed filter, the normal KF is used as the reference filter. The initial state vector is:

$$\begin{array}{c}\hfill {\mathit{x}}_0={\left[\begin{array}{cccccccccccc}0\mathrm{m}& 0\mathrm{m}& 0\text{m/s}& 0\text{m/s}& {20}^{\prime }& {20}^{\prime }& {20}^{\prime }& {0.01}^{\circ }/h& {0.01}^{\circ }/h& {0.01}^{\circ }/h& {10}^{-4}{g}_0& {10}^{-4}{g}_0\end{array}\right]}^T\end{array}$$

(55)

Additionally, the corresponding covariance matrix is:

$$\begin{array}{cc}{P}_0=diag\hfill & \left\{{\left(10/\phantom{10\mathrm{Re}}\mathrm{Re}\right)}^2,{\left(10/\phantom{10\mathrm{Re}}\mathrm{Re}\right)}^2,{0.1}^2,{0.1}^2,{\left({20}^{\prime }\right)}^2,{\left({20}^{\prime }\right)}^2,{\left({20}^{\prime }\right)}^2,\right.\hfill \\ & \left.{\left({0.01}^{\circ }/h\right)}^2,{\left({0.01}^{\circ }/h\right)}^2,{\left({0.01}^{\circ }/h\right)}^2,{\left({10}^{-4}{g}_0\right)}^2,{\left({10}^{-4}{g}_0\right)}^2\right\}\hfill \end{array}$$

(56)

wherein $Re$ indicates the Earth’s radius, $Re=$ 6,378,393.0 m.

Under the same simulation conditions, the proposed robust $H_{\infty }$ filter and the normal KF were applied to estimate the states of the SINS/CNS integrated navigation system. We compared the errors of misalignment angles in different conditions when $\alpha =0.75$ and $\alpha =0.5$, respectively. The estimated results are shown in Figure 2 and Figure 3, respectively. To compare the running times of these two filters, Monte Carlo simulations are carried out 20-times on the same computer equipped with an Intel Core 2 T6570, 2.1-GHz processor and 3 GB RAM under Windows XP. Additionally, the means of the running times are compared in

Table 2. Comparisons of the running times.

	Running Time (s)
	Kalman Filter	Proposed Filter
$\alpha =0.5$	$0.6972$	$0.9457$
$\alpha =0.75$	$0.7146$	$0.9691$

.

From Figure 2 and Figure 3, it is obvious that when $\alpha =0.75$, the heading errors with the KF and proposed robust $H_{\infty }$ filter are 4’ and −0.3’, respectively; when $\alpha =0.5$, the heading error with the robust $H_{\infty }$ filter is merely 0.4’, while the one with KF is about 7’. The average running times of normal Kalman filter are 0.7146 s and 0.6972 s, while the average times of the proposed robust filter are 0.9691 s and 0.9457 s when $\alpha =0.75$ and $\alpha =0.5$, respectively. Therefore, we can know that obviously when the CNS measurement is lost occasionally, the estimated accuracy of the normal KF is decreased severely from 4’ to 7’. In addition, the more measurements are lost, the worse the estimated results are. However, under these two conditions, utilizing the proposed robust $H_{\infty }$ filter, the misalignment angles not only can be estimated availably, but also have better convergence rates and stability than the KF method at a cost of few computations. Therefore, the robustness and superiority of the proposed $H_{\infty }$ filter can be verified.

4.2. Experiments and Analysis

To further validate the performance of the proposed robust $H_{\infty }$ filter based on the Krein space theory, some experiments are carried out, as well. In these experiments, the SINS and star sensor are fixed on the ship’s deck together. The schematic of experimental setup and sensors are shown in Figure 4 and Figure 5, respectively. In the SINS, the inertial measurement unit (IMU) is composed of the accelerometer and the gyroscope, which was developed by our lab and shown as the left picture in Figure 5. The performance parameters of the SINS and the star sensor are detailed in

Table 3. Main parameters of the SINS and star sensor.

Sensors	Parameters	Values
Gyro	Dynamic range	$\pm {100}^{\circ }$/s
	Bias stability	$\le 0.{01}^{\circ }$/h
	Random walk	$\le 0.{005}^{\circ }/\sqrt{h}$
	Scale factor stability	$\le 20$ ppm
Accelerometer	Dynamic range	$\pm 4$ g
	Bias stability	$\le {10}^{-4}$ g
	Random walk	$\le 5*{10}^{-5}$ g
	Scale factor stability	$\le 20$ ppm
Star Sensor	Field of view	${24}^{\circ }$
	Attitude accuracy	$5^{\prime \prime }$
	Data update frequency	20 Hz

.

During the experiment, the lens of the star sensor is covered discontinuously to simulate the invalid state of the star Sensor from about 22 h to 24 h. In addition, the GPS is also used as the reference information, shown as the right picture in Figure 5. Therefore, in the experiments, the SINS/GPS integrated system can be used as the standard to estimate the accuracy of the SINS/CNS integrated system.

Figure 6 and Figure 7 give the experiment results of the SINS/CNS integrated system with the normal KF and robust $H_{\infty }$ filter based on the Krein space theory, respectively.

From the experiment results, we can see that the angle errors are vibrational when the star sensor is covered from about 22 h to 24 h. For the pitch error, the amplitude values are about $0.{14}^{\circ }$ and $0.{11}^{\circ }$ with the normal KF and with the proposed robust filter, respectively, while the values of the roll error are $0.{44}^{\circ }$ and $0.{21}^{\circ }$. Compared to the normal KF method, the horizontal and azimuth angle errors of the SINS/CNS integrated system are a bit smaller with the new robust filter based on the $H_{\infty }$ filter and the Krein space theory. Regarding the yaw error, the maximal errors of the normal KF and the proposed robust filter have an extremely great distance. With the traditional KF method, the maximal are $38.{76}^{\circ }$, as the one with the proposed robust filter algorithm is nearly $0.{69}^{\circ }$. Therefore, with the proposed robust filter, the attitude errors can be decreased dramatically when the measurements of the integrated system are uncertain or lost. Furthermore, the robustness of this novel robust filter is also verified.

5. Conclusions

In order to solve the uncertainty problem of the SINS/CNS integrated navigation system caused by the missing measurements, a novel robust $H_{\infty }$ filter based on the Krein space theory was proposed in this manuscript. Firstly, the system model of the SINS/CNS integrated navigation system was established, and then, a novel robust filter taking the uncertainty problem into account was proposed. Then, the superiority of the Krein space was described in principle, and the derivational process of the novel robust $H_{\infty }$ filter was presented in detail. Numerical simulations and experiments were carried out to verify the new robust $H_{\infty }$ filter. The results proved the advantages of the presented robust $H_{\infty }$ filter on the basis of the Krein space theory, which can improve the navigation accuracy of the integrated navigation system availably when the CNS measurements are lost. Therefore, the feasibility and the superiority of this new robust filter were verified. However, the system model of the SINS/CNS integrated system was assumed as a linear model, which is clearly unrealistic in practical applications. Therefore, our future work will focus on the nonlinear robust filter, which is suitable for practical systems.