A Fast North-Finding Algorithm on the Moving Pedestal Based on the Technology of Extended State Observer (ESO)

Abstract

We propose a kind of fast and high-precision alignment algorithm based on the ESO technology. Firstly, in order to solve the problems of rapid, high-accuracy, and anti-interference alignment on the moving pedestal in the north-seeker, the ESO technology in control theory is introduced to improve the traditional Kalman fine-alignment model. This method includes two stages: the coarse alignment in the inertial frame and fine alignment based on the ESO technology. By utilizing the ESO technology, the convergence speed of the heading angle can be greatly accelerated. The advantages of this method are high-accuracy, fast-convergence, strong ability of anti-interference, and short time-cost (no need of KF recursive calculation). Then, the algorithm model, calculation process, and the setting initial-values of the filter are shown. Finally, taking the shipborne north-finder based on the FOG (fiber-optic gyroscope) as the investigated subject, the test on the moving ship is carried out. The results of first off-line simulation show that the misalignment angle of the heading angle of the proposed (traditional) method is ≤2.1′ (1.8′) after 5.5 (10) minutes of alignment. The results of second off-line simulation indicate that the misalignment angle of the heading angle of the proposed (traditional) method is ≤4.8′ (14.2′) after 5.5 (10) minutes of alignment. The simulations are based on the ship-running experimental data. The measurement precisions of Doppler velocity log (DVL) are different in these two experiments.

1. Introduction

North-seeking (i.e., alignment of the attitude angles) is very important for people’s life and scientific research. The north-seeker is an ideal orientation equipment for mobile launch of the strategic, tactical, and campaign weapons. It can provide azimuth reference for missiles, radars, artillery, and vehicles. The portability, automation, rapidity of north-finding, and high-precision are the development-trend of the north-seeker. With the development of inertial-navigation technology, gyro north-seeker has been widely used in the military and civil field. It does not need external information and can find north independently all day and all time. It has always been a research hotspot. Presently, it is urgently hoped that the north-finder can not only be high-precision but also be rapid (the short north-finding time).

The performance indexes (the accuracy and time of north-seeking) of the gyro north-seeker are mainly affected by the north-finding principle and calculation method. Recently, the methods of gyro north-seeking mainly include the methods of compass and rotation-velocity measurement. The north-finding accuracy of the compass method is high (generally higher than 5″ (1δ)). However, its north-seeking time is long (generally more than 10 min). Therefore, it is difficult to miniaturize, and its cost is very high. The method of rotation-velocity measurement utilizes the gyroscope to measure the horizontal north-component of the earth’s rotation-angle-velocity (ω_ie), and the angle between the gyroscope’s main-axis and the true north direction is calculated. This article mainly studies the north-seeker based on the method of rotation-velocity measurement.

At present, the research about north-seeker includes basic inertial navigation error model [1,2], observability analysis [3,4], in-drilling alignment with general dynamic error model [5], alignment algorithms based on gyro-compassing mode [6,7], alignment based on the interacting multiple model and the Huber methods [8], rapid fine alignment under marine mooring condition [9], alignment for SINS (strapdown inertial navigation system) in vehicular environment [10], alignment based on Riccati Equation and EM (expectation-maximization) convergence [11], alignment based on adjustment on separate-bias Kalman filter [12], application of nonlinear filtering in alignment [13], initial attitude estimation of tactical grade inertial measurement [14], alignment with robust adaptive unscented Kalman filter [15], alignment based on a group of double direct spatial isometries [16], alignment with state-dependent extended Kalman filter [17], application of redundant technology in north-finding [18], two-position algorithm [19,20], multi-position algorithm [21,22], rotary-modulation algorithm [23,24,25], nonlinear filter model for large misalignment angle [26], transfer north-seeking algorithm [27,28,29], transfer alignment based on cubature Kalman filter (CKF) method [30], north-finding based on the neural network technology [31,32], transfer algorithm based on sensors network and estimation of wing flexure deformation [33], fast stationary initial alignment based on extended measurement information [34], accurate fine alignment based on adaptive extended Kalman filters [35], compact north-seeker technology [36], north-seeking based on the information fusion technology [37], and so on. Most of the theory and application research about the north-seeking algorithm of strapdown inertial navigation system (SINS) adopts the Kalman filter (KF) method. However, the weak observability of the inertial navigation system affects the convergence speed and estimation accuracy of the filter for the state estimation. Then, it impacts the accuracy and rapidity of north-seeking algorithm [23], resulting in the contradiction between the accuracy and rapidity. In order to solve this contradiction, by analyzing the error model and the factors affecting the algorithm accuracy on the moving base (e.g., ship and vehicle), a method for high-precision and quick north-seeking is proposed.

As early as 1988, some scholars had proposed that the automatic control theory can be applied in the study of north-seeker [38]. Various observers are widely used in the control field. The bias observers are used for a class of uncertain nonlinear systems [39]. The ESO is utilized in the active disturbance rejection control [40], model free predictive current control [41], incremental actor faults [42], robust predictive current control [43], heat pump system [44], non-iteration predicted entry guidance [45], active disturbance rejection control [46], path following control for underactuated vehicles [47], and shipborne transfer alignment under the uncertain disturbance [48].

In the following, some existing research work closely related to this paper are selected for detailed discussion. The literature [16] had proven that the group state model satisfies a particular “group affine” property and the corresponding error model satisfies a “log-linear” autonomous differential equation on the Lie algebra. Based on this property, the attitude can be estimated based on the linear error model with even extreme large misalignments. Hence, the coarse alignment can be omitted. The simulation results of intermediate-grade SINS indicate that the pitch, roll, and heading misalignment angles are ≤6′, ≤6′, and ≤52′ after 150 s, 150 s, and 200 s of alignments, respectively. Ref. [17] proposed a novel initial alignment method by integrating the advantages of the special orthogonal group of order 3 [SO(3)] representation. This method is called as the state-dependent extended Kalman filter (SDEKF). The experimental results of navigation-grade SINS indicate that the pitch, roll and heading misalignment angles are ≤0.9′, ≤1.0′, and ≤5.6′ after 413 s of alignment, respectively. However, the mathematical fundamentals of these two alignments [16,17] are obscure. Additionally, the models, calculation process, and deductions of these methods are complicated. The article [18] had studied the alignment of regular tetrahedron RSINS (Redundant-SINS). A 13-dimensional error state equation and the Kalman filter model of fine alignment were established. The emulation results of navigation-grade SINS indicate that the pitch, roll, and heading misalignment angles are ≤0.6′, ≤0.6′, and ≤5.8′ after 3 s, 3 s, and 150 s of alignments, respectively. However, this method requires redundant inertial sensors. Hence, the structure and fabrication of SINS is complicated, and its cost is high. The literature [19] proposed a high accuracy two-position alignment for lunar rovers. Ref. [20] has proposed a rapid SINS two-position ground alignment. A two-position ground alignment algorithm for SINS is designed based on the proposed piecewise combined Kalman filter. The calculation results of navigation-grade SINS indicate that the heading misalignment angle is ≤0.5′ after 300 s of north-finding. A kind of multi-position alignment algorithm was studied in refs. [21,22]. The advantages of this kind of algorithm are high-accuracy and short time-cost. Especially, the performance of multi-position alignment algorithm is better than two-position alignment algorithm. However, such algorithms all require a precise rotating mechanism, some need the assistance of star sensor [19], and some are only suitable for static base [20]. Therefore, these algorithms are not suitable for the portable shipborne north-finder. Ref. [23] proposed a fast initial alignment based on the error model of the SINS. The algorithm is high accuracy, but it can only be applied to static base. Refs. [24,25] had proposed an alignment method based on the technology of continuous-rotation modulation. The performance of this method is better than the multi-position alignment. However, this method also requires a precise rotating mechanism, which will greatly increase the weight and volume of the north-finder. The literature [26] has introduced nonlinear-filtering technology into alignment, which has improved accuracy and shortened convergence time. However, the calculation process of this method is complex and requires a chip or hardware platform with high performance. This will increase the cost of the north-finder. Refs. [27,28,30], respectively, proposed the applications of KF, EKF (extended KF), and CKF to transfer alignment. This kind of algorithm has very high accuracy and extremely short time-cost. However, it needs a high-precision navigation coordinate system (such as north-east-up frame) as the reference benchmark. Therefore, it is not suitable for north-finder, which acquires autonomous north-seeking. The literature [31] proposed the initial alignment based on wavelet neural network. This algorithm is very advanced and has excellent performance. However, compared with the traditional KF alignment algorithm, its calculation process is much more complex. Ref. [33] proposed a transfer alignment based on inertial sensors network. The accuracy of this transfer algorithm is very high. However, because it needs a network of sensors, the cost is very high when it is applied in engineering practices. Ref. [34] proposed a fast initial alignment based on extended measurement information. This method introduces many new observations and greatly improves the alignment accuracy. However, it can only be applied to static pedestals. The literature [35] has studied a fine alignment based on adaptive extended KF. This method negates the need for multi-position rotation or a complex rotating mechanism. The fine alignment process is aimed to reduce initial attitude and inertial sensor errors before system operation. Results show that a proper choice of an adaptive filter in the alignment process can greatly improve the accuracy of the navigation error-states. The numerical-simulation results of MEMS (micro-electromechanical systems) SINS indicate that the pitch and roll misalignment angles are ≤78′ and ≤79′ after 2 s of alignments, respectively. This method not only can provide the fast and accurate estimation of the alignment error but also can predict the gyro bias online. However, the establishment and improvement of the extended measurement equation is complicated. Ref. [48] studied a shipborne transfer alignment based on ESO. This study verifies that ESO can improve the accuracy and anti-interference ability of transfer alignment. A method has been well designed to improve the Kalman filter techniques based on the extended state observer. The usage of the method makes it possible to estimate the disturbance of the uncertain model and realizes dynamic feedback linearization in the inertial navigation system for ship during the process of transfer alignment. The emulation results of navigation-grade SINS indicate that the pitch, roll, and heading misalignment angles are ≤2′, ≤2′, and ≤3′ after 10 s, 10 s, and 15 s of alignments, respectively. However, the application of transfer alignment is limited, compared with self-alignment. The study of this paper should be expanded and deepened.

In order to make up for the above deficiencies, in this article, the ESO and tracking differentiator (TD) are used to the north-finding algorithm on the motive base. The innovations of this paper include: ① In order to solve the problems of slow convergence, large amount of calculations, and poor anti-interference ability of KF fine alignment, the proposed method introduces ESO into the autonomous alignment and quickly obtains more accurate east and north misalignment angles. ② The differential of the east misalignment angle is required to calculate the azimuth misalignment angle. However, it is difficult to obtain this differential value. Here, TD filter is used to calculate the differential value of the east misalignment angle. Finally, the azimuth misalignment angle is obtained from this differential value and the north misalignment angle. ③ For faster convergence, the parameters of ESO and TD filters are determined by optimization software through a large number of simulations. This method promotes the convergence speed of all the pitch, roll, and heading misalignment angles. The advantages of this method are high accuracy, fast convergence, strong ability of anti-interference, and short time-cost (only ESO filter and TD filter equations need to be solved, and KF recursive calculation is not required). The disadvantage of this method is that only three misalignment angles can be obtained, while the KF alignment method can estimate a set of navigation parameters. If other navigation parameters (such as position, velocity, and so on) are needed after fine alignment for calculation of integrated navigation, the KF is indispensable.

The organization of this paper is as follows: firstly, the method of coarse alignment is introduced. Then, the KF model of fine alignment is established, and the KF model is simplified according to the motion characteristics of the ship, and the expressions of ESO filter and TD filter are derived. Next, the algorithm is tested on a static base. Then, the accuracy, convergence speed, and anti-interference ability of the algorithm are verified by experiments of ship running. Finally, a summary is given.

The innovations of this method are summarized as follows: (1) The ESO filter is introduced to replace the traditional KF fine alignment algorithm to obtain the roll and pitch angles. (2) Combined with the results of ESO, the TD filter is utilized to achieve accurate and fast alignment of heading angle. (3) The parameters of ESO and TD filters are adjusted to be a set of optimized values. The advantages of the proposed algorithm are: ① The convergence of heading angle is fast. ② The alignment precision is high. ③ The ability of anti-interference is relatively strong. ④ The consumed computation time is short, and the required hardware resources are very few.

2. Theory

As mentioned above, this method includes two stages: ① Coarse alignment in the inertial frame. ② Fine alignment based on the ESO technology. They are explained as follows.

2.1. Coarse Alignment in Inertial Frame

Two important inertial coordinate systems are defined: ① the inertial system (b₀) of carrier at the initial time. It coincides with the carrier coordinate system (b) at the beginning of the north-seeking. Subsequently, no rotation of b₀ frame relative to the inertial space exists; ② the navigation inertial system (n₀) at the initial time. It coincides with the navigation coordinate system (i.e., the geographic coordinate system, n frame) at the beginning of the north-seeking. Later, no rotation of n frame relative to the inertial space occurs. The key to indirect initial alignment method is to solve the azimuth relationship between systems, that is, constant matrix $C_{b_0}^{n_0}$. In this article, the variable representing vector or matrix is written in bold.

Referring to Figure 1, one can observe the gravity vector ($g_0^{n_0}$) of a fixed point on the earth’s surface in the inertial coordinate system. Then, the direction of the vector will gradually change with the rotation of earth. It will rotate exactly once within 24 h to form a cone. The direction of $g_0^{n_0}$ is the sky-ground direction. Additionally, the direction of change rate (${\dot{g}}_0^{n_0}$) of $g_0^{n_0}$ is the east-west direction. Therefore, the horizontal and azimuth information of geographic coordinate system is included in the $g_0^{n_0}$ and ${\dot{g}}_0^{n_0}$. In Figure 1, $g_0^{n_0}$, $g_t^{n_0}$ and ${\omega }_{ie}^{}$ are the $g_{}^{n_0}$ at the initial time (0 time), $g_{}^{n_0}$ at the t time (0 time) and the rotation rate of the earth. The $g_{}^{n_0}$ is the projection of the gravity vector $g$ in the n₀ coordinate system.

Firstly, the projection of $g$ in the n₀ frame is:

$$g^{n_0}=C_n^{n_0}{g}^n$$

(1)

where $g^n={\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^{\mathrm{T}}$ is a constant vector, and $C_n^{n_0}$ is attitude transition matrix from n frame to n₀ frame. The equation about $C_n^{n_0}$ can be obtained:

$${\dot{C}}_n^{n_0}=C_n^{n_0}({\omega }_{n_{0}n}^n\times )=C_n^{n_0}({\omega }_{ie}^n\times )$$

(2)

Since ${\omega }_{ie}^n=\left[0\hspace{1em}{\omega }_{ie}^{}\cos L\hspace{1em}{\omega }_{ie}^{}\sin L\right]$ is a constant value, the rotation (${\omega }_{n_{0}n}^{}$) of n frame relative to n₀ frame is around a fixed axis. ${\omega }_{n_{0}n}^n$ is the rotation of n frame relative to n₀ frame projected in the n frame. Here, ω_ie is the rotation rate of the earth, and L is the latitude. According to Equation (2), we can obtain:

$$\begin{array}{ll}\hfill C_n^{n_0}& ={\mathrm{e}}^{(t{\omega }_{ie}^n\times )}=I+\frac{\sin {\omega }_{ie}t}{{\omega }_{ie}t}(t{\omega }_{ie}^n\times )+\frac{1-\cos {\omega }_{ie}t}{{({\omega }_{ie}t)}^2}{(t{\omega }_{ie}^n\times )}^2\hfill \\ & =\left[\begin{array}{ccc}\cos {\omega }_{ie}t& -\sin {\omega }_{ie}t\sin L& \sin {\omega }_{ie}t\cos L\\ \sin {\omega }_{ie}t\sin L& 1-(1-\cos {\omega }_{ie}t){\sin }^{2}L& (1-\cos {\omega }_{ie}t)\sin L\cos L\\ -\sin {\omega }_{ie}t\cos L& (1-\cos {\omega }_{ie}t)\sin L\cos L& 1-(1-\cos {\omega }_{ie}t){\cos }^{2}L\end{array}\right]\hfill \end{array}$$

(3)

Hence,

$$g^{n_0}=-g\left[\begin{array}{c}\sin {\omega }_{ie}t\cos L\\ (1-\cos {\omega }_{ie}t)\sin L\cos L\\ 1-(1-\cos {\omega }_{ie}t){\cos }^{2}L\end{array}\right]$$

(4)

Here, t is the time. Secondly, the specific force output ($f_{\mathrm{sf}}^{b_0}$) of the accelerometer is projected in the b₀ frame as:

$$f_{\mathrm{sf}}^{b_0}=C_b^{b_0}{f}_{\mathrm{sf}}^b$$

(5)

where $f_{\mathrm{sf}}^b$ is the specific force output of the accelerometer projected in the b frame. $C_b^{b_0}$ is attitude transition matrix from b frame to b₀ frame. The equation about $C_b^{b_0}$ can be obtained where

$${\dot{C}}_b^{b_0}=C_b^{b_0}({\omega }_{b_{0}b}^b\times )=C_b^{b_0}({\omega }_{ib}^b\times )$$

(6)

Here, ${\omega }_{ib}^b$ is the measured value of the gyroscope. The initial value ($C_b^{b_0}(0)$) of the attitude matrix is I (identity matrix). ${\omega }_{b_{0}b}^b$ is the rotation of b frame relative to b₀ frame projected in the b frame. The real-time attitude matrix ($C_b^{b_0}$) can be obtained by using the algorithm of attitude update. There is no need to limit the magnitude of ${\omega }_{ib}^b$. Therefore, the ability of anti-angular-shaking interference of this coarse north-seeking algorithm is strong.

Finally, the relationship between the gravity acceleration and measurement of specific force of accelerometer is established by $C_{b_0}^{n_0}$. These results are obtained:

$$C_n^{n_0}(C_b^n{\tilde{f}}_{\mathrm{sf}}^b-{\stackrel{⌢}{\nabla }}^n)=C_n^{n_0}(-g^n)$$

(7)

That is:

$$C_{b_0}^{n_0}(C_b^{b_0}{\tilde{f}}_{\mathrm{sf}}^b-{\stackrel{⌢}{\nabla }}^{b_0})=-g^{n_0}$$

(8)

where ${\stackrel{⌢}{\nabla }}^{b_0}=C_b^{b_0}\mathsf{\delta }{f}_{\mathrm{sf}}^b+{\dot{v}}^{b_0}$ refers to the measurement error ($\mathsf{\delta }{f}_{\mathrm{sf}}^{b_0}$) of the accelerometer and the interference of linear acceleration (${\dot{v}}^{b_0}$) in the b₀ frame. The ${\tilde{f}}_{\mathrm{sf}}^b$ is defined as ${\tilde{f}}_{\mathrm{sf}}^b=f_{\mathrm{sf}}^b+\delta f_{\mathrm{sf}}^b$.

By Equation (8), theoretically, the two measured values of g and ${\mathit{f}}_{\mathit{sf}}^{\mathit{b}}$ at two different times are obtained, and two matrix equations are established; $C_{b_0}^{n_0}$ can be solved by the algorithm of attitude determination based on the double vectors. However, in order to reduce the influence of the interference of linear motion, we can integrate Equation (8) during the whole coarse north-seeking and obtain:

$$\left\{\begin{array}{l}{\tilde{F}}_i^{b_0}={\displaystyle {\int }_0^{t_i}{C}_b^{b_0}{\tilde{f}}_{\mathrm{sf}}^b\mathrm{d}t}\\ G_i^{n_0}=-{\displaystyle {\int }_0^{t_i}{g}^{n_0}\mathrm{d}t}\end{array}\right.(i=1,2)$$

(9)

where ${\tilde{F}}_i^{b_0}$ and $G_i^{n_0}$ are integrated specific force (including the bias of the accelerometer) and gravity acceleration projected in the n₀ frame.

This result is acquired:

$$C_{b_0}^{n_0}{\tilde{F}}_i^{b_0}-{\displaystyle {\int }_0^{t_i}{C}_{b_0}^{n_0}{\stackrel{⌢}{\nabla }}^{b_0}\mathrm{d}t}=G_i^{n_0}$$

(10)

Usually, t₂ = 2t₁ and t₂ are taken as the end time of the coarse north-seeking.

According to the algorithm of attitude determination based on the double vectors, by ignoring the influence of ${\stackrel{⌢}{\nabla }}^{b_0}$ in Equation (9), Equation (10) can be obtained:

$${\widehat{C}}_{b_0}^{n_0}=\left[\frac{G_1^{n_0}}{\left|G_1^{n_0}\right|}\hspace{1em}\frac{G_1^{n_0}\times G_2^{n_0}}{\left|G_1^{n_0}\times G_2^{n_0}\right|}\hspace{1em}\frac{G_1^{n_0}\times G_2^{n_0}\times G_1^{n_0}}{\left|G_1^{n_0}\times G_2^{n_0}\times G_1^{n_0}\right|}\right]\left[\begin{array}{c}{({\tilde{F}}_1^{b_0})}^T/\left|{\tilde{F}}_1^{b_0}\right|\\ {({\tilde{F}}_1^{b_0}\times {\tilde{F}}_2^{b_0})}^T/\left|{\tilde{F}}_1^{b_0}\times {\tilde{F}}_2^{b_0}\right|\\ {({\tilde{F}}_1^{b_0}\times {\tilde{F}}_2^{b_0}\times {\tilde{F}}_1^{b_0})}^T/\left|{\tilde{F}}_1^{b_0}\times {\tilde{F}}_2^{b_0}\times {\tilde{F}}_1^{b_0}\right|\end{array}\right]$$

(11)

Here, ${\widehat{C}}_{b_0}^{n_0}$ represents the calculated attitude matrix at the initial time of rough north-seeking. In order to obtain the calculated attitude matrix ${\widehat{C}}_b^n$ at the end time of rough north-seeking, the following chain multiplication formula can be utilized:

$${\widehat{C}}_b^n=C_{n_0}^n{\widehat{C}}_{b_0}^{n_0}{C}_b^{b_0}$$

(12)

where $C_{n_0}^n$ and $C_b^{b_0}$ are ideal attitude matrix, and they are determined by Equations (3) and (6), respectively.

Compared with analytical rough alignment, the ability of anti-angular-shaking interference of this alignment method is better. However, the ability of anti linear-shaking interference of this method is relatively weak during a short time. If the coarse alignment time is properly extended (generally in the order of minutes), then this method can often achieve good results. This method for coarse north-finding is very practical, and it has been widely applied in many north-seekers.

2.2. Fine Alignment Algorithm

Before introducing the ESO method, we bring in the traditional fine alignment algorithm firstly.

The relationship between V_N (V_E) and $\dot{L}$ ($\dot{\lambda }$) is expressed as Equation (13):

$$\left\{\begin{array}{l}\dot{L}=V_N/R\\ \dot{\lambda }=V_E\sec L/R\end{array}\right.$$

(13)

Here, V_N, V_E, λ, and R are northward velocity of the carrier, eastward velocity, longitude, and earth radius, respectively. $\dot{L}$ and $\dot{\lambda }$ are differentials of $L$ and $\lambda$.

The error model of north-seeker (or strapdown inertial navigation) under the condition of moving base is established as follows:

$$\left[\begin{array}{c}\delta {\dot{V}}_E\\ \delta {\dot{V}}_N\\ {\dot{\varphi }}_E\\ {\dot{\varphi }}_N\\ {\dot{\varphi }}_U\end{array}\right]=\left[\begin{array}{ccccc}\frac{V_N}{R}tgL& (2\mathsf{\Omega }+\dot{\lambda })SL& 0& -f_U^n& f_N^n\\ -2(\mathsf{\Omega }+\dot{\lambda })SL& 0& f_U^n& 0& -f_E^n\\ 0& -\frac{1}{R}& 0& (\mathsf{\Omega }+\dot{\lambda })SL& -(\mathsf{\Omega }+\dot{\lambda })CL\\ \frac{1}{R}& 0& -(\mathsf{\Omega }+\dot{\lambda })SL& 0& -\dot{L}\\ \frac{tgL}{R}& 0& (\mathsf{\Omega }+\dot{\lambda })CL& \dot{L}& 0\end{array}\right]\left[\begin{array}{c}\delta {\dot{V}}_E\\ \delta {\dot{V}}_N\\ {\varphi }_E\\ {\varphi }_N\\ {\varphi }_U\end{array}\right]+\left[\begin{array}{c}{\nabla }_{aE}\\ {\nabla }_{aN}\\ {\epsilon }_{gE}\\ {\epsilon }_{gN}\\ {\epsilon }_{gU}\end{array}\right]$$

(14)

where Ω = ω_ie, SL = sinL, and CL = cosL. The ${\nabla }_{aE},{\nabla }_{aN},{\epsilon }_{aE},{\epsilon }_{aN},{\epsilon }_{aU}$ are bias (or drift) in the n frame of accelerometer and gyroscope. $f_E^n$, $f_N^n$, and $f_U^n$ are three components of the specific force projected in the n frame.

We establish the Kalman model:

$$\dot{X}=AX+W$$

(15)

where $X={\left[\begin{array}{cccccccccc}\delta V_E& \delta V_N& {\varphi }_E& {\varphi }_N& {\varphi }_U& {\nabla }_E& {\nabla }_N& {\epsilon }_E& {\epsilon }_N& {\epsilon }_U\end{array}\right]}^T$ is the vector of system state. $W={\left[\begin{array}{cccccccccc}{w}_{aE}& w_{aN}& w_{gE}& w_{gN}& w_{gU}& 0& 0& 0& 0& 0\end{array}\right]}^T$ is the vector of system noise. A is the system matrix, and $w_{gE},w_{gN},w_{gU}$ are the white noise part of gyro bias. The $w_{aE}$ and $w_{aN}$ are the white noise part of accelerometer bias. Here,

$$A=\left[\begin{array}{cc}{A}^{\prime }& I_{5\times 5}\\ 0_{5\times 5}& 0_{5\times 5}\end{array}\right]$$

(16)

$$A^{\prime }=\left[\begin{array}{ccccc}\frac{V_N}{R}tgL& (2\mathsf{\Omega }+\dot{\lambda })SL& 0& -f_U^n& f_N^n\\ -2(\mathsf{\Omega }+\dot{\lambda })SL& 0& f_U^n& 0& -f_E^n\\ 0& -\frac{1}{R}& 0& (\mathsf{\Omega }+\dot{\lambda })SL& -(\mathsf{\Omega }+\dot{\lambda })CL\\ \frac{1}{R}& 0& -(\mathsf{\Omega }+\dot{\lambda })SL& 0& -\dot{L}\\ \frac{tgL}{R}& 0& (\mathsf{\Omega }+\dot{\lambda })CL& \dot{L}& 0\end{array}\right]$$

(17)

All variables in this formula have been described above. The observation equation is:

$$\left[\begin{array}{c}\delta V_E\\ \delta V_N\end{array}\right]=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]X+\left[\begin{array}{c}{\eta }_E\\ {\eta }_N\end{array}\right]$$

(18)

$$Z=HX+\eta$$

(19)

$Z={\left[\begin{array}{cc}{Z}_1& Z_2\end{array}\right]}^T={\left[\begin{array}{cc}\delta V_E& \delta V_N\end{array}\right]}^T={\left[\begin{array}{cc}{V}_{INS-E}-V_{DVL-E}& V_{INS-N}-V_{DVL-N}\end{array}\right]}^T$ is the observation vector; $H$ is the measurement matrix; $\eta ={\left[\begin{array}{cc}{\eta }_x,& {\eta }_y\end{array}\right]}^T$ is the observation noise, which is the Gaussian white noise with zero mean. $V_{INS-E(N)}$ is the eastward (northward) velocity calculated by the inertial navigation system or north-seeker. $V_{DVL-E(N)}$ is the eastward (northward) velocity measured by the Doppler velocity log (DVL).

Equations (15) and (19) constitute the Kalman model/system. That is:

$$\left\{\begin{array}{l}\dot{X}=AX+W\\ Z=HX+\eta \end{array}\right.$$

(20)

It can be proved that this system is not completely observable. Among the ten state variables $\delta V_E,\delta V_N,{\varphi }_E,{\varphi }_N,{\varphi }_U,{\nabla }_E,{\nabla }_N,{\epsilon }_E,{\epsilon }_N,{\epsilon }_U$, these three state variables ${\nabla }_E,{\nabla }_N,{\epsilon }_E$ are unobservable. However, the Kalman model is still available. The observed variables $\delta V_E,\delta V_N$ can be utilized to estimate the state variables ${\varphi }_E,{\varphi }_N,{\varphi }_U,{\epsilon }_N,{\epsilon }_U$. Therefore, the north-seeking can be completed.

The equations for time update are:

$${\widehat{X}}_{k/k-1}={\mathsf{\Phi }}_{k/k-1}{\widehat{X}}_{k-1}$$

(21)

where ${\widehat{X}}_{k/k-1}$, ${\mathsf{\Phi }}_{k/k-1}$, and ${\widehat{X}}_{k-1}$ are the state of one-step prediction, state transition matrix, and state estimation of $X_{k-1}$, respectively.

$$P_{k/k-1}={\mathsf{\Phi }}_{k/k-1}{P}_{k-1}{\mathsf{\Phi }}_{k/k-1}^T+Q$$

(22)

where $P_{k/k-1}$ is the mean square error matrix of state of one-step prediction; $P_{k-1}$ is the mean square error matrix of state estimation; and $Q$ is the system noise.

The equations for measurement update are:

$$K_k^{}=P_{k/k-1}^{}{H}_k^T{(H_k^{}{P}_{k/k-1}^{}{H}_k^T+R_{}^{})}^{-1}$$

(23)

where $K_k^{}$ is the filter gain, and $R$ is the observation noise. $P_k^{}$ and ${\widehat{X}}_k^{}$ can be obtained by Equations (24) and (25):

$$P_k^{}=(I-K_k^{}{H}_k^{})P_{k/k-1}$$

(24)

$${\widehat{X}}_k^{}={\widehat{X}}_{k/k-1}^{}+K_k^{}(Z_k^{}-H_k^{}{\widehat{X}}_{k/k-1}^{})$$

(25)

The initial conditions are set as follows:

$$\left\{\begin{array}{l}{L}_0=30°\\ T=0.005 s\\ \delta V_{E0}=\delta V_{N0}=0.1 m/s\\ {\varphi }_{E0}={\varphi }_{N0}={\varphi }_{U0}=3°\end{array}\right.$$

(26)

where L₀ is the initial latitude. The latitude of experimental site is close to 30° (as shown by following Section 3). Additionally, T is the sampling period of inertial devices or the calculation step of the algorithm. The values of $\delta V_{E0},\delta V_{N0},{\varphi }_{E0},{\varphi }_{N0},{\varphi }_{U0}$ are determined by the experience and simulations.

The initial matrixes of Kalman model are:

$$\left\{\begin{array}{l}{P}_0=\mathrm{diag}\left[\begin{array}{cccccccccc}{(0.1 m/s)}^2& {(0.1 m/s)}^2& {(3°)}^2& {(3°)}^2& {(3°)}^2& {(2\times {10}^{-5}g)}^2& {(2\times {10}^{-5}g)}^2& {(0.02°/h)}^2& {(0.02°/h)}^2& {(0.02°/h)}^2\end{array}\right]\\ Q=\mathrm{diag}\left[\begin{array}{cccccccccc}{(1\times {10}^{-5} g)}^2& {(1\times {10}^{-5} g)}^2& {(0.01°/h)}^2& {(0.01°/h)}^2& {(0.01°/h)}^2& 0& 0& 0& 0& 0\end{array}\right]\\ R=\mathrm{diag}\left[\begin{array}{cc}{(0.1 m/s)}^2& {(0.1 m/s)}^2\end{array}\right]\end{array}\right.$$

(27)

where P₀ is initial value of P_k. Q is the system noise, and R is the observation noise.

Equation (27) is obtained by the parameters shown in

Table 1. Errors of the inertial sensors in FOG north-seeker.

	Gyro Bias (°/h)		Accelerometer Bias (μg)
	Constant	Random (White Noise)	Constant	Random (White Noise)
x-Axis	0.021	0.011	21	12
y-Axis	0.019	0.01	22	13
z-Axis	0.023	0.013	19	8

and the results exhibited by Equation (26). The initial values shown by Equation (27) are optimal for KF, and the performance of KF with these initial values is high. Hence, the comparison between KF and ESO is fair.

The above is the traditional fine north-seeking algorithm. The following describes how to modify it to obtain the ESO method.

Generally, the carrier (such as vehicle and ship) moves in a straight line at a uniform speed. Here, the rapid north-seeking is studied under the condition of constant speed and straight-line motion of the carrier.

Under this condition, the output of the accelerometer is $f_E^n=f_N^n=0$ and $f_U^n=g$. When the speed is small, the constant speed has little effect on the system matrix. For example, when $V_x=\sqrt{V_E^2+V_N^2}=3 m/s$, then $V_x/R=4.7\times {10}^{-7}\mathrm{rad}/s$, $V_x/R\ll {\omega }_{ie}$, $V_E/R\ll {\omega }_{ie}$, $V_N/R\ll {\omega }_{ie}$. The $\dot{\lambda }$ and $\dot{L}$ in the above $A^{\prime }$ matrix can be approximately 0.

Therefore, Kalman filter model under uniform linear motion is as follows. We substitute the condition $V_E=V_N=0,f_E^n=f_N^n=0;f_U^n=g$ into Equation (17) and obtain:

$$A^{\prime }=\left[\begin{array}{ccccc}0& -2{\mathsf{\Omega }}_D& 0& -g& 0\\ 2{\mathsf{\Omega }}_D& 0& g& 0& 0\\ 0& -1/R& 0& -{\mathsf{\Omega }}_D& -{\mathsf{\Omega }}_N\\ 1/R& 0& {\mathsf{\Omega }}_D& 0& 0\\ tgL/R& 0& {\mathsf{\Omega }}_N& 0& 0\end{array}\right]$$

(28)

where ${\mathsf{\Omega }}_D=-{\omega }_{ie}\sin L$ and ${\mathsf{\Omega }}_N={\omega }_{ie}\cos L$ are the down and north components of ${\omega }_{ie}$ projected in the n frame.

We record the error model shown by Equation (14) as followings:

$$\left\{\begin{array}{l}\delta {\dot{V}}_E=-2{\mathsf{\Omega }}_D\cdot \delta V_N-g\cdot {\varphi }_N+{\nabla }_E\\ \delta {\dot{V}}_N=2{\mathsf{\Omega }}_D\cdot \delta V_E+g\cdot {\varphi }_E+{\nabla }_N\\ {\dot{\varphi }}_E=-\frac{1}{R}\delta V_N-{\mathsf{\Omega }}_D\cdot {\varphi }_N-{\mathsf{\Omega }}_N\cdot {\varphi }_U+{\epsilon }_E\\ {\dot{\varphi }}_N=\frac{1}{R}\delta V_E+{\mathsf{\Omega }}_D\cdot {\varphi }_E+{\epsilon }_N\\ {\dot{\varphi }}_U=\frac{\mathrm{t}gL}{R}\delta V_E-{\mathsf{\Omega }}_N\cdot {\varphi }_E+{\epsilon }_U\end{array}\right.$$

(29)

Here, the white noises are ignored. Based on the first and second formulas in Equation (29), the following is acquired:

$$\left\{\begin{array}{l}{\varphi }_N=\frac{1}{g}(-\delta {\dot{V}}_E+2{\mathsf{\Omega }}_D\cdot \delta V_N)+\frac{{\nabla }_E}{g}\\ {\varphi }_E=\frac{1}{g}(\delta {\dot{V}}_N-2{\mathsf{\Omega }}_D\cdot \delta V_E)-\frac{{\nabla }_N}{g}\end{array}\right.$$

(30)

According to the conclusions of observability analysis [3], the ${\nabla }_E$ and ${\nabla }_N$ are not observable. Hence, they cannot be estimated. All the variables $\delta {\dot{V}}_E,\delta {\dot{V}}_N,\delta V_E,\delta V_N$ are observable. The observability analysis based on singular value decomposition or eigenvalue/eigenvector theories of KF model can be found in refs. [3,4]. Based on the steady-state analysis of error model of KF [1,2], if both the ${\nabla }_E$ and ${\nabla }_N$ are set to be 0, then the steady states of ${\varphi }_N$ and ${\varphi }_E$ can be obtained. Hence, the steady-state errors $\delta {\varphi }_N$ and $\delta {\varphi }_E$ are shown as follows:

$$\left\{\begin{array}{l}\delta {\varphi }_N=\frac{{\nabla }_E}{g}\\ \delta {\varphi }_E=-\frac{{\nabla }_N}{g}\end{array}\right.$$

(31)

From the third formula in Equation (29), we can obtain:

$${\varphi }_U=\frac{1}{{\mathsf{\Omega }}_N}(-{\mathsf{\Omega }}_D{\varphi }_N-{\dot{\varphi }}_E+{\epsilon }_E-\frac{\delta V_N}{R})$$

(32)

By substituting Equation (30) into Equation (32), the following can be acquired:

$${\varphi }_U=-\frac{1}{g{\mathsf{\Omega }}_N}(\delta {\ddot{V}}_N-3{\mathsf{\Omega }}_D\delta {\dot{V}}_E-2{\mathsf{\Omega }}_D^2\delta V_N)+\frac{{\epsilon }_E}{{\mathsf{\Omega }}_N}-\frac{{\mathsf{\Omega }}_N}{g{\mathsf{\Omega }}_D}{\nabla }_E-\frac{\delta V_N}{R{\mathsf{\Omega }}_N}$$

(33)

All the variables $\delta {\ddot{V}}_N,\delta {\dot{V}}_E$, and $\delta V_N$ are observable. The variables ${\nabla }_E$ and ${\epsilon }_E$ are not observable [3,4]. For obtaining a steady-state value of ${\varphi }_U$, the parameters ${\nabla }_E$ and ${\epsilon }_E$ in Equation (33) are taken to be 0 [1,2]. Therefore, the steady-state error ($\delta {\varphi }_U$) of ${\varphi }_U$ can be obtained:

$$\delta {\varphi }_U=\frac{{\epsilon }_E}{{\mathsf{\Omega }}_N}-\frac{{\mathsf{\Omega }}_N}{g{\mathsf{\Omega }}_D}{\nabla }_E$$

(34)

The Kalman filtering results of ${\varphi }_E$, ${\varphi }_N$, and ${\varphi }_U$ show that the convergence speed of ${\varphi }_E$ and ${\varphi }_N$ is much faster than that of ${\varphi }_U$. Therefore, we use the steady-state value of ${\varphi }_E$ and ${\varphi }_N$ to estimate the ${\varphi }_U$. When ${\varphi }_E$ and ${\varphi }_N$ converge, ${\varphi }_U$ will converge. In Equation (32), assuming ${\epsilon }_E=0.02°/h$ and $\delta V_N=0.1 \mathrm{m}/\mathrm{s}$, we obtain ${\epsilon }_E=9.6\times {10}^{-8}\mathrm{rad}/s$ and $\delta V_N/R=1.5\times {10}^{-8}\mathrm{rad}/s$. And hence, ${\epsilon }_E-\frac{\delta V_N}{R}\ll -{\mathsf{\Omega }}_D{\varphi }_N-{\dot{\varphi }}_E$ (both ${\mathsf{\Omega }}_D{\varphi }_N$ and ${\dot{\varphi }}_E$ are about $5.0\times {10}^{-6}\mathrm{rad}/s$). We can obtain:

$${\overline{\varphi }}_U=\frac{1}{{\mathsf{\Omega }}_N}(-{\mathsf{\Omega }}_D{\overline{\varphi }}_N-{\dot{\overline{\varphi }}}_E)$$

(35)

In this formula, ${\overline{\varphi }}_E$ and ${\overline{\varphi }}_N$ are the estimated values of ${\varphi }_E$ and ${\varphi }_N$, respectively. Therefore, the estimation speed of ${\overline{\varphi }}_U$ (${\overline{\varphi }}_U$ is the estimation value of ${\varphi }_U$) based on ${\overline{\varphi }}_E$ and ${\overline{\varphi }}_N$ will be greatly accelerated. This can be achieved by designing an ESO. We construct a measurement equation:

$$y_1={\overline{\varphi }}_E$$

(36)

Regarding the third formula in Equations (29) and (36) as a first-order system, an ESO can be designed:

$$\left\{\begin{array}{l}{e}_{}=z_1-y_1\\ {\dot{z}}_1=z_2-{\beta }_1\cdot e_{}\\ {\dot{z}}_2=-{\beta }_2\cdot fal(e_{},{\alpha }_{},{\delta }_{})\end{array}\right.$$

(37)

where fal is a nonlinear feedback function. The e is the estimated error. The β₁ and β₂ are adjustable parameters, and the z₁ and z₂ are the two outputs of ESO, which are shown by Equation (38).

$$\left\{\begin{array}{l}{z}_1={\varphi }_E\\ z_2=-\frac{1}{R}\delta V_N-{\mathsf{\Omega }}_D\cdot {\varphi }_N-{\mathsf{\Omega }}_N\cdot {\varphi }_U+{\epsilon }_E\end{array}\right.$$

(38)

The ESO equations are established based on the control theory [42,46,47]. The first expression in Equation (37) defines the error. The second formula is based on the third formula in Equation (29). The error term $-{\beta }_1\cdot e_{}$ added in this formula is to accelerate the convergence speed of z₁. The third formula uses the nonlinear feedback function to express the differential of z₂. After solving Equation (37), z₁ and z₂ are obtained.

Similarly, if the fourth expression in Equations (29) and (39) are regarded as a first-order system, an ESO can also be designed, as shown in Equation (40). Additionally, the z₃ and z₄ are the two outputs, which are shown by Equation (41).

$$y_2={\overline{\varphi }}_N$$

(39)

$$\left\{\begin{array}{l}{e}_{}=z_3-y_2\\ {\dot{z}}_3=z_4-{\beta }_3\cdot e_{}\\ {\dot{z}}_4=-{\beta }_4\cdot fal(e_{},{\alpha }_2,{\delta }_2)\end{array}\right.$$

(40)

$$\left\{\begin{array}{l}{z}_3={\varphi }_N\\ z_4=\frac{1}{R}\delta V_E+{\mathsf{\Omega }}_D\cdot {\varphi }_E+{\epsilon }_N\end{array}\right.$$

(41)

In Equations (37) and (40), the function fal is:

$$fal(e,{\alpha }_i,{\delta }_i)=\left\{\begin{array}{ll}{\left|e\right|}^{{\alpha }_i}\mathrm{sign}(e),& \left|e\right|>{\delta }_i\\ \frac{e}{{\delta }_i{}^{1-{\alpha }_i}},& \left|e\right|\le {\delta }_i\end{array}\right.(i=1,2)$$

(42)

where the error e is the input variable of the fal function. The δ_i is any small positive number and the parameter ${\alpha }_i\in (0,1)$. There are many forms of nonlinear feedback functions to be chosen [46,47], of which the most commonly used is the fal function shown by formula (42). This function has the advantages of simple form, less computational efforts, high accuracy, and wide application.

The ${\overline{\varphi }}_E$ can be estimated by Equation (37) (${\overline{\varphi }}_E{=\mathrm{z}}_1$). Similarly, ${\overline{\varphi }}_N$ can be estimated by Equation (40) (${\overline{\varphi }}_N{=\mathrm{z}}_2$). However, ${\dot{\overline{\varphi }}}_E$ is difficult to be calculated, and an extra tracking differential (TD) filter is needed. The TD is widely applied to study the fine alignment of platform inertial navigation systems. In this article, the ESO with the filtering function is called the ESO filter, and TD with filtering function is called TD filter. The principle and calculation method of the TD filter are similar to those of ESO filter. TD filter is introduced as follows:

Assuming $x_1={\varphi }_E,x_2={\dot{\varphi }}_E$, then the third formula in Equation (29) becomes:

$$\left\{\begin{array}{l}{\dot{x}}_1=x_2\\ {\dot{x}}_2=f(x_1,x_2)\end{array}\right.$$

(43)

Equation (43) is expressed in TD form:

$$\left\{\begin{array}{l}{\dot{x}}_1=x_2\\ {\dot{x}}_2=-r\cdot sign\left[x_1-v+\frac{\left|x_2\right|\cdot x_2}{2r}\right]\end{array}\right.$$

(44)

In the specific implementation, the discrete form is adopted:

$$\left\{\begin{array}{l}{x}_1(k+1)=x_1(k)+h\cdot x_2(k)\\ x_2(k+1)=x_2(k)+h\cdot fst(x_1(k),x_2(k),v(k),r,h_1)\end{array}\right.$$

(45)

where x₁ is the tracking signal of v and can be obtained by TD filter; x₂ is the differential signal of v; h is the sampling step size and is set to be 5 ms; r is the parameter that determines the tracking speed, and it is called the speed factor; h₁ is the filter factor; fst ($•$) is a nonlinear function, and the expression is:

$$fst(x_1,x_2,v,r,h_1)=-r\cdot sat(g^{\prime },\delta )$$

(46)

Here,

$$\left\{\begin{array}{l}{g}^{\prime }=\left\{\begin{array}{l}{x}_2-sign(y)({\delta }_{00}-8r\left|y\right|+{\delta }_{00}^2)/2,\left|y\right|\ge {\delta }_{01}\\ x_2+y/h_1,\left|y\right|<{\delta }_{01}\end{array}\right.\\ sat(x,{\delta }_{00})=\left\{\begin{array}{l}sign(x),\left|x\right|\ge {\delta }_{00}\\ x/{\delta }_{00},\left|x\right|<{\delta }_{00}\end{array}\right.\end{array}\right.$$

(47)

The parameters in Equation (47) are:

$$\left\{\begin{array}{l}{\delta }_{00}=h_1\mathrm{r}\\ {\delta }_{01}=h_1{\delta }_{00}\\ e=x_1-v\\ y=e+h_1{x}_2\end{array}\right.$$

(48)

The results show that when h₁ > h (generally, h₁ = 2h), the filtering performance of TD for the signal with noise is better, and the obtained differential signal is accurate and ideal. Formulas (43)–(48) can be used to obtain ${\dot{\varphi }}_E$ (${\dot{\varphi }}_E=x_2$).

Only when ${\overline{\varphi }}_E$ and ${\overline{\varphi }}_N$ are close to the steady-state values are they then converted to be estimated by the ESO. Additionally, ${\overline{\varphi }}_U$ can be obtained by Equation (28). Generally, after coarse alignment, both the ${\varphi }_E$ and ${\varphi }_N$ are small enough and can be considered to be the input of ESO. The parameters in two ESOs are set as follows:

$$\left\{\begin{array}{l}{\alpha }_1={\alpha }_2=0.5\\ {\delta }_1={\delta }_2=0.025\\ {\beta }_1={\beta }_3{=1 \mathrm{s}}^{-1}\\ {\beta }_2={\beta }_4=0{.2 \mathrm{s}}^{-2}\end{array}\right.$$

(49)

The selection of parameters in fal function is based on a lot of simulations. The simulation results show that the fal function with the parameters shown by Equation (49) is excellent performance, high accuracy, and fast convergence. The determinations of the optimal parameters in ESO and TD are essentially a problem of multi-parameter optimization, which is very difficult to accurately solve. In this paper, the module of simulated annealing of commercial optimization software “Optimus” is applied for numerical calculation, and a set of solutions as shown in Equation (49) are obtained. The numerical simulation experiments show that this set of parameters is at least a group of local optimal solutions. Additionally, the parameters shown by Equation (49) can ensure the faster convergence speeds and higher calculation accuracies of ESO and TD.

After the coarse north-seeking, the fine north-seeking based on the ESO and TD filters are a start-up to greatly accelerate the convergence of $\overline{{\varphi }_U}$ and reduce the calculating time (generally from 5 min to 0.5 min). In the process of north-seeking, the controlled object is the navigation error equation. The error without estimation by the model is calculated by using the expanded state so as to realize the dynamic-feedback linearization in the process of initial alignment. The nonlinear configuration is used to form a nonlinear feedback control law to improve the control performance of the system.

As previously analyzed, the innovations of this algorithm are as follows: ① The ESO does not include KF, as shown in Equations (37) and (40). Therefore, no matrix calculation is required. ② The TD as shown in Equation (45) solves the problems of fast convergence and high accuracy of alignment of heading angle. ③ The parameters shown by Equation (49) are obtained by optimization theory and to ensure the excellent performances of ESO and TD.

3. Experiment and Discussion

The experimental process and detail are shown as follows.

The north-seeker based on fiber optical gyro (FOG) is exhibited in Figure 2a. The black object in the shape of a round cake is the FOG. The interfaces of test software of the north-seeker is shown in Figure 2b.

Based on the testing reports from the company supplying the gyro and the accelerometer, the bias of the inertial devices in FOG north-seeker is shown in Table 1. The constant gyro bias, random gyro bias, constant accelerometer bias, and random accelerometer bias in FOG north-seeker are about 0.02°/h, 0.01°/h, 20 μg and 10 μg, respectively.

Both the traditional Kalman-filter (KF) algorithm and proposed ESO algorithm were embedded into the FOG north-seeker. The coarse alignment algorithms of these two methods were the same. Additionally, this method is shown in Section 2.1. The fine alignment algorithms of these two methods were traditional KF and ESO, respectively. The static north-seeking experiment on the rotary-table was performance. The misalignment angles were exhibited in

Table 2. ${\varphi }_N$ and ${\varphi }_U$ of the static experimental results.

	Static Experiment—Traditional KF Method	Static Experiment—Proposed ESO Method
${\varphi }_E$	5 min(coarse) + 0.5 min(fine): 0.4′ 5 min(coarse) + 5 min(fine): 0.08′	5 min(coarse) + 0.5 min(fine): 0.1′ 5 min(coarse) + 5 min(fine): 0.09′
${\varphi }_N$	5 min(coarse) + 0.5 min(fine): 0.7′ 5 min(coarse) + 5 min(fine): 0.2′	5 min(coarse) + 0.5 min(fine): 0.2′ 5 min(coarse) + 5 min(fine): 0.2′
${\varphi }_U$	5 min(coarse) + 0.5 min(fine): 19.2′ 5 min(coarse) + 5 min(fine): 0.6′	5 min(coarse) + 0.5 min(fine): 0.8′ 5 min(coarse) + 5 min(fine): 0.8′

.

After 5.5 min, the ${\varphi }_U$ of ESO method was very small, and the convergence time of ${\varphi }_U$ of traditional KF method is long. The ${\varphi }_E$ and ${\varphi }_N$ were easy to converge, and the differences in results of these two methods are infinitesimal. The alignment precision of this north-seeker was high because of the high performance of the inertial devices.

However, the application of static north-seeking is very limited. The dynamic north-seeking is acquired in the field exploration, mobile-launching weapon, and so on. The application value of north-seeking, orientation keeping, and course maintenance on the moving base are greater than those of static north-seeking. In order to meet the demand of dynamic orientation and positioning, the ship-running tests were carried out. In this experiment, the accurate attitude reference was very important. The high-precision north-seeker based on the exact ring laser gyro (RLG) were selected as the attitude reference. This north-seeker is shown in Figure 3.

The parameters of the sensors in RLG north-seeker were shown by

Table 3. Errors of the inertial sensors in RLG north-seeker.

	Gyro Bias (°/h)		Accelerometer Bias (μg)
	Constant	Random (White Noise)	Constant	Random (White Noise)
x-Axis	0.0011	0.0004	9	4
y-Axis	0.0007	0.0008	10	7
z-Axis	0.001	0.0007	10	5

, based on the factory report. Based on this table, the constant gyro bias, random gyro bias, constant accelerometer bias, and random accelerometer bias in SLG north-seeker were about 0.001°/h, 0.0005°/h, 10 μg, and 5 μg, respectively. Hence, the attitude error was very small and can be ignored. The test software of the RLG north-seeker was the same as that of FOG north-seeker.

A ship-running experiment was carried out in the Zhanghe Reservoir in Jingmen City, Hubei Province, China. The experimental ship, GNSS installed at the stern, the electronic box of DVL, and the transducer of DVL are shown in Figure 4a–d.

The proposed ESO algorithm was embedded into the ring laser gyroscope (RLG) north-seeker, and the alignment is completed under the state of mooring. After the alignment, the ship was started. Twenty minutes after the moment when RLG north-seeker completed ESO, fine alignment was considered as the starting point of the whole experiment. After that starting point, the speed of ship was stable. Therefore, the results of the attitude angles after the fine alignment can be regarded as the accurate values or benchmark values. The misalignment angles of FOG north-seeker can be calculate based on these accurate attitude angles. The whole duration of the test was about 4.8493 × 10⁴ s (about 13.4703 h). In this experiment, FOG north-seeker accomplished several times of alignment experiment. The representative two among these experiments were selected as the first and second experiment. The duration time of the first (second) experiment was 4301.4–4601.4 s (5825.7–6125.7 s). The length of duration was 10 min (5 min coarse alignment +5 min traditional KF fine alignment).

Figure 5a,b shows the sailing trajectories of the ship in the first and second experiment, respectively. These two figures were obtained based on the GNSS measurements of position. The trajectory of the ship in the first (second) experiment was straight line (curve). In order to improve the integrated alignment and navigation accuracy, both velocity measurement of DVL and position measurement of GNSS were utilized as the observation vector in the integrated algorithm of RLG north-finder. For the universality of the ESO algorithm, only the velocity measurement of DVL was applied as the observation vector in the integrated algorithms (both traditional and ESO algorithms) of FOG north-seeker.

Figure 5c,d exhibits the data of z-axis gyro and z-axis accelerometer in the first test. The average values of Figure 5c,d were 0 rad/s and 9.8 m/s². Additionally, these indicated that the attitude and speed of the ship are relatively steady. Figure 5e,f exhibits the data of z-axis gyro and z-axis accelerometer in the second test. The mean value of Figure 5e was much larger than 0 rad/s during the process of turnaround. Additionally, the average value of Figure 5f was slightly larger than 9.8 m/s². These imply that the attitude and speed of the ship were not steady. Due to the measurement noise of inertial devices, too dense sampling, vibration, and angular shaking of moving ships, the curves in Figure 5c–f are very thick and not clear enough. However, the mean or center line of these corpulent curves can still reflect the motion characteristics of the ship and measurement results from inertial devices.

Figure 6a,b shows the curves of the ship speed obtained by the DVL measurement in the first and second test. Turning a corner implied slowing down and caused unstable attitude of the ship and impacted the velocity measurement of DVL. Therefore, the measurement precision of DVL was high/low in the first/second experiment.

Figure 7a,b exhibits the convergence processes of ${\varphi }_U$ of the two fine alignment algorithms in the first and second experiments, respectively. These curves were obtained by off-line simulations. The starting points of Figure 7a,b were 4451.4 s and 5975.7 s, respectively. Due to the fast convergence of ${\varphi }_E$ and ${\varphi }_N$, there was little difference between the traditional methods and ESO methods. Hence, only the curve of ${\varphi }_U$ was shown here. Figure 7a presents that the ESO method converged faster than the traditional method, and its alignment accuracy was slightly higher than the traditional method.

Figure 7b indicates that ${\varphi }_U$ of ESO method was roughly stable after 5.5 min, and the misalignment angle (error) was small, while the traditional method needed longer convergence time and was easy to be disturbed by inaccurate speed measurement information.

Table 4. ${\varphi }_N$ and ${\varphi }_U$ of the two experimental results.

	First Experiment—Traditional KF Method	First Experiment—Proposed ESO Method	Second Experiment—Traditional KF Method	Second Experiment—Proposed ESO Method
${\varphi }_E$	5 min(coarse) + 0.5 min(fine): −2.5′ 5 min(coarse) + 5 min(fine): −0.1′	5 min(coarse) + 0.5 min(fine): −0.2′ 5 min(coarse) + 5 min(fine): −0.2′	5 min(coarse) + 0.5 min(fine): 0.4′ 5 min(coarse) + 5 min(fine): 0.5′	5 min(coarse) + 0.5 min(fine): 0.3′ 5 min(coarse) + 5 min(fine): 0. 3′
${\varphi }_N$	5 min(coarse) + 0.5 min(fine): 1.9′ 5 min(coarse) + 5 min(fine): 0.1′	5 min(coarse) + 0.5 min(fine): 0.2′ 5 min(coarse) + 5 min(fine): 0.2′	5 min(coarse) + 0.5 min(fine): −0.9′ 5 min(coarse) + 5 min(fine): −0.3′	5 min(coarse) + 0.5 min(fine): −0.6′ 5 min(coarse) + 5 min(fine): −0.5′
${\varphi }_U$	5 min(coarse) + 0.5 min(fine): 26.7′ 5 min(coarse) + 5 min(fine): 1.8′	5 min(coarse) + 0.5 min(fine): 2.1′ 5 min(coarse) + 5 min(fine): 2.1′	5 min(coarse) + 0.5 min(fine): 63.1′ 5 min(coarse) + 5 min(fine): 14.2′	5 min(coarse) + 0.5 min(fine): 4.8′ 5 min(coarse) + 5 min(fine): 3.3′

shows the ${\varphi }_E$, ${\varphi }_N$, and ${\varphi }_U$ at some critical moments. ${\varphi }_U$ at some key time points have been shown in the above Figure 7a,b.

The results in Table 4 present that the accuracy and anti-interference ability of ESO algorithm are better than those of traditional algorithms. The misalignment angle of the heading angle of the proposed method is ≤3.3′ after 10 min of alignment. This demonstrates that an extension of alignment time of ESO will slightly improve the precision. In the calculation process, the consumptions of calculation time and hardware resources of this alignment method are statistically analyzed. The results show that the time and resource consumptions of the ESO method are far less ($\ll 0.1$) than those of the traditional KF method.

4. Conclusions

In this paper, ESO are applied in the algorithm of north-seeking, and an effective fast method is proposed. Three innovations of the proposed algorithm include ESO filter, TD filter, and the optimized parameters of these two filters. The advantages of this method are analyzed as follows: ① The experimental results show that the response characteristics of this method are outstanding. The results of first off-line simulation show that the misalignment angle of the heading angle of the proposed (traditional) method is ≤2.1′ (1.8′) after 5.5 (10) min of alignment. The results of second off-line simulation indicate that the misalignment angle of the heading angle of the proposed (traditional) method is ≤4.8′ (14.2′) after 5.5 (10) min of alignment. The simulations are based on the ship-running experimental data. The measurement precision of DVL is high (relatively low) in the first (second) experiment. Hence, the anti-interference ability of ESO can be investigated. These results demonstrate that the convergence speed, accuracy, and anti-interference ability of proposed ESO algorithm are better than those of traditional KF algorithm. ② The hardware resource consumption and time consumption of the ESO algorithm are far lower ($\ll 0.1$) than those of traditional KF algorithm.

This method can greatly shorten the azimuth alignment time, and its steady-state accuracy is very close to the theory limit. The anti-interference ability of the proposed algorithm is strong. The remarkable advantages show the effectiveness and practicability of ESO in the north-finding algorithm.