In-Flight Alignment Algorithm Based on ADD2 for Airborne POS

Abstract

The Position and Orientation System (POS) is a special Strapdown Inertial Navigation System (SINS)/Global Positioning System (GPS) integrated system, widely employed in airborne remote sensing. In-Flight Alignment (IFA) is an effective way to improve the accuracy and speed of initial alignment for an airborne POS. IFA is normally accomplished with references from the position and velocity of GPS for SINS, so that unstable GPS measurements will result in poor alignment accuracy. To improve alignment accuracy under unstable GPS conditions, an adaptive filtering algorithm of the Second-order Divided Difference filter (DD2) based on adaptive innovation estimation is proposed, which introduces calculated innovation covariance directly into computation of the filter gain matrix. Then, the adaptive DD2 algorithm is used for the IFA of the POS with a large initial heading error. To validate the proposed algorithm, simulations are undertaken, followed by IFA experiments for the prototype of the airborne POS (TX-F30) under a turning manoeuvre in a car-mounted experiment, and under an “8” manoeuvre in-flight. The simulations and experimental results show that the proposed algorithm can reach better alignment accuracy under unknown statistical characteristic of GPS measurement noises.

1. INTRODUCTION

The Position and Orientation System (POS) is a special Strapdown Inertial Navigation System (SINS)/Global Positioning System (GPS). Compared with the conventional SINS/GPS integrated system, POS can provide position and orientation with greater accuracy and frequency from the airborne sensor; thus, it has become one of the key technologies in airborne remote sensing.

SINS alignment is an important process in determining the angular relationship between the navigation frame and body frame. Thus, initial alignment is critical if precision navigation is to be achieved. In-Flight Alignment (IFA) is defined as in-flight estimates of attitude errors between navigation frame and body frame.

In a number of cases, an Inertial Navigation System (INS) is to be aligned under conditions where external course information is unstable, which makes it necessary to take account of the nonlinear character of the problem. Therefore, the large-heading-error-model is adopted to describe nonlinearity during IFA (Kong et al., 1999; Scherzinger, 1996; Scherzinger and Reid, 1994).

In recent years, the problems of IFA based on nonlinear filter of INS alignment algorithms have been considered in a great number of publications (Dmitriyev et al., 1997; Fang and Yang, 2011; Han and Wang, 2009; Park et al., 2006).

The Extended Kalman Filter (EKF) linearises the nonlinear models by first-order Taylor approximations of state transition, so that the traditional linear Kalman filter can be applied. Further, an EKF is employed in IFA (Fang and Yang, 2011). However, its inadequacies, such as a vast calculation of matrix differentiation and higher-order truncation errors, restrict its wide application. In addition, the larger the initial heading error, the more inaccurate is the performance of the EKF.

These EKF drawbacks can be overcome by using an Unscented Kalman Filter (UKF) (Park et al., 2006). A UKF is based on the Unscented Transformation (UT), which is founded on the concept that an approximation of a probability distribution is easier than that of an arbitrary nonlinear function (Julier and Uhlmann, 2004). However the UT has both a local and a global sampling problem. So, modified UKFs are proposed to solve this issue (Hong et al., 2004; Kim and Park, 2006; Kim and Park, 2010). The more advanced techniques generally improve estimation accuracy, but they often perform at the expense of further complications in implementation and an increased computational burden (NØrgaard et al., 2000). Meanwhile, other technologies are also applied in an IFA (Han and Wang, 2009; Hao et al., 2006; Hong et al., 2010; Wang et al., 2011; Yu et al., 2004), but with a complex procedure.

The Divided Difference Filter (DDF), which is based on polynomial approximations of the nonlinear transformations, obtained with particular multidimensional extension of Stirling's interpolation formula, is simple to implement as no derivatives are needed and it can achieve better covariance estimates compared to EKF (Ali and Ullah Baig Mirza, 2011; Setoodeh et al., 2007).

However, the estimation performance depends on correct statistical characterisation of the process and measurement noise covariance matrices (Q and R, respectively). For IFA of POS, GPS measurement noise will change with aircraft manoeuvre or electromagnetic interference and other factors. Unstable GPS measurement disturbance will inevitably degrade the performance of the filter with the fixed R matrix (Fang and Yang, 2011).

In this paper, an Adaptive Second-Order Divided Difference filter (ADD2) based on innovation covariance estimation is proposed which introduces the covariance estimation of innovation into the calculation of the Second-order Divided Difference filter (DD2) gain matrix directly, instead of adjusting the R matrix. Then, the proposed ADD2 is applied in the IFA of the airborne POS with a large initial heading error. Moreover the accuracy of EKF and DD2 are analysed. Finally, experiments are carried out to validate the effectiveness of ADD2 compared with Adaptive Extended Kalman Filter (AEKF) with large heading error under unstable GPS measurement.

The remainder of this paper is organized as follows. Section 2 presents the proposed ADD2 algorithm based on innovation covariance estimation. Section 3 extends the application of the ADD2 to the IFA of airborne POS, and then the performance of the proposed scheme is evaluated in Section 4. The Conclusions are set out in Section 5.

2. ADAPTIVE DD2 ALGORITHM BASED ON INNOVATION COVARIANCE ESTIMATION

The innovation of the filter can be observed directly. So by observing the covariance of the innovation sequence, the filter performance can be corrected to some extent. Generally, the innovation of the filter should be a white noise sequence with zero mean. However, an inexact knowledge of the process and/or measurement noise will lead to complex statistical characteristics of the innovation sequence. Therefore, through the innovation covariance estimation, the process noise covariance matrix Q and/or measurement noise covariance matrix R can be adjusted adaptively to prevent the divergence of the filter (Bian et al., 2006). In this paper, an adaptive DD2 algorithm based on Innovation-based Adaptive Estimation (IAE) is discussed, considering the incomplete or change information of the measurement noise. Consider a nonlinear system with a nonlinear state model and a linear measurement model:

(1)$$\left\{ {\matrix{ {{\bf \dot x}(t) = {\bf f} \left( {{\bf x},t} \right) + {\bf w}(t)} \cr {{\bf y}(t) = {\bf H}(t){\bf x}(t) + {\bf v}(t)} \cr}} \right.$$

where:

• x(t) is the state vector.

• f is the nonlinear function.

• w(t) is the system noise with covariance Q(t).

• y(t) is the measurement vector.

• H is the linear function.

• v(t) is the measurement noise with covariance R(t).

2.1. The Basic Structure of the DDF

The DDF adopts an alternative linearization method, called a divided difference approximation, in which derivatives are replaced by functional evaluations and an easy expansion of the nonlinear functions to higher order terms is possible.

The basic DD2 filter is described as follows (NØrgaard et al., 2000).

2.1.1. Initialization

The initial state vector ${\bf \hat x}_0 $ and the square root matrix ${\bf \hat S}_x $ of its covariance P₀ are given as:

(2)$${\bf P}_0 = {\bf \hat S}_x {\bf \hat S}_x^T, \quad {\bf Q}_0 = {\bf \hat S}_w {\bf \hat S}_w^T $$$$

2.1.2. The a priori Update

For the a priori update of the state estimate, the improved state estimate is obtained:

(3)$${\bf \bar x}_k = \displaystyle{{h^2 - n_x - n_w} \over {h^2}} {\bf f}({\bf \hat x}_{k - 1} ) + \displaystyle{1 \over {2h^2}} \sum\limits_{\,p = 1}^{n_x} {\left[ {{\bf f}({\bf \hat x}_{k - 1} + h{\bf \hat s}_{x,p} ) + {\bf f} ({\bf \hat x}_{k - 1} - h{\bf \hat s}_{x,p} )} \right]} $$

where:

• n _x and n _w are dimension of state and process noise, respectively.

• h is the selection of interval length, and assuming that the estimation errors are Gaussian and unbiased, one should set h ² = 3.

The triangular Cholesky factor of the a priori covariance is obtained by Householder transformation of the following compound matrix:

(4)$${\bf \bar S}_x (k) = \left[ {\matrix{ {{\bf S}_{x\hat x}^{(1)} (k - 1)} & {{\bf S}_{xw}^{(1)} (k - 1)} & {{\bf S}_{x\hat x}^{(2)} (k - 1)} & {{\bf S}_{xw}^{(2)} (k - 1)} \cr}} \right]$$

(5)$$\openup6\left\{ {\matrix{ {{\bf S}_{x\hat x}^{(1)} (k) = \left\{ {{\bf S}_{x\hat x}^{(1)} (k)_{(i,j)}} \right\} = \left\{ {{{\left( {{\bf f}_i ({\bf \hat x}_k + h{\bf \hat s}_{x,j} ) + {\bf f}_i ({\bf \hat x}_k - h{\bf \hat s}_{x,j} )} \right)} / {2h}}} \right\}} \hfill \cr {{\bf S}_{xw}^{(1)} (k) = \left\{ {{\bf S}_{xw}^{(1)} (k)_{(i,j)}} \right\} = \left\{ {{{\left( {{\bf f}_i ({\bf \hat x}_k + h{\bf \hat s}_{w,j} ) + {\bf f}_i ({\bf \hat x}_k - h{\bf \hat s}_{w,j} )} \right)} / {2h}}} \right\}} \hfill \cr {{\bf S}_{x\hat x}^{(2)} (k) = \left\{ {{{\sqrt {h^2 - 1} \left( {{\bf f}_i ({\bf \hat x}_k + h{\bf \hat s}_{x,j} ) + {\bf f}_i ({\bf \hat x}_k - h{\bf \hat s}_{x,j} ) - 2{\bf f}_i ({\bf \hat x}_k )} \right)} / {2h^2}}} \right\}} \hfill \cr {{\bf S}_{xw}^{(2)} (k) = \left\{ {{{\sqrt {h^2 - 1} \left( {{\bf f}_i ({\bf \hat x}_k + h{\bf \hat s}_{w,j} ) + {\bf f}_i ({\bf \hat x}_k - h{\bf \hat s}_{w,j} ) - 2{\bf f}_i ({\bf \hat x}_k )} \right)} / {2h^2}}} \right\}} \hfill \cr}} \right.$$

where:

• subscript j denotes the jth column of related matrix, and similarly for the other factors.

2.1.3. The a posteriori Update

The a priori estimate of the output and its covariance is calculated by the following equations:

(6)$${\bf \hat x}_k = {\bf \bar x}_k + {\bf K}_k ({\bf y}_k - {\bf \bar y}_k )$$

(7)$${\bf \bar y}_k = \displaystyle{{h^2 - n_x - n_v} \over {h^2}} {\bf H}_k \cdot {\bf \bar x}_k + \displaystyle{1 \over {2h^2}} \sum\limits_{p = 1}^{n_x} {\left( {{\bf H}_k \cdot ({\bf \bar x}_k + h{\bf \hat s}_{x,p} ) + {\bf H}_k \cdot ({\bf \bar x}_k - h{\bf \hat s}_{x,p} )} \right)} $$

where:

• n _v is the dimension of measurement,

and:

(8)$${\bf K}_k = {\bf P}_{xy} (k)\left[ {{\bf S}_y (k){\bf S}_y^T (k)} \right]^{ - 1} $$

(9)$${\bf P}_{xy} (k) = {\bf \bar S}_x (k){\bf S}_{y\bar x}^{(1)} (k)^T $$

The correlative parameters are obtained using the following equations:

(10)$${\bf S}_y (k) = \left[ {\matrix{ {{\bf S}_{y\bar x}^{(1)} (k - 1)} & {{\bf S}_{yv}^{(1)} (k - 1)} & {{\bf S}_{y\bar x}^{(2)} (k - 1)} & {{\bf S}_{yv}^{(2)} (k - 1)} \cr}} \right]$$

(11)$$\openup6pt\left\{ {\matrix{ {{\bf S}_{y\bar x}^{(1)} (k) = \left\{ {{\bf S}_{y\bar x}^{(1)} (k)_{(i,j)}} \right\} = \left\{ {{{\left( {{\bf H} _i \cdot ({\bf \bar x}_k + h{\bf \bar s}_{x,j} ) + {\bf H} _i \cdot ({\bf \bar x}_k - h{\bf \bar s}_{x,j} )} \right)} / {2h}}} \right\}} \hfill \cr {{\bf S}_{yv}^{(1)} (k) = \left\{ {{\bf S}_{yv}^{(1)} (k)_{(i,j)}} \right\} = \left\{ {{{\left( {{\bf H} _i \cdot ({\bf \bar x}_k + h{\bf s}_{v,j} ) + {\bf H} _i \cdot ({\bf \bar x}_k - h{\bf s}_{v,j} )} \right)} / {2h}}} \right\}} \hfill \cr {{\bf S}_{y\bar x}^{(2)} (k) = \left\{ {{{\sqrt {h^2 - 1} \left( {{\bf H} _i \cdot ({\bf \bar x}_k + h{\bf \bar s}_{x,j} ) + {\bf H} _i \cdot ({\bf \bar x}_k - h{\bf \bar s}_{x,j} ) - 2{\bf H} _i \cdot ({\bf \bar x}_k )} \right)} / {2h^2}}} \right\}} \hfill \cr {{\bf S}_{yv}^{(2)} (k) = \left\{ {{{\sqrt {h^2 - 1} \left( {{\bf H} _i \cdot ({\bf \bar x}_k + h{\bf s}_{v,j} ) + {\bf H} _i \cdot ({\bf \bar x}_k - h{\bf s}_{v,j} ) - 2{\bf H} _i \cdot ({\bf \bar x}_k )} \right)} / {2h^2}}} \right\}} \hfill \cr}} \right.$$

(12)$${\bf R} = {\bf S}_v {\bf S}_v^T $$

The a posteriori update of the estimation error covariance is:

(13)$$\eqalign{ {\bf \hat P}_x (k) =&\,\, {\bf \bar S}_x (k) - {\bf K}_k {\bf S}_{y\bar x}^{(1)} (k)\left( {{\bf \bar S}_x (k) - {\bf K}_k {\bf S}_{y\bar x}^{(1)} (k)} \right)^T + {\bf K}_k {\bf S}_{yv}^{(1)} (k)\left( {{\bf K}_k {\bf S}_{yv}^{(1)} (k)} \right)^T \cr & + {\bf K}_k {\bf S}_{y\bar x}^{(2)} (k)\left( {{\bf K}_k {\bf S}_{y\bar x}^{(2)} (k)} \right)^T + {\bf K}_k {\bf S}_{yv}^{(2)} (k)\left( {{\bf K}_k {\bf S}_{yv}^{(2)} (k)} \right)^T} $$

This has the Cholesky factor:

(14)$${\bf \hat S}_x (k) = \left[ {\matrix{ {{\bf \bar S}_x (k) - {\bf K}_k {\bf S}_{y\bar x}^{(1)} (k)} & {{\bf K}_k {\bf S}_{yv}^{(1)} (k)} & {{\bf K}_k {\bf S}_{y\bar x}^{(2)} (k)} & {{\bf K}_k {\bf S}_{yv}^{(2)} (k)} \cr}} \right]$$

2.2. Adaptive DD2 Algorithm Based On Innovation Covariance Estimation

Innovation information is defined as ${\bf z}_k = {\bf y}_k - {\bf \bar y}_k $, and according to Shademan and Sharifi, (2005), its covariance estimation can be obtained as follows:

(15)$$D({\bf z}_k ) = \displaystyle{1 \over N}\sum\limits_{i = i0}^k {\left[ {{\bf z}_k {\bf z}_k^T} \right]} $$

where:

• N is the window size over which the moving average of z_kz_k^T is taken as the estimation of the innovation covariance.

The theoretical innovation covariance of DD2 with a fixed R should be P _y(k) = S_y(k)S_y(k)^T, where S_y(k) is calculated as Equation (10); however, it cannot reflect the variations of the external measurement noise with a fixed R and will decrease the filter performance when the statistical characteristic of the measurement noise is changed.

An adaptive strategy based innovation covariance estimation is introduced. When the innovation covariance estimation differs from the theoretical value, this hints that the statistical characteristic of the measurement noise is changed. Then, the dissimilarity between them can be used to adjust the measurement noise covariance matrix R.

(16)$${\bf P}_y (k) = E\left[ {{\bf z}_k {\bf z}_k^T} \right] = \displaystyle{1 \over N}\sum\limits_{i = i0}^k {\left[ {{\bf z}_k {\bf z}_k^T} \right]} $$

The calculation of K_k is:

(17)$${\bf K}_k = {\bf P}_{xy} (k)\left[ {{\bf S}_y (k){\bf S}_y^T (k)} \right]^{ - 1} = {\bf P}_{xy} (k)\left[ {{\bf P}_y (k)} \right]^{ - 1} $$

Then, if measurement noise increases, the estimated innovation covariance will also increase. Moreover, the filter gain K_k will decrease, which means that it depends less on measurement information.

In this paper, estimation of innovation covariance is introduced into the calculation of the gain matrix K_k directly, and the ADD2 algorithm is as follows:

2.2.1. Initialization

The initial state vector ${\bf \hat x}_0 $ and the square root matrix ${\bf \hat S}_x $ of its covariance P₀ are given as:

(18)$${\bf P}_0 = {\bf \hat S}_x {\bf \hat S}_x^T, \quad {\bf Q}_0 = {\bf \hat S}_w {\bf \hat S}_w^T $$$$

2.2.2. The a priori Update

For the a priori update of the state estimate, the improved state estimate is obtained as:

(19)$${\bf \bar x}_k = \displaystyle{{h^2 - n_x - n_w} \over {h^2}} {\bf f}({\bf \hat x}_{k - 1} ) + \displaystyle{1 \over {2h^2}} \sum\limits_{\,p = 1}^{n_x} {\left[ {{\bf f}({\bf \hat x}_{k - 1} + h{\bf \hat s}_{x,p} ) + {\bf f} ({\bf \hat x}_{k - 1} - h{\bf \hat s}_{x,p} )} \right]} $$

2.2.3. The a posterior Update

The a posterior update is:

(20)$${\bf \bar y}_k = \displaystyle{{h^2 - n_x - n_v} \over {h^2}} {\bf H}_k \cdot {\bf \bar x}_k + \displaystyle{1 \over {2h^2}} \sum\limits_{\,p = 1}^{n_x} {\left( {{\bf H}_k \cdot ({\bf \bar x}_k + h{\bf \hat s}_{x,p} ) + {\bf H}_k \cdot ({\bf \bar x}_k - h{\bf \hat s}_{x,p} )} \right)} $$

(21)$${\bf z}_k = {\bf y}_k - {\bf \bar y}_k $$

then:

the estimation of the innovation covariance is:

(22)$${\bf P}_y (k) = E\left[ {{\bf z}_k {\bf z}_k^T} \right] = \displaystyle{1 \over N}\sum\limits_{i = i0}^k {\left[ {{\bf z}_k {\bf z}_k^T} \right]} $$

and:

the gain K_k is obtained as:

(23)$$\openup3pt\eqalign{& {\bf K}_k = {\bf P}_{xy} (k)\left[ {{\bf S}_y (k){\bf S}_y^T (k)} \right]^{ - 1} = {\bf P}_{xy} (k)\left[ {{\bf P}_y (k)} \right]^{ - 1} \cr & {\bf \hat x}_k = {\bf \bar x}_k + {\bf K}_k {\bf z}_k} $$$$

In the next section, the proposed ADD2 is used in the IFA of POS with large initial heading error.

3. IFA WITH LARGE INITIAL HEADING ERROR USING ADD2

3.1. Main Coordinate Frames

3.1.1. i-frame (Inertial Frame)

The inertial frame located at the centre of the Earth (point O) is non-rotating with respect to the fixed stars. Its xi-axis is in the equatorial plane and points to the vernal equinox, its zi-axis is normal to that plane, and its yi-axis completes the right-handed system.

3.1.2. e-frame (Earth frame)

The Earth frame located at the centre of the Earth has its ze-axis through the true North pole and its xe-axis through the intersection of the prime meridian (0° longitude) and the Equator (0° latitude).

3.1.3. n-frame (Navigation Frame)

The navigation frame is a local-level frame, located at the surface of the Earth (point P). Its xn-axis points eastward, its yn-axis points northward, and its zn-axis is parallel to the upward vertical.

3.1.4. b-frame (Body Frame)

The body frame is fixed to the vehicle and is located at the centroid of the vehicle. Its yb-axis along the longitudinal axis of the vehicle points forward, and its xb-axis points to the right side. Its zb-axis completes the right-handed system.

3.1.5. p-frame (Platform Frame)

The platform frame is also located at point P and has an angle error with respect to the n-frame.

3.2. State Model

In this paper, a state vector of 13 dimensions is chosen, including level position error $\delta {\bf P} = \left[ {\matrix{ {\delta L} & \delta \cr} \lambda} \right]$, level velocity error $\delta {\bf V} = \left[ {\matrix{ {\delta V_E} & {\delta V_N} \cr}} \right]$, attitude error $\phi = \left[ {\matrix{ {\phi _E} & {\phi _N} & {\phi _U} \cr}} \right]$, and inertial sensors error ${\bf \varepsilon} = \left[ {\matrix{ {\varepsilon _x} & {\varepsilon _y} & {\varepsilon _z} \cr}} \right]$, $\nabla = \left[ {\matrix{ {\nabla _x} & {\nabla _y} & {\nabla _z} \cr}} \right]$.

Subscripts E, N, U denote the East, North, and Up components in the n-frame, respectively, and subscripts x, y, and z denote the right, front, and up components in the b-frame, respectively.

The large heading error model (Yu et al., 1999) of the SINS will be used in this paper for nonlinear IFA, which can be separated into a linear part and a nonlinear part as follows:

(24)$$\openup4\left[ {\matrix{ {\delta {\bf \dot P}} \cr {\delta {\bf \dot V}} \cr {\dot \phi} \cr {{\bf \dot \varepsilon}} \cr {\dot \nabla} \cr}} \right] = \left[ {\matrix{ {{\bf F}_1} & {{\bf F}_2} & {\bf 0} & {\bf 0} & {\bf 0} \cr {\bf 0} & {{\bf F}_3} & {\bf 0} & {\bf 0} & {{\bf C}_b^p} \cr {\bf 0} & {\bf 0} & {\bf 0} & {{\bf C}_b^p} & {\bf 0} \cr {\bf 0} & {\bf 0} & {\bf 0} & {\bf 0} & {\bf 0} \cr {\bf 0} & {\bf 0} & {\bf 0} & {\bf 0} & {\bf 0} \cr}} \right]\left[ {\matrix{ {\delta {\bf P}} \cr {\delta {\bf V}} \cr \phi \cr {\bf \varepsilon} \cr \nabla \cr}} \right] + \left[ {\matrix{ {\bf 0} \cr {({\bf C}_n^p - {\bf I}){\bf C}_b^n {\bf f}^b - \delta {\bf \omega} _{en}^n \times {\bf V}} \cr {({\bf I} - {\bf C}_n^p ){\bf \omega} _{in}^n - \delta {\bf \omega} _{en}^n} \cr {\bf 0} \cr {\bf 0} \cr}} \right]$$

where:

$$\openup4{\bf F}_1 = \left[ {\matrix{ 0 & 0 \cr {{{V_E \tan L} / {\left( {(R_N + h)\cos L} \right)}}} & 0 \cr}} \right],$

$$\openup4{\bf F}_2 = \left[ {\matrix{ 0 & {{1 / {\left( {R_{\rm M} + h} \right)}}} \cr {{{\sec L} / {\left( {R_{\rm N} + h} \right)}}} & 0 \cr}} \right],$

F₃ is the first two rows of − (2Ω_ieⁿ + Ω_enⁿ).

The definition of some other parameter and constant are referenced in Yu et al. (1999).

3.3. Measurement Model

The level position and velocity errors are taken as observations for the filter. This can be obtained from the errors between the SINS and the GPS. Therefore, the measurement model is linear and can be written as:

(25)$${\bf z}(t) = \left[ {\matrix{ {L_{{\rm INS}} - L_{{\rm GPS}}} \cr {\lambda _{{\rm INS}} - \lambda _{{\rm GPS}}} \cr {V_{E\,{\rm INS}} - V_{E\,{\rm GPS}}} \cr {V_{N\,{\rm INS}} - V_{N\,{\rm GPS}}} \cr}} \right] = {\bf Hx}(t) + {\bf v}(t)$$

where:

• ${\bf H} = \left[ {\matrix{ {{\bf I}_{4 \times 4}} & {{\bf 0}_{4 \times 9}} \cr}} \right]$ is the observation matrix.

When applying the proposed ADD2 in IFA, the dimension effect of accelerometers is compensated first, then the position and velocity differences between the SINS and the GPS after lever-arm compensation are taken as the measurements for the filter. The estimated errors of position, velocity and attitude are fed back directly. However, for the inertial sensor errors, feeding them back directly may cause additional errors to the filter due to their poor observability. Since the manoeuvre during the IFA can improve the observability of these states, inertial sensor errors are simply fed back to the SINS when the IFA is finished. A block diagram of IFA based on ADD2 is shown in Figure 1.

Figure 1. Block diagram of IFA based on ADD2.

3.4. Attitude Error Based on EKF and DD2

The DD2 is able to achieve better estimation accuracy of IFA under large heading error than EKF. Using the indirect feedback Kalman filter, the estimated values of the states tend to be always zero (Park et al., 2006). Thus, there exists linearised error. However, we show that the DD2 has less linearized error than EKF.

The attitude error model is given as:

(26)$$\openup3{\bf C}_p^n = \left[ {\matrix{ {\cos \phi _U} & {\sin \phi _U} & {\phi _N \cos \phi _U + \phi _E \sin \phi _U} \cr { - \sin \phi _U} & {\cos \phi _U} & {\phi _N \sin \phi _U - \phi _E \cos \phi _U} \cr { - \phi _N} & {\phi _E} & 1 \cr}} \right]$$

where:

• C_pⁿ is the direction cosine matrix from the p-frame to the n-frame.

• C_b^p is the direction cosine matrix from the b-frame to the p-frame.

Assuming C_b^p = I_3 × 3:

(27)$$\openup3\Delta {\bf C} = \left[ {\matrix{ {1 - \cos \phi _U} & { - \sin \phi _U} & { - \phi _N \cos \phi _U - \phi _E \sin \phi _U} \cr { - \sin \phi _U} & {1 - \cos \phi _U} & { - \phi _N \sin \phi _U + \phi _E \cos \phi _U} \cr {\phi _N} & { - \phi _E} & 0 \cr}} \right]$$

As the linearized point of EKF is ϕ _E = ϕ _N = ϕ _U = 0, the EKF attitude error from Equation (27) is represented as (Park et al., 2006):

(28)$$\openup3\Delta {\bf C}_{{\rm EKF}} = \left[ {\matrix{ 0 & 0 & 0 \cr 0 & 0 & 0 \cr 0 & 0 & 0 \cr}} \right]$$

And using the matrix trace in Equations (27) and (28), the results are as follows:

(29)$${\rm tr}(\Delta {\bf C}) = 2 - 2\cos \phi _U $$

(30)$${\rm tr}(\Delta {\bf C}_{{\rm EKF}} ) = 0$$

The trace difference of EKF is calculated as:

(31)$$\delta {\bf C}_{{\rm EKF}} = {\rm tr}(\Delta {\bf C}) - {\rm tr}(\Delta {\bf C}_{{\rm EKF}} ) = 2 - 2\cos \phi _U = 2\left( {\displaystyle{{\phi _U^2} \over {2!}} - \displaystyle{{\phi _U^4} \over {4!}} + \displaystyle{{\phi _U^6} \over {6!}} - \cdots} \right)$$

The ΔC_DD2 of DD2 can be deduced as follows:

Considering the attitude covariance matrix p_A for convenience:

(32)$$\openup2\eqalign{\displaylines{\Delta {\bf C}_{{\rm DD2}}& = \displaystyle{1 \over 6}\left[ {\matrix{ {1 - \cos \sqrt {3p_{\phi _U}}} & {\sin \sqrt {3p_{\phi _U}}} & 0 \cr { - \sin \sqrt {3p_{\phi _U}}} & {1 - \cos \sqrt {3p_{\phi _U}}} & 0 \cr 0 & 0 & 0 \cr}} \right]}\hfill\cr&{\hskip-170pt+ \displaystyle{1 \over 6}\left[ {\matrix{ {1 - \cos \left( { - \sqrt {3p_{\phi _U}}} \right)} & {\sin \left( { - \sqrt {3p_{\phi _U}}} \right)} & 0 \cr { - \sin \left( { - \sqrt {3p_{\phi _U}}} \right)} & {1 - \cos \left( { - \sqrt {3p_{\phi _U}}} \right)} & 0 \cr 0 & 0 & 0 \cr}} \right]}\hfill \cr &{\hskip-180pt= \displaystyle{1 \over 3}\left[ {\matrix{ {2 - 2\cos \sqrt {3p_{\phi _U}}} & 0 & 0 \cr 0 & {2 - 2\cos \sqrt {3p_{\phi _U}}} & 0 \cr 0 & 0 & 0 \cr}} \right]}}}\hfill $$

(33)$${\rm tr}(\Delta {\bf C}_{{\rm DD2}} ) = {{\left( {2 - 2\cos \sqrt {3p_{\phi _U}}} \right)} / 3}$$

(34)$$\eqalign{\hskip40pt\displaylines{\delta {\bf C}_{{\rm DD2}} =& {\rm tr}(\Delta {\bf C}) - {\rm tr}(\Delta {\bf C}_{{\rm DD2}} )\hskip-92pt} \cr &{= {{\left( {2 - 2\cos \phi _U} \right) - \left( {2 - 2\cos \sqrt {3p_{\phi _U}}} \right)} / 3}} \cr &= \delta {\bf C}_{{\rm UKF}} \approx - \displaystyle{2 \over {4!}}\left( {\phi _U^4 - 3P_{\phi _U} ^2} \right) + \displaystyle{2 \over {6!}}\left( {\phi _U^6 - 9P_{\phi _U} ^4} \right) - \cdots} $$

Equations (31) and (34) show that DD2 error is introduced into the fourth and higher order terms, while the EKF is second and higher. Therefore compared with EKF, the accuracy estimated by DD2 is better with the larger heading error.

4. SIMULATION AND EXPERIMENT

To evaluate the proposed method, simulations and experiments are carried out, then the data are processed using ADD2 and AEKF.The window size in filters is N = 10 (Wang, 2000; Wang et al., 2000).

4.1 Simulation and Analysis

Simulations are designed based on high precision inertial unit and GPS. The specifications of the system are at Table 1.

Table 1. Specifications of POS.

A general flight trajectory is designed as part of a practical flight with a turning manoeuvre. The trajectory is composed of several segments of flying in line and turning with an attitude manoeuvre. The initial simulation information is listed in Table 2 and the simulation trajectory is shown in Figure 2.

Figure 2. Planning track.

Table 2. Simulation Specifications.

To test the validity of ADD2, keep the statistical characteristic of GPS noise normal for the first 20 seconds, change it four times during the manoeuvre and then revert to normal. The estimation results of ADD2 and AEKF are shown in Figure 3 through Figure 5. Comparison of attitude errors and time consumption in MATLAB® are shown in Table 3.

Figure 3. Estimation of the heading error.

Figure 4. Estimation of the roll error.

Figure 5. Estimation of the pitch error.

Table 3. Attitude Accuracy Comparison.

It can be concluded that ADD2 performance can provide a substantial performance increase over AEKF when measurement noise is changed. Consistent with the preceding analysis, because of the large linearized error of EKF, the estimation errors of AEKF are much higher than ADD2, particularly for the heading error. It can be concluded that the two filters have almost the same computational burden. Thus, it would be effortless for the proposed method to be used in real time.

4.2. Experiment and Analysis

To further validate the algorithm, experiments are carried out with the prototype of the high-accuracy airborne POS (designated TX-F30) developed by BeiHang University, which is comprised of the high-accuracy Fibre Optic Gyro (FOG) Inertial Measurement Unit (IMU) and the NovAtel DL-V3 GPS OEM board. The specifications are listed in Table 4, and the POS is shown in Figure 6. The GPS is adopted in real time processing, while the Carrier-phase Differential GPS (CDGPS) is employed for post processing. Forward filter and backward smoothing is used in post-processing, to gain higher accuracy.

Figure 6. POS TX-F30.

Figure 7. Configuration and car-mounted experiment.

Table 4. Specifications for TX-F30.

4.2.1. Car-Mounted Experiment

The experiments were carried out on the 5th ring road of Beijing; the trajectory is shown in Figure 8, in which the thick line in green of 200 s duration represents the segment used for IFA.

Figure 8. Ground trajectory and segment for IFA.

The alignment results are given in Figures 9 and 10. The results of ADD2 converge more quickly than AEKF, and with higher accuracy.

Figure 9. Curve of attitude during IFA.

Figure 10. Error of attitude during IFA.

The alignment results are given in Table 5. The attitude of ADD2 is more precise than that of AEKF.

Table 5. Alignment Results of IFA in Car-mounted Experiment.

Other than comparison with true initial attitude, there is another way to validate attitude precision. The CDGPS output is taken as a reference to calculate the level position error of SINS with 20 s after IFA with the same initial position. The position differences between the strapdown navigations with different IFA and GPS are in Figure 11.

Figure 11. SINS error of plan position of car-mounted experiment.

The result means that the proposed IFA method can improve the alignment accuracy and reduce the positioning errors (for the SINS only).

4.2.2. Flight Experiment

The POS is used for motion compensation of the X-band airborne InSAR. The aircraft and flight trajectory are shown in Figures 12 and 13, respectively. The thick line in green, which lasts for 450 s, represents the segment used for IFA.

Figure 12. Citation II and equipment installation.

Figure 13. 3-D flight trajectory and Plan Flight-Path of Mapping Area.

The alignment results are given in Figures 14 and 15.

Figure 14. Attitude during IFA.

Figure 15. Error of attitude during IFA.

These experimental results show that both AEKF and ADD2 filters can be adjusted with changes in measurement noise through innovation covariance estimation. However, the ADD2 filter yields the better accuracy. The alignment results are given in Table 6. The ADD2 filter also shows better results.

Table 6. Alignment Result of IFA in the Flight.

The CDGPS output is taken as a reference to calculate the level position error of SINS with 20 s after IFA with the same initial position. The position differences between the strapdown navigation with different IFA and the CDGPS are shown in Figure 16.

Figure 16. SINS error of plan position of the flight.

This result means that the proposed IFA method based on ADD2 can improve alignment accuracy and reduce positioning errors (for SINS only).

5. CONCLUSIONS

To resolve the issue that incorrect knowledge of measurement noise will cause degradation of filter performance, an innovation-covariance estimation-based Adaptive Second-order Divided Difference filter (ADD2) algorithm is proposed. The proposed method is applied to the In-Flight Alignment (IFA) of Fibre Optic Gyro (FOG) - Position and Orientation System (POS) with large initial heading error. The flight experimental results show excellent performance of the filter under the condition variation of characteristic of measurement noise. Compared with an Adaptive Extended Kalman Filter (AEKF), the proposed ADD2 can effectively suppress the impact of unstable Global Positioning System (GPS) measurement noise on state estimation and improve attitude accuracy. Further, the proposed ADD2 has an almost identical computational burden, but much higher estimation precision than AEKF.