A fast initial alignment for SINS based on disturbance observer and Kalman filter

Abstract

Initial alignment for a strap-down inertial navigation system (SINS) plays an important role in the following navigation and positioning operation. Initial alignment incorporates two stages: coarse and fine. This paper mainly investigates fine alignment for SINS under static base. A new fast SINS initial alignment scheme, a disturbance observer-based Kalman filter (DOBKF), is proposed to estimate the misalignment angles. As the name implies, the DOBKF is composed of a Kalman filter and a disturbance observer (DO). The Kalman filter is used to estimate horizontal misalignment angles, and the DO is applied to estimate the azimuth misalignment angle. In addition, when the estimations from the Kalman filter reach a steady state, they will be used as input for designing the DO. Compared with traditional filters, such as a Kalman filter used in initial alignment, the filter proposed by this paper not only greatly hastens the overall initial alignment process, but has comparable accuracy. Comparing simulation results shows that the proposed filter satisfies the requirement of SINS alignment.

Keywords

Inertial navigation systems initial alignment disturbance observer Kalman filter

Introduction

Inertial navigation systems (INS) are widely used for navigation and positioning of aircraft, vehicles, ships, etc., since they can successively supply attitude, velocity and position information. Roughly speaking, the INS can be commonly classified alongside the gimballed INS (GINS) and the strapdown INS (SINS) (Wang, 2009). Because they have similar principles, we mainly focus on the SINS in this paper. As a dead-reckoning navigation system based on integral operation, the accuracy of SINS is restricted by the initial conditions. Consequently, there is a crucial problem to ascertain the initial conditions for the subsequent navigation operation, which contains initial velocity, position and attitude. In practice, the initial velocity and position can be obtained by other sensors, such as Global Positioning Systems (GPS). Thus, the purpose is to determine the initial attitude, or coordinate transformation matrix, from the body frame to the navigation frame. The overall process is called initial alignment (Groves, 2008; Rogers, 2007; Titterton and Weston, 2004).

Considering that SINS is stationary with respect to the Earth, the key point of the initial alignment is to determine the misalignment between the navigation frame and the computer navigation frame within smaller tolerances; then the misalignment is expected to drive to zero in a short period of time (Fang and Wan, 1996; Naser and Sameh, 2004; Rogers, 2007; Titterton and Weston, 2004).

During the SINS initial alignment, coarse alignment is the first stage to obtain a small initial attitude for the second stage, named fine alignment process (Naser and Sameh, 2004). The coarse alignment stage can provide the roughly known initial attitude in a short time; then the small misalignment angles are expected to be obtained for the fine alignment. Many filters are utilized to optimally estimate the misalignment during the fine alignment. The famous Kalman filter is one of the most effective estimators to minimize the error variance in this stage (Rogers, 2007; Simon, 2006; Kalman, 1961).

There are some assumptions on the systems model and characteristics of noise when the standard Kalman filter is utilized to estimate an unknown state. For example, the systems model and characteristics of noise need to be known. However, it is difficult to obtain an exact model and the corresponding parameters for initial alignment. In recent decades, many kinds of improved Kalman filter have been proposed to relax the assumptions. An adaptive Kalman filtering is developed in Ahn and Won (2006) to automatically tune the measurement noise covariance matrix. In order to improve the robustness of the initial alignment, the adaptive extended Kalman filter (AEKF) is proposed by Fang and Yang; it is applied to in-flight alignment of airborne position and the orientation system (Fang and Yang, 2011). Shin and Naser first used the unscented Kalman filter (UKF) for initial alignment of low-cost inertial measurement units with the large initial attitude errors (Shin and Naser, 2004). This method is also extended by (Park and Kim, 2006; Zhang et al., 2011) for in-motion alignment. The UKF via different unscented transformations is considered and the performance of UKF and EKF is compared in (Park and Kim, 2006). The results show that the estimation accuracy of EKF and UKF is almost the same when the initial heading error is less than $30 \circ$ , but UKF performs better than EKF when the initial heading error is about $45 \circ$ . The coarse alignment for the moving base under substantial swing is researched in use of UKF (Zhang et al., 2011).

Recently, Wu et al. found that the initial alignment can be transformed into attitude determination using infinite vector observation (Chang et al., 2014; Wu et al., 2011). Thus, many fruitful attitude determination methods can be introduced into the initial alignment. The q-method algorithm is developed in Wu et al. (2011) for finding the optimal attitude. However, only the initial attitudes are estimated: the inertial sensors in the SINS, for example gyroscope biases, are not considered. This problem is researched in Chang et al. (2014). An optimization-based alignment is developed to estimate the gyroscope biases coupled with the attitude. As the accuracy of initial alignment is restricted by the multiple disturbances (Liu et al., 2012), a class of multi-objective filter is constructed for the SINS with disturbance rejection and attenuation performance (Cao and Guo, 2012). As part of a multi-objective filter, a class of drift estimator is developed to estimate the initial drifts, which can be described by the first order Gauss-Markov process.

All the designed filters above can improve the accuracy or robustness of fine alignment for different models to a certain extent. However, there is a contradiction between the accuracy and speed in initial alignment phase. It is well known that greater alignment accuracy requires longer time (Li et al., 2013). In fact, levelling misalignment angles reach steady state faster than the azimuth misalignment angle, which means the azimuth alignment time takes the most time for initial alignment. In terms of the observability analysis of SINS, it can be found that the misalignment angles have different convergence speeds. Accordingly, many effective methods have been proposed to estimate the azimuth misalignment angle in a short time. A novel fast initial alignment method is proposed for the SINS under stationary base by Fang and Wan (1996). The estimation of azimuth misalignment angle is obtained from the stable levelling misalignment angles; the initial alignment time is reduced greatly comparing with the standard Kalman filter. A similar idea can be found in Zhang et al. (2004) and Wang (2009), where the azimuth misalignment angle is estimated by the measurement output and its derivatives. In order to shorten the alignment time, the estimation of misalignment angles is given by observable measurements and their derivatives (Zhang et al., 2004). A non-singular transform is introduced for the error model in Wang (2009), and the states of error model are decomposed into observable and unobservable parts. Then the estimation of misalignment angles is obtained by observable measurements and their derivatives: the gyro drift can be directly used by levelling misalignment angle rates, which can also greatly reduce the alignment time. Due to the fact that unobservable states are hard to estimate accurately, the reduced-order filter may be another method to reduce the computing burden and time by ignoring unobservable states. A ten-state error model is given in Berman and Bar-Itzhack (1988), which is reduced to a five-state Kalman filter by ignoring the unobservable state in (Aranda et al., 1994). A closed-loop five-state Kalman filter is implemented for initial alignment of a Ring Laser Gyroscope SINS in Lv et al. (2008).

In order to enhance the robustness and save the computation time of initial alignment, a disturbance observer (DO) can be a preferred approach. DOs first appeared in the 1980s in the form of frequency-domain; they are used to estimate and compensate disturbance torque (Ohishi et al., 1987). The disturbance, which is too difficult or expensive to measure, can be estimated by the observer from the available measurement and state. The influence of the disturbance can be compensated by a controller based on DO (Chen et al., 2016). In order to estimate and compensate for the friction in robotic manipulators, Chen et al. have given the first formulation of a DO in the time-domain (Chen et al., 2000). The design procedure is also presented, and the performance of the proposed DO, containing convergence rate and asymptomatic stability, is analyzed. However, only constant disturbance is considered in (Chen et al., 2000), which may restrict the application of the DO in engineering. An exogenous system is introduced to describe more kinds of disturbance (Guo and Chen, 2005). This model can be used to describe many kinds of disturbance: for example, constant disturbance, ramp disturbance and harmonic disturbance with unknown phase and magnitude. Since the DO serves as a path to the baseline controller (Yang et al., 2013), many effective control methods combine with a DO: such as, PID (Liu et al., 2012), $H_{\infty}$ control (Wei and Guo, 2009), sliding-mode control (Wei and Guo, 2010; Yang et al., 2013), and so on. More details about the DO can be found in two recent books (Guo and Cao, 2013; Li et al., 2014).

However, few results have been reported in which a DO and a filter have been conjoined to estimate an unknown state. Recently, some explored methods have attracted researchers. A class of Kalman-filter-based disturbance observer has been designed to estimate forces and friction (Mitsantisuk et al., 2011, 2012; Phuong et al., 2014). The estimation which is obtained from the Kalman filter is applied as input to construct the disturbance observer in the form of frequency-domain. According to the fact that horizontal misalignment angles reach steady state faster than the azimuth misalignment angle in the structure of the Kalman filter, the DO in the form of time-domain can be based on a Kalman filter for the SINS under stationary base.

The rest of this paper is organized as follows. A SINS error model for a stationary base is established in the second section. In the third section, a class of disturbance observer based on ten-state and five-state Kalman filters is constructed for SINS. In the fourth section, the simulation for alignment of SINS is given to demonstrate the efficiency of the disturbance observer based on a Kalman filter. Finally, we conclude in the fifth section.

SINS error model for stationary base

In order to avoid confusion, the coordinate frames used in this paper are given as follows (Groves, 2008).

The Earth-centred, Earth-fixed frame (e-frame):

is centred at the Earth’s centre;

the z-axis is coincident with the direction of the Earth’s rotation;

the x-axis locates the Greenwich meridian in the equatorial plane;

the y-axis completes the right-handed orthogonal set within the equatorial plane.

Body frame (b-frame):

the origin of the b-frame locates the mass of the object;

the z-axis is the up axis of the object, opposite the direction of gravity;

the x-axis is along the object’s longitudinal axis, pointing in the direction of movement;

the y-axis completes the right-handed orthogonal set.

The navigation frame (n-frame):

the navigation frame is a local geographic coordinate frame in this article;

it has origin at the location of the navigation system;

the x- and y-axes are aligned to east and north;

the z-axis completes the right-handed orthogonal set.

Figure 1 shows the e-frame, b-frame and n-frame.

Figure 1.

Diagrams of coordinate systems.

SINS error model

The SINS error model for a stationary base can be formed from the differential equations of the error model in (Berman and Bar-Itzhack, 1988). Considering the stationary base, both the position error and the vertical velocity error are ignored. The SINS error model in the n-frame can be written as

\overset{\cdot}{x} = Ax + w

(1)

where the state vector x is given by $x (t) = [δ ν_{E} δ ν_{N} ϕ_{E} ϕ_{N} ϕ_{U} \nabla_{x} \nabla_{y} ε_{x} ε_{y} ε_{z}]^{T}$ , and $δ ν_{E}$ , $δ ν_{N}$ are the east and north velocity errors, $ϕ_{E}$ , $ϕ_{N}$ , $ϕ_{U}$ are three misalignment angles along the east, north, and up gyros respectively. $\nabla_{x}$ and $\nabla_{y}$ are accelerometer constant bias errors in the b-frame, $ε_{x}$ , $ε_{y}$ and $ε_{z}$ denote three gyro drifts in the b-frame. The system matrix A is given by

A = [\begin{matrix} F & T \\ 0_{5 \times 5} & 0_{5 \times 5} \end{matrix}]

where

\begin{matrix} F = [\begin{matrix} 0 & 2 Ω_{U} & 0 & - g & 0 \\ - 2 Ω_{U} & 0 & g & 0 & 0 \\ 0 & 0 & 0 & Ω_{U} & - Ω_{N} \\ 0 & 0 & - Ω_{U} & 0 & 0 \\ 0 & 0 & Ω_{N} & 0 & 0 \end{matrix}], \\ T = [\begin{matrix} C_{11} & C_{12} & 0 & 0 & 0 \\ C_{21} & C_{22} & 0 & 0 & 0 \\ 0 & 0 & C_{11} & C_{12} & C_{13} \\ 0 & 0 & C_{21} & C_{22} & C_{23} \\ 0 & 0 & C_{31} & C_{32} & C_{33} \end{matrix}], \\ Ω_{U} = w_{ie} \sin (L_{0}), Ω_{N} = w_{ie} \cos (L_{0}) \end{matrix}

$C_{b}^{n} = {C_{ij}}_{i, j = 1, 2, 3}$ is the coordinate transformation matrix from the b-frame to the n-frame, g is the local gravity acceleration, $w_{ie}$ is the Earth’s rotation rate, and $L_{0}$ is the local latitude. $w (t)$ represents the Gaussian stochastic white noise of SINS.

The measured horizontal velocities are just horizontal velocity errors during the process of stationary alignment. The measurement model can then be expressed by

z = Hx + v

(2)

where $z = [δ ν_{E} δ ν_{N}]^{T}$ , the observation matrix $H = [I_{2} 0_{2 \times 8}]$ , and v is the Gaussian stochastic white noise.

Estimation of the azimuth misalignment angle based on a combination disturbance observer with a Kalman filter

Development of disturbance observer

The observable states can be characterized by two-time-scale behaviour: one class of states tends to steady-state fast, which can be considered as input for the disturbance observer; the other class of states tends to steady-state slow, which disturbance can be estimated by the disturbance observer. Depending on the initial alignment, we can design the disturbance observer based on a Kalman filter. Considering the dynamic of misalignment angle $ϕ_{E}$ and $ϕ_{U}$ in system (1), we have

{\overset{\cdot}{ϕ}}_{E} = ϕ_{N} Ω_{U} - ϕ_{U} Ω_{N} + ε_{E}

(3)

{\overset{\cdot}{ϕ}}_{U} = ϕ_{E} Ω_{N} + ε_{U}

(4)

where $ε_{E} = c_{11} ε_{x} + c_{12} ε_{y} + c_{13} ε_{z}$ , $ε_{N} = c_{21} ε_{x} + c_{22} ε_{y} + c_{23} ε_{z}$ , $ε_{U} = c_{31} ε_{x} + c_{32} ε_{y} + c_{33} ε_{z}$

Let

\begin{matrix} ϕ_{U}^{0} \overset{Δ}{=} ϕ_{U} Ω_{N} - ε_{E} \\ d \overset{Δ}{=} [ϕ_{U}^{0} ε_{U}]^{T} \end{matrix}

then the dynamic equations (3) and (4), augmented with constant sensor error $ε_{U}$ , can be modified as

{\overset{\cdot}{ϕ}}_{E} = u_{1} (ϕ_{N}) - Bd

(5)

\overset{\cdot}{d} = Dd + u_{2} (ϕ_{E})

(6)

where

\begin{matrix} u_{1} (ϕ_{N}) = ϕ_{N} Ω_{U}, u_{2} (ϕ_{E}) = [ϕ_{E} Ω_{N}^{2} 0]^{T} \\ D = [\begin{matrix} 0 & Ω_{N} \\ 0 & 0 \end{matrix}], B = [10] \end{matrix}

In this subsection, the inputs $u_{1} (ϕ_{N}), u_{2} (ϕ_{E})$ and the state $ϕ_{E}$ are all assumed to be available, and $ϕ_{U}^{0}$ is considered as unknown disturbance to be estimated. Before the disturbance observer is designed, the observability of disturbance should first be analyzed. For the dynamic model (5) and (6) the observability test matrix of disturbance can be written as

Q_{d} = [B^{T} D^{T} B^{T}]^{T}

the determinant of $| Q_{d} |$ can be obtained $| Q_{d} | = Ω_{N} .$

Then it indicates that if only $Ω_{N} \neq 0$ , the disturbance d is observable (Guo and Chen, 2005). The linear disturbance observer proposed by (Chen et al., 2000; Guo and Chen, 2005) can be given by

\overset{\cdot}{z} = (D + LB) z + (D + LB) L ϕ_{E} + u_{2} (ϕ_{E}) - L u_{1} (ϕ_{N})

(7)

\hat{d} = z + L ϕ_{E}

(8)

L is the disturbance gain to be determined, when $Ω_{N} \neq 0$ , the existence of L can be guaranteed from the observability test matrix. The block diagram of linear disturbance observer (7)–(8) is shown in Figure 2. Now we are in a position to analyze the rate of convergence for the disturbance observer (7) and (8).

Figure 2.

Block diagram of a Disturbance Observer.

Define the disturbance estimation error

e_{d} = d - \hat{d}

the disturbance estimation error is governed by

{\overset{\cdot}{e}}_{d} = (D + LB) e_{d}

(9)

The solution of dynamic equation (9) satisfies

∥ e_{d} (t) ∥ \leq ∥ e_{d} (t_{0}) ∥ e^{λ_{\max} (D + LB) (t - t_{0})}

(10)

where $λ_{\max} (D + LB)$ is the maximal eigenvalue of $D + LB$ .

The disturbance observer gain L can be designed by pole placement to guarantee $D + LB$ is Hurwitz: that is, the real parts of all eigenvalues of $D + LB$ are negative. It is implied that the disturbance estimation error $e_{d}$ exponentially decays to the origin. In other words, the estimation of azimuth misalignment angle base on disturbance observer converges to steady state at an exponential rate.

The convergence rate of the azimuth misalignment angle, based on a Kalman filter for SINS at a stationary base needs a long time to reach steady state, which means the overall initial alignment process is restricted by the convergence rate of the azimuth misalignment angle. The initial alignment of the DO offers a fast method to estimate the azimuth misalignment angle at an exponential rate. The convergence time for initial alignment based on a DO is faster than a Kalman filter under the same initial conditions.

In order to realize practical engineering during the initial alignment, the discretization form of DOs (7) and (8) can be concluded as follows

z_{k + 1} = F_{k} z_{k} + G_{k} [(D + LB) L ϕ_{E, k} + u_{2} (ϕ_{E, k}) - L u_{1} (ϕ_{N, k})]

(11)

{\hat{d}}_{k} = z_{k} + L ϕ_{E, k}

(12)

where $F_{k}$ and $G_{k}$ are given by (Simon, 2006)

\begin{matrix} F_{k} = e^{(D + LB) Δ t}, Δ t = t_{k + 1} - t_{k} \\ G_{k} = F_{k} [I - e^{- (D + LB) Δ t}] (D + LB)^{- 1} \end{matrix}

Remark 1. In this subsection, the azimuth misalignment angle $ϕ_{U}$ and the east gyro drift $ε_{E}$ are combined to be estimated. The main reason is that, when the disturbance is formulated as $d = [ϕ_{U}^{0} ε_{U}]$ , then the disturbance is observable. While, if the disturbance is augmented as $d = [ϕ_{U}^{0} ε_{E} ε_{U}]$ , it can be found that the disturbance will be unobservable by means of calculating the observability test matrix. In fact, it is known that the east gyro drift $ε_{E}$ under stationary base is unobservable based on the analysis of observability in Fang and Wan (1996); Wang (2009). Thus, it is difficult to estimate the east gyro drift separately.

Disturbance observer based on Kalman filter

In the above subsection, there is an assumption on the design of the DO. That is, the states $ϕ_{E}$ and $ϕ_{N}$ are assumed to be available to the DO. During the initial alignment, the horizontal misalignment angles $ϕ_{E}$ and $ϕ_{N}$ are also needed to be estimated by some filters. The Kalman filter is a common method of estimating misalignment angles and sensor errors (Rogers, 2007).

To conveniently design a Kalman filter, the discretization form of error dynamics (1) and (2) can be formulated as follows

x_{k + 1} = Φ_{k} x_{k} + N_{k} w_{k}

(13)

y_{k} = H_{k} x_{k} + v_{k}

(14)

In terms of (13)–(14), the Kalman filter can be constructed as (Kalman,1961)

(1) Time update (predict)

{\hat{x}}_{k + 1 | k} = Φ_{k} {\hat{x}}_{k | k}

(15)

P_{k + 1 | k} = Φ_{k} P_{k | k} Φ_{k}^{T}

(16)

(2) Measurement update (correct)

{\hat{x}}_{k + 1 | k + 1} = {\hat{x}}_{k + 1 | k} + K_{k + 1} (y_{k} - H_{k} {\hat{x}}_{k + 1 | k})

(17)

K_{k + 1} = P_{k + 1 | k} H_{k}^{T} (H_{k} P_{k + 1 | k} H_{k}^{T} + R_{k})^{- 1}

(18)

P_{k + 1 | k + 1} = P_{k + 1 | k} - K_{k + 1} H_{k} P_{k + 1 | k}

(19)

where ${\hat{x}}_{k + 1 | k}$ is prior estimation of $x (k)$ , and $P_{k + 1 | k}$ is covariance of estimation error of ${\hat{x}}_{k + 1 | k}$ . ${\hat{x}}_{k + 1 | k + 1}$ is posteriori estimation of $x (k)$ and $P_{k + 1 | k + 1}$ is covariance of estimation error of ${\hat{x}}_{k + 1 | k + 1}$ .

Based on the Kalman filter (15)–(19), one can obtain the estimation of misalignment angles. However, the misalignment angles have different degrees of observability, which means they have different rates of convergence. It is known that the horizontal misalignment angles $ϕ_{E}$ and $ϕ_{N}$ have faster convergence rates than the azimuth misalignment angle. Thus, when the estimation of horizontal misalignment angles based on a Kalman filter reaches steady state in a short period of time, they can be used as the input for the development of a disturbance observer. The horizontal misalignment angles estimated by Kalman filtering are applied to the DO to obtain the azimuth misalignment angle, as shown in Figure 3.

Figure 3.

Block diagram of Disturbance Observer based on Kalman filter.

To develop a disturbance observer, only the horizontal misalignment angles need to be estimated. Inspired by Wang (2009) and Aranda et al. (1994), the accuracy and observability are considered simultaneously. Unobservable states, such as $ε_{E}$ , $\nabla_{E}$ and $\nabla_{N}$ , have little influence on the designed filter, and are thus ignored. The other observable constant sensor biases are also neglected to improve the rate of convergence. So, the simplified form of error dynamics can be given by

\overset{\cdot}{\bar{x}} = F \bar{x} + \bar{w}

(20)

\bar{z} = \bar{H} \bar{x} + v

(21)

where $\bar{x} (t) = [δ V_{E} δ V_{N} ϕ_{E} ϕ_{N} ϕ_{U}]$ , $\bar{z} (t) = [δ V_{E} δ V_{N}]^{T}$ . $\bar{H} = [I_{2} 0_{2 \times 3}]$ , the process noise $\bar{w} (t)$ is the white noise of simplified dynamics systems, and $\bar{w} (t) ~ N (0, \bar{Q})$ . The system matrix F and measurement noise $v (t)$ have been defined in Section 2.

The Kalman filter can be designed according to (15) to (19), which will reduce the computing burden for estimating the misalignment angles. The algorithm of a DO based on a Kalman filter, for a ten-state (13–14) and a five-state (20–21) model are given as follows.

Step 1. Initialization of the parameters of the DOBKF.

Step 2. Time update by equations (15)–(16).

Step 3. Measurement update by equations (17)–(19).

Step 4. Check the horizontal misalignment angles to be stable or not: if the answer is yes, go to the next step; otherwise return to step 2.

Step5.Execute the estimation of azimuth misalignment angle by (11)–(12) until the alignment is finished.

Simulation example

In this section, the simulation study is presented to confirm the effectiveness of the DO, based on a Kalman filter at a stationary base. The DOs based on a ten-state and a five-state Kalman filter are considered respectively.

In this simulation, the local latitude is 39.9 deg N. The initial misalignment angles $ϕ_{N}$ , $ϕ_{E}$ , $ϕ_{U}$ are all chosen as $1 \circ$ . The constant and random drifts of gyro are chosen as $0.02 \circ /$ hr and $0.01 \circ /$ hr/Hz respectively. The constant and random drifts of accelerators are chosen as $100 μ$ g and $50 μ$ g/Hz respectively (Fang and Wan, 1996). The process noise and the measurement noise are both Gaussian white noise. The parameters of the ten-state filter are selected as follows

\begin{matrix} P_{0} = & diag [(1 m / s)^{2} (1 m / s)^{2} (1 \circ)^{2} (1 \circ)^{2} \\ (1 \circ)^{2} (100 μ g)^{2} (100 μ g)^{2} (0.02 \circ / hr)^{2} \\ (0.02 \circ / hr)^{2} (0.02 \circ / hr)^{2}] \\ Q = & diag [(50 μ g / Hz)^{2} (50 μ g / Hz)^{2} \\ (0.01 \circ / hr / Hz)^{2} (0.01 \circ / hr / Hz)^{2} (0.01 \circ / hr / Hz)^{2} \\ 0 0 0 0 0] \\ R = & diag [(0.1 m / s)^{2} (0.1 m / s)^{2}] \end{matrix}

The DO gain L is given by $L = - 100 \times [0.8 1 / Ω_{N}]^{T}$ , L is chosen to guarantee that $D + LB$ is Hurwitz. The parameters of the five-state filter are modified by

\begin{matrix} {\bar{P}}_{0} = & diag [(1 m / s)^{2} (1 m / s)^{2} (1 \circ)^{2} (1 \circ)^{2} (1 \circ)^{2}] \\ \bar{Q} = & diag [(50 μ g / Hz)^{2} (50 μ g / Hz)^{2} (0.01 \circ / hr / Hz)^{2} \\ (0.01 \circ / hr / Hz)^{2} (0.01 \circ / hr / Hz)^{2}] \\ R = & diag [(0.1 m / s)^{2} (0.1 m / s)^{2}] . \end{matrix}

In this simulation, the Kalman filter and the method in Fang and Wan (1996) are carried out to provide a bias for comparison with DOBKF, and we mainly focus on the performance of misalignment angles and attitude errors to illustrate the effectiveness of proposed filter. The estimation of misalignment angles for a ten-state model and a five-state model are described in Figures 4 and 6, respectively. It shows the accuracy and convergence speed of three different methods. The estimation of attitude errors for the ten- and five-state models, based on the three methods, are described in Figures 5 and 7. In Figures 4(a) and (b), it can be seen that the estimated horizontal misalignment angles $ϕ_{E}$ and $ϕ_{N}$ have already reach steady states at $t = 25 s$ . Then the estimation of horizontal misalignment angles is used as input for designing a DO after $t = 30 s$ . The curves in Figures 4(a) and (b) show that the estimation of $ϕ_{E}$ and $ϕ_{N}$ obtained by DOBKF is comparable to the Kalman filter and the method in Fang and Wan (1996). As shown in Figure 4(c), the azimuth misalignment angle estimated by the DOBKF performs better than the Kalman filter and the method in Fang and Wan (1996), but the convergence time is shorter is the shortest among three methods.

Figure 4.

Estimation of misalignment angles for ten-state error model. (a) $ϕ_{E}$ , (b) $ϕ_{N}$ , (c) $ϕ_{U}$ .

Figure 5.

Attitude errors for ten-state error model. (a) pitch error, (b) roll error, (c) yaw error.

The azimuth misalignment angle based on DOBKF reaches 1′ at $t = 40$ , and the method used in Fang and Wan (1996) needs about 100s to reach 1′; the method through the Kalman filter requires almost 200s to reach 1′. Figure 5 shows the attitude errors of the three different methods. It can be seen that these are about $0.09 \circ$ in terms of DOBKF, and the stable time is about 60s. Fang and Wan (1996)’s method needs 150s to reach $- 0.4 \circ$ ; the Kalman filter requires 200s. Since the unobservable states are ignored in the five-state model (20)–(21), the state of the reduced model becomes observable and the burden on computation can be reduced. Figures 4 and 5 show that DOBKF is equally accurate, but the convergence speeds are fastest.

Compared to Figures 4 –7, the five-state model (20)–(21), based on DOBKF, performs better than the ten-state model (1)–(2) with the same convergence speeds. According to Figures 6 and 7, DOBKF based on the reduced model performs better than the Kalman filter and Fang and Wan (1996)’s method. Table 1 illustrates the results of root mean square (RMS) error of misalignment angles for three methods at 300s. The convergence time is given in Table 2. Tables 1 and 2 indicate that: (1) the RMS error of misalignment angles, based on DOBKF for both ten-and five-state models have comparable accuracy to the Kalman filter and the method in Fang and Wan (1996); (2) the convergence speed of DOBKF is fastest; (3) a five-state model, based on DOBKF, performs better than a ten-state model at almost the same convergence speed.

Figure 6.

Estimation of misalignment angles for five-state error model. (a) $ϕ_{E}$ , (b) $ϕ_{N}$ , (c) $ϕ_{U}$ .

Figure 7.

Attitude errors for five-state error model. (a) pitch error, (b) roll error, (c) yaw error.

Table 1.

RMS error of misalignment angles for different methods.

Filtering Method	$ϕ_{E} / ('')$	$ϕ_{N} / ('')$	$ϕ_{U} / (')$
Kalman Filter (ten-state)	20.6270	20.6653	7.2905
Fang and Wan (1996)	20.7045	20.6689	7.2695
DOBKF (ten-state )	20.6887	20.6689	7.2650
Kalman Filter (five-state)	2.2623	0.7945	4.2494
DOBKF (five-state )	2.2625	0.7945	4.2492

Table 2.

Convergence time of misalignment angles for different methods.

Filtering Method (s)	$ϕ_{E}$	$ϕ_{N}$	$ϕ_{U}$
Kalman Filter (ten-state)	25	25	200
Fang and Wan (1996)	25	25	100
DOBKF (ten-state)	25	25	60
Kalman Filter (five-state)	25	25	200
DOBKF (five-state)	25	25	60

Conclusion

In this paper, a new form of DOBKF approach has been proposed for initial alignment of the SINS on stationary base. In this method, estimations of horizontal misalignment angles are obtained by a Kalman filter, which are then used as the input to design a disturbance observer, which is used to estimate the azimuth alignment angle. DOBKF solves the problem of fast estimating azimuth alignment angles. It is applied to two kinds of error model in this study, which are ten-state and five-state error models. An example is given to confirm the effectiveness of the proposed method. The simulation results show the estimation accuracy and convergence speed of three filtering methods. Compared with the Kalman filter, the convergence time of DOBKF is improved by $70 %$ in the simulation example, which means it needs about a third of the time a Kalman filter takes. Compared with Fang and Wan’s (1996) method, the convergence time of DOBKF is improved by $40 %$ in the simulation example; DOBKF has comparable accuracy with a Kalman filter. Put simply, the method in this paper greatly reduces the time of initial alignment while retaining comparable accuracy with KF. However, both the accuracy and the convergence speed of DOBKF are validated based on the simulation data, and real navigation data will be required to evaluate and compare for different methods. The robustness of the proposed method in a real sophisticated environment will also be tested in further research.

Footnotes

Declaration of conflicting interests

The authors declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported in part by Key Project of Chinese National Programs for Fundamental Research and Development (973 program) under grant number 2012CB720003, the National Natural Science Foundation of China (grant numbers 613201060107,61127007), Special Program for International Science and Technology Cooperation from Ministry of Science and Technology of China under Grant(2013DFE13040), the Fundamental Research Funds for the Central Universities under grant number CXZZ13_0103.

References

Ahn

Won

(2006) Fast alignment using rotation vector and adaptive Kalman Filter. IEEE Transactions on Aerospace and Electronic Systems 42(1): 70–82.

Aranda

De La Cruz

Dormido

. (1994) Reduced-order Kalman Filter for alignment. Cybernetics and Systems: An International Journal 25(1): 1–16.

Bar-Itzhack

Berman

(1988) Control theoretic approach to inertial navigation systems. Journal of Guidance and Control 11(3): 237–245.

Fang

Yang

(2011) Study on innovation adaptive EKF for in-flight alignment of airborne POS. IEEE Transactions on Instrumentation and Measurement 60(4): 1378–1388.

Cao

Guo

(2012) Multi-objective robust initial alignment algorithm for inertial navigation system with multiple disturbances. Aerospace Science and Technology 21(1): 1–6.

Chang

. (2014) Optimization-based alignment for strapdown inertial navigation system: comparison and extension. arXiv preprint arXiv:1411.5869.

Chen

Ballance

Gawthrop

. (2000) A nonlinear disturbance observer for robotic manipulators. IEEE Transactions on Industrial Electronics 47(4): 932–938.

Chen

Yang

Guo

. (2016) Disturbance observer-based control and related methods: an overview. IEEE Transactions on in Industrial Electronics 63(2): 1083–1095.

Fang

Wan

(1996) A fast initial alignment method for strapdown inertial navigation system on stationary base. IEEE Transaction on Aerospace and Electronics Systems 32(4): 1501–1505.

10.

Groves

(2008) Principles of GNSS, Inertial, and Multisensor Integrated Navigation SystemsLondon: Artech House.

11.

Guo

Chen

(2005) Disturbance attenuation and rejection for systems with nonlinearity via DOBC approach. International Journal of Robust and Nonlinear Control 15: 109–125.

12.

Guo

Cao

. (2012) Initial alignment for nonlinear inertial navigation systems with multiple disturbances based on enhanced anti-disturbance filtering. International Journal of Control 85(5): 491–501.

13.

Guo

Cao

(2013) Anti-Disturbance Control for Systems with Multiple DisturbancesCRC Press.

14.

Kalman

(1961) A New Approach to Linear Filtering and Prediction Theory, Journal of Basic Eng(ASME), 83D: 95–108.

15.

Yang

Chen

. (2014) Disturbance Observer-Based Control: Methods and Applications. Boca Raton, FL: CRC Press.

16.

Wang

. (2013) A fast SINS initial alignment scheme for underwater vehicle applications. The Journal of Navigation 66: 181–198.

17.

Liu

Guo

Zhang

(2012) An anti-disturbance PD control scheme for attitude control and stabilization of flexible spacecrafts. Nonlinear Dynamics 67: 2081–2088.

18.

Liu

Wang

Deng

(2012) Rapid alignment method of INS with large initial azimuth uncertainty under complex dynamic disturbances. In:UKACC International Conference on Control, pp. 1070–1075.

19.

Xie

Chen

(2008) Reduced-order Kalman Filter for RLG SINS initial alignment. In: Chinese control and decision conference, Yantai, China, 2–4 June 2008, pp.3675–3680.

20.

Mitsantisuk

Ohishi

Urushihara

. (2011) Kalman filter-based disturbance observer and its applications to sensorless force control. Advanced Robotics 25: 335–353.

21.

Mitsantisuk

Ohishi

Katsura

(2012) Estimation of action/reaction forces for the bilateral control using a Kalman Filter. IEEE Transactions on Industrial Electronics 59(11): 4383–4393.

22.

Naser

Sameh

(2004) Wavelet de-noising for IMU alignment. IEEE Transaction on Aerospace and Electronics Systems 19(1): 32–39.

23.

Ohishi

Nakao

Ohnishi

. (1987) Microprocessor-controlled DC motor dor load-insensitive position servo system. IEEE Transactions on Industrial Electronics 1(IE 34): 44–49.

24.

Park

Kim

(2006) UKF based in-flight alignment using low cost IMU. In: AIAA guidance, navigation, and control conference and exhibit, Keystone, Colorado, 21–24 August 2006, pp.2637–2648.

25.

Phuong

Ohishi

Yokokura

. (2014) FPGA-based high-performance force control system with friction-free and noise-free force observation. IEEE Transactions on Industrial Electronics 61(2): 994–1008.

26.

Rogers

(2007) Applied Mathematics in Integrated Navigation Systems. 3rd ed.Virginia: American Institute of Aeronautics and Astronautics, Inc.

27.

Shin

Naser

(2004) An unscented Kalman Filter for in-motion alignment of low-cost IMUs. In: IEEE/ION position location and navigation symposium, Canada, 26–29 April 2004, pp.273–279.

28.

Simon

(2006) Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. New York: John Wiley & Sons.

29.

Titterton

Weston

(2004) Strapdown Inertial Navigation Technology. 2nd ed.London: Lavenham Press Ltd.

30.

Wang

(2009) Fast alignment and calibration algorithms for inertial navigation system. Aerospace Science and Technology 13(2): 204–209.

31.

Wei

Guo

(2009) Composite disturbance-observer-based control and terminal sliding mode control for nonlinear systems with disturbances. International Journal of Control 82: 1082–1098.

32.

Wei

Guo

(2010) Composite disturbance-observer-based control and H Infinity control for complex continuous models. International Journal of Robust and Nonlinear Control 20(1): 106–118.

33.

. (2011) Optimization-based alignment for inertial navigation systems: theory and algorithm. Aerospace Science and Technology 15(1): 1–17.

34.

Yang

(2013) Sliding-mode control for systems with mismatched uncertainties via a disturbance observer. IEEE Transactions on Industrial Electronics 60(1): 160–169.

35.

Zhang

Liu

. (2011) Research for the application of UKF in moving-vased alignment of SIMU. Procedia Engineering 15: 4397–4402.

36.

Zhang

Tian

Jin

(2004) A novel method improving the alignment accuracy of a strapdown inertial navigation system on a stationary base. Measurement Science and Technology 15: 765–769.