Open AccessArticle

An Improved ACKF/KF Initial Alignment Method for Odometer-Aided Strapdown Inertial Navigation System

Kang Gao

^1,*,

Shunqing Ren

^1,*,

Guoxing Yi

¹,

Jiapeng Zhong

² and

Zhenhuan Wang

Space Control and Inertial Technology Research Center, Harbin Institute of Technology, Harbin 150080, China

HIT (Anshan) Institute of Industrial Technology, Anshan 114000, China

Authors to whom correspondence should be addressed.

Sensors 2018, 18(11), 3896; https://doi.org/10.3390/s18113896

Submission received: 21 October 2018 / Revised: 1 November 2018 / Accepted: 11 November 2018 / Published: 12 November 2018

(This article belongs to the Special Issue Gyroscopes and Accelerometers)

Download

Browse Figures

Versions Notes

Abstract

For a land-vehicle strapdown inertial navigation system (SINS), the problem of initial alignment with large misalignment angle in-motion needs to be solved urgently. This paper proposes an improved ACKF/KF initial alignment method for SINS aided by odometer. The SINS error equation with large misalignment angle is established first in the form of an Euler angle. The odometer/gyroscope dead reckoning (DR) error equation is deduced, which makes the observation equation linear when the position is taken as the observation of the Kalman filter. Then, based on the cubature Kalman filter, the Sage-Husa adaptive filter and the characteristics of the observation equation, an improved ACKF/KF method is proposed, which can accomplish initial alignment well in the case of unknown measurement noise. Computer simulation results show that the performance of the proposed ACKF/KF algorithm is superior to EKF, CKF and AEKF method in accuracy and stability, and the vehicle test validates its advantages.

Keywords:

strapdown inertial navigation system (SINS); initial alignment; odometer; Sage-Husa adaptive filter; cubature Kalman filter (CKF)

1. Introduction

Strapdown inertial navigation systems (SINSs) are widely used in modern navigation applications, especially in the military field because of their advantage of complete autonomy [1,2,3]. Initial alignment plays an important role in SINS, and the accuracy and efficiency of initial alignment affect the SINS capability directly [4,5,6]. The information about speed and location is more available than misalignment. Thus, the main task of initial alignment is to estimate the misalignment [7].

Unlike the alignment on the static base, the alignment on moving base calls for the carrier motion information provided by the external device [8,9]. The GPS is commonly onboard navigation equipment, which can provide high accuracy navigation information [10,11,12]. However, GPS signals are not stable, it can be affected by interference and shielding [13], and the poor stability of GPS is not competent for SINS initial alignment in the military field. With the development of image recognition technology, camera is becoming a promising choice for navigation system [14,15]. While, the visual navigation system calls for the easily identifiable features and known locations on paths, which is not available in the war [16]. Odometers measure the distance increase along the vehicle trajectory. They are a kind of economical, conveniently-deployed and widely-used sensor for land vehicles, and their fully self-contained characteristics are valuable in the military field [17,18], so we choose the odometer to accomplish the in-motion initial alignment.

However, for the SINS initial alignment in-motion with large misalignment angles, the standard Kalman filter is not competent, which leads to the development of non-linear filtering methods in recent years [19,20,21,22,23,24,25,26]. The extended Kalman filter (EKF) is one of the most outstanding nonlinear filters, and in [19] the GPS/SINS tightly coupled integration was completed by state transformation EKF. However, the truncation error caused by one-order Taylor expansion linearization approximation in EKF is large and the calculation of the Jacobian matrix for the alignment model with its high-dimensionality and strong nonlinear character is very complicated, which increases the computational load and brings about unacceptable calculation errors [20]. Based on the idea that approximating a probability distribution is easier than approximating a nonlinear function, Julier et al. proposed the Unscented Kalman Filter (UKF), in which an Unscented Transformation (UT) is used to obtain the statistical properties of the states instead of the local linearization [21]. It is proved that the UKF can achieve two-order accuracy. However, the covariance matrix is sometimes non-positive in high-dimensional systems, and the parameters in the UKF need to be adjusted accordingly [22,23]. In order to solve the high dimensionality nonlinear integral problem, Arasaratnam et al. [24] proposed the spherical radial cubature rule (SRCR), based on which the cubature Kalman filter (CKF) is established., The CKF has been widely used in many applications because of the better numerical stability and accuracy than UKF [25,26].

The Kalman filter should be initialized properly, especially when a coarse estimated initial attitude is asked for, and the statistical characteristics of system noise are needed [27]. However, for a vehicle running freely, the measurement noise changes with the environment and the coarse initial alignment is hard to achieved on a moving-base [28]. Without proper initialization, the Kalman filter- based SINS initial alignment aided by an odometer would hardly converge. Therefore, the adaptive filter method which can estimate the system noise online is necessary. Variance component estimation (VCE) is a kind of adaptive method which can estimate process and measurement noise simultaneously by utilizing residual vectors, but the algorithm is complicated and is rarely used in engineering [29,30,31]. The Sage–Husa adaptive filter is a widely-used adaptive filter, which uses the maximal posterior estimation principle to estimate the system noise Q or measurement noise R to improve the filtering accuracy [7,32,33]. Because of its simplicity and high reliability, the Sage–Husa algorithm has been widely used in engineering, therefore, this algorithm is selected for the estimation of measurement noise to improve the process of SINS initial alignment.

This paper is organized as follows: Section 2 deduces the SINS system equation and the odometer/gyroscope dead reckoning error equation with large misalignment error, and the Kalman filter equation is given. In Section 3, the Cubature Kalman filter and Sage-Husa adaptive filter are introduced, and the improved ACKF/KF method is proposed. The simulation and experiment results and analysis are reported in Section 4. Conclusions are finally drawn in Section 5.

2. Nonlinear Initial Alignment Equation

In order to better understand and deduce the SINS initial alignment aided by odometer, it is necessary to define the related coordinate systems, that is, the inertial frame (i-frame), the Earth frame (e-frame), the navigation frame (n-frame), the vehicle frame (a-frame), the SINS frame (b-frame), the odometer frame (m-frame) and the calculation frame (n’-frame). Here, we denote the geographic coordinate system “east-north-up (ENU)” as the n-frame and select the “right-front-up (RFU)” coordinate system as the a-frame. As the odometer is installed on the front wheel of the vehicle and measures the front speed of the vehicle, thus the m-frame is coincident with the a-frame. There is a small misaligned angle between the b-frame and the a-frame because of the installation error.

2.1. SINS Error Equation with Large Misalignment Angle

In general, there are misalignment angles between the n-frame and n′-frame. Denote the transformation from n-frame to n′-frame by rotating around z-axis, x-axis, and y-axis with

α_{z}

α_{x}

α_{y}

, and the Euler angle is defined as

α ≜ [α_{z} ， α_{x} ， α_{y}]

In practical applications, horizontal alignment is easier to achieve by mechanical means, such as designing the SINS installation surface to be a parallel structure to ensure that the horizontal misalignment angle is small. However, the azimuth angle is more complex. Owing to this, we can assume that the horizontal misalignment angles are small angles, only the azimuth misalignment angle is considered to be large one. Then the attitude transformation matrix from n-frame to n′-frame can be simplified as:

C_{n}^{n^{'}} = C_{α_{y}} C_{α_{x}} C_{α_{z}} \approx [\begin{matrix} \cos α_{z} & \sin α_{z} & - α_{y} \\ - \sin α_{z} & \cos α_{z} & α_{x} \\ α_{y} \cos α_{z} + α_{x} \sin α_{z} & α_{y} \sin α_{z} - α_{x} \cos α_{z} & 1 \end{matrix}]

(1)

Denote the angular velocity of n′-frame with respect to n-frame is

ω_{n n^{'}}^{n^{'}}

ω_{n n^{'}}^{n^{'}} = C_{α_{y}} C_{α_{x}} [\begin{matrix} 0 \\ 0 \\ {\dot{α}}_{z} \end{matrix}] + C_{α_{y}} [\begin{matrix} {\dot{α}}_{x} \\ 0 \\ 0 \end{matrix}] + [\begin{matrix} 0 \\ {\dot{α}}_{y} \\ 0 \end{matrix}] = C_{ω} \dot{α}

(2)

The differential equation of Euler angle is obtained as:

\dot{α} = C_{ω}^{- 1} ω_{n n^{'}}^{n^{'}}

(3)

where:

C_{ω}^{- 1} = [\begin{matrix} 1 & 0 & α_{y} \\ 0 & 1 & - α_{x} \\ - α_{y} & 0 & 1 \end{matrix}]

(4)

The matrix differential equation of SINS is

{\dot{C}}_{b}^{n} = C_{b}^{n} [ω_{i b}^{b} \times] - [ω_{i n}^{n} \times] C_{b}^{n}

(5)

where,

C_{b}^{n}

denotes the attitude transformation matrix from b-frame to n-frame,

ω_{i b}^{b}

is the body angular rate measured by gyroscopes denoted in b-frame,

ω_{i n}^{n}

is the angular rate of n-frame with respect to i-frame denoted in n-frame.

The attitude matrix differential equation including the calculation error is:

{\dot{C}}_{b}^{n^{'}} = C_{b}^{n^{'}} [{\tilde{ω}}_{i b}^{b} \times] - [ω_{i n^{'}}^{n^{'}} \times] C_{b}^{n^{'}}

(6)

where,

{\tilde{ω}}_{i b}^{b} = ω_{i b}^{b} + ε^{b}

ω_{i n^{'}}^{n^{'}} = ω_{i n}^{n} + δ ω_{i n^{'}}^{n^{'}}

ε^{b}

is gyroscope bias.

The attitude error matrix is defined as:

C_{b}^{n} = C_{n^{'}}^{n} C_{b}^{n^{'}} = C_{b}^{n^{'}} + δ C_{b}^{n^{'}}

(7)

Namely:

δ C_{b}^{n^{'}} = C_{b}^{n} - C_{b}^{n^{'}}

(8)

δ C_{b}^{n^{'}} = (C_{n^{'}}^{n} - I) C_{b}^{n^{'}}

(9)

The two sides of Equations (8) and (9) are differentiated respectively, subtracted and arranged as follows:

{\dot{C}}_{n^{'}}^{n} + C_{n^{'}}^{n} [ε^{n^{'}} \times] - C_{n^{'}}^{n} [ω_{i n^{'}}^{n^{'}} \times] + [ω_{i n^{'}}^{n^{'}} \times] C_{n^{'}}^{n} + [δ ω_{i n^{'}}^{n^{'}} \times] C_{n^{'}}^{n} = 0

(10)

Substituting

{\dot{C}}_{n^{'}}^{n} = C_{n^{'}}^{n} [ω_{n n^{'}}^{n^{'}} \times]

into Equation (10):

ω_{n n^{'}}^{n^{'}} + ε^{n^{'}} - ω_{i n^{'}}^{n^{'}} + C_{n}^{n^{'}} (ω_{i n^{'}}^{n^{'}} + δ ω_{i n^{'}}^{n^{'}}) = 0

(11)

By substituting Equation (2) into Equation (11), the nonlinear attitude error equation of SINS represented by Euler angle can be obtained:

\dot{α} = C_{ω}^{- 1} (- ε^{n^{'}} + (I - C_{n}^{n^{'}}) ω_{i n^{'}}^{n^{'}} - C_{n}^{n^{'}} δ ω_{i n^{'}}^{n^{'}})

(12)

According to the SINS specific force equation, when the error is not considered, the ideal velocity equation is:

{\dot{v}}^{n} = C_{b}^{n} f^{b} - (2 ω_{i e}^{n} + ω_{e n}^{n}) \times v^{n} + g^{n}

(13)

where,

v^{n}

is the velocity relative to n-frame measured by SINS,

f^{b}

is the specific force measured by accelerometers in b-frame,

ω_{e n}^{n}

is the angular rate of the e-frame with respect to n-frame, and

g^{n}

is the gravity vector.

The speed equation including the calculation error is:

{\dot{v}}^{n^{'}} = C_{b}^{n^{'}} {\tilde{f}}^{b} - (2 ω_{i e}^{n^{'}} + ω_{e n^{'}}^{n^{'}}) \times v^{n^{'}} + g^{n^{'}}

(14)

where,

v^{n} = v^{n^{'}} + δ v^{n^{'}}

ω_{i e}^{n} = ω_{i e}^{n^{'}} + δ ω_{i e}^{n^{'}}

ω_{e n}^{n} = ω_{e n^{'}}^{n^{'}} + δ ω_{e n^{'}}^{n^{'}}

g^{n} = g^{n^{'}} + δ g^{n^{'}}

{\tilde{f}}^{b} = f^{b} + \nabla^{b}

C_{b}^{n} = C_{n^{'}}^{n} C_{b}^{n^{'}} = C_{b}^{n^{'}} + δ C_{b}^{n^{'}}

Equation (13) minus Equation (14):

\begin{array}{l} δ {\dot{v}}^{n^{'}} = (C_{n^{'}}^{n} - I) C_{b}^{n^{'}} {\tilde{f}}^{b} - C_{n^{'}}^{n} C_{b}^{n^{'}} \nabla^{b} - (2 ω_{i e}^{n^{'}} + ω_{e n^{'}}^{n^{'}}) \times δ v^{n^{'}} \\ - (2 δ ω_{i e}^{n^{'}} + δ ω_{e n^{'}}^{n^{'}}) \times v^{n^{'}} - (2 δ ω_{i e}^{n^{'}} + δ ω_{e n^{'}}^{n^{'}}) \times δ v^{n^{'}} + δ g^{n^{'}} \end{array}

(15)

where,

C_{n^{'}}^{n}

is defined by Equation (1). The above Equation (15) is the speed error equation of SINS with large misalignment angle.

The position error equation of SINS defined as:

\begin{array}{l} δ \dot{L} = \hat{\dot{L}} - \dot{L} = \frac{1}{R_{M} + h} δ v_{N}^{n} \\ δ \dot{λ} = \hat{\dot{λ}} - \dot{λ} = \frac{\sec L}{R_{N} + h} δ v_{E}^{n} + \frac{v_{E}^{n} \sec L \tan L}{R_{N} + h} δ L \end{array}

(16)

where,

L

λ

and

h

denote the longitude, latitude and altitude respectively.

R_{M}

and

R_{N}

denote the radius of curvature in meridian and in prime vertical, respectively.

2.2. Odometer/Gyroscopes Dead Reckoning Error Equation

The dead reckoning (DR) is a commonly used carrier independent positioning technique that uses posture, heading, and mileage information to calculate the vehicle’s position relative to the starting point. In this paper, the angular rate measured by gyroscopes of the IMU and the velocity measured by odometer are used to realize the dead reckoning. The dead reckoning algorithm includes the position updating algorithm and the attitude updating algorithm.

Define the b-frame with respect to the n-frame is

C_{b D}^{n}

, which is calculated by the gyroscopes of IMU, and the subscript D means the variable output by the dead reckoning algorithm.

The velocity measured by odometer expressed in n-frame is:

v_{D}^{n} = C_{b}^{n} v_{D}^{b}

(17)

The position differential equations are similar to the SINS position update equations and can be expressed as:

{\begin{cases} {\dot{L}}_{D} = \frac{v_{D N}^{n}}{R_{M} + h_{D}} \\ {\dot{λ}}_{D} = \frac{v_{D E}^{n} \sec L_{D}}{R_{N} + h_{D}} \end{cases}

(18)

where,

v_{D}^{n} = {[\begin{matrix} v_{D E}^{n} & v_{D N}^{n} & v_{D U}^{n} \end{matrix}]}^{T}

p_{D} = {[\begin{matrix} L_{D} & λ_{D} & h_{D} \end{matrix}]}^{T}

Then, considering the velocity error, the DR horizontal position error can be expressed as:

{\begin{cases} δ {\dot{L}}_{D} = {\hat{\dot{L}}}_{D} - {\dot{L}}_{D} = \frac{1}{R_{M} + h_{D}} δ v_{D N}^{n} \\ δ {\dot{λ}}_{D} = {\hat{\dot{λ}}}_{D} - {\dot{λ}}_{D} = \frac{\sec L_{D}}{R_{N} + h_{D}} δ v_{D E}^{n} + \frac{v_{D E}^{n} \sec L_{D} \tan L_{D}}{R_{N} + h_{D}} δ L_{D} \end{cases}

(19)

We denote the IMU installation error angle as

θ = {[\begin{matrix} θ_{x} & θ_{y} & θ_{z} \end{matrix}]}^{T}

, that is the misalignment angle between b-frame and m-frame. Assuming the IMU installation error angle is small and taking the odometer scale factor error

δ K_{D}

and attitude error into account, the speed measured by odometer

v_{D}

expressed in n-frame is:

{\hat{v}}_{D}^{n} = C_{n}^{n^{'}} C_{b}^{n} [\begin{matrix} - θ_{z} \\ 1 + δ K_{D} \\ θ_{x} \end{matrix}] v_{D}

(20)

The velocity error is defined as the calculated value minus the true value, as:

δ v_{D}^{n} = {\hat{v}}_{D}^{n} - v_{D}^{n}

(21)

Only taking the horizontal velocity into account, the component form of velocity error is:

{\begin{cases} δ v_{D E}^{n} = [θ_{x} (T_{13} \cos α_{z} + T_{23} \sin α_{z} - T_{33} α_{y}) - θ_{z} (T_{11} \cos α_{z} + T_{21} \sin α_{z} - T_{31} α_{y}) \\ + (1 + δ K_{D}) (T_{12} \cos α_{z} + T_{22} \sin α_{z} - T_{32} α_{y}) - T_{12}] v_{D} \\ δ v_{D N}^{n} = [θ_{x} (- T_{13} \sin α_{z} + T_{23} \cos α_{z} + T_{33} α_{x}) - θ_{z} (- T_{11} \sin α_{z} + T_{21} \cos α_{z} + T_{31} α_{x}) \\ + (1 + δ K_{D}) (- T_{12} \sin α_{z} + T_{22} \cos α_{z} + T_{32} α_{x}) - T_{22}] v_{D} \end{cases}

(22)

where,

δ v_{D E}^{n}

and

δ v_{D N}^{n}

are the east and north velocity error,

T_{i j}

is the element of

C_{b}^{n}

By substituting Equation (22) into Equation (19), the calculated position error equation can be obtained:

{\begin{cases} δ {\dot{L}}_{D} = \frac{1}{R_{M} + h_{D}} [θ_{x} (- T_{13} \sin α_{z} + T_{23} \cos α_{z} + T_{33} α_{x}) - θ_{z} (- T_{11} \sin α_{z} + T_{21} \cos α_{z} + T_{31} α_{x}) \\ + (1 + δ K_{D}) (- T_{12} \sin α_{z} + T_{22} \cos α_{z} + T_{32} α_{x}) - T_{22}] v_{D} \\ δ {\dot{λ}}_{D} = \frac{\sec L_{D}}{R_{N} + h_{D}} [θ_{x} (T_{13} \cos α_{z} + T_{23} \sin α_{z} - T_{33} α_{y}) - θ_{z} (T_{11} \cos α_{z} + T_{21} \sin α_{z} - T_{31} α_{y}) \\ + (1 + δ K_{D}) (T_{12} \cos α_{z} + T_{22} \sin α_{z} - T_{32} α_{y}) - T_{12}] v_{D} + \frac{v_{D E}^{n} \sec L_{D} \tan L_{D}}{R_{N} + h_{D}} δ L_{D} \end{cases}

(23)

2.3. Kalman Filter Equation

Taking attitude errors, horizontal velocity errors, SINS positioning errors, DR positioning errors, constant gyro drifts, accelerometer biases, misalignment angles and odometer factor error into account, the state error equation can be expressed as:

\dot{x} = f (x) + w

(24)

where, the state vector is:

\begin{array}{l} x = [[\begin{matrix} α_{x} & α_{y} & α_{z} \end{matrix}] [\begin{matrix} δ v_{E}^{n} & δ v_{N}^{n} \end{matrix}] [\begin{matrix} δ L & δ λ \end{matrix}] [\begin{matrix} δ L_{D} & δ λ_{D} \end{matrix}] \\ {[\begin{matrix} ε_{x} & ε_{y} & ε_{z} \end{matrix}] [\begin{matrix} \nabla_{x} & \nabla_{y} & \nabla_{z} \end{matrix}] [\begin{matrix} θ_{x} & θ_{z} & δ K_{D} \end{matrix}]]}^{T} \end{array}

(25)

The position difference between SINS and DR is used as the observation:

z = [\begin{matrix} L - L_{D} \\ λ - λ_{D} \end{matrix}] = [\begin{matrix} δ L - δ L_{D} \\ δ λ - δ λ_{D} \end{matrix}]

(26)

And the observing matrix H can be indicated as:

H = [\begin{matrix} 0_{2 \times 3} & 0_{2 \times 2} & I_{2 \times 2} & - I_{2 \times 2} & 0_{2 \times 3} & 0_{2 \times 3} & 0_{2 \times 3} \end{matrix}]

(27)

The advantage of deriving the DR error equation is that the linear measurement equation can be obtained when the position error is taken as the observation of Kalman filter as shown in Equation (27). Owing to this, the calculation process of the initial alignment can be sharply reduced. If the odometer output expression in b-frame

{\hat{v}}_{D}^{b}

is taken as the observation directly, the measurement equation will be very complicated with strong nonlinear.

3. Improved Adaptive CKF/KF Method

3.1. Cubature Kalman Filter

Cubature Kalman Filter (CKF) is a nonlinear filtering algorithm proposed by Ienkaran Arasaratnam and Simon Haykin to solve the problem of multi-dimensional state estimation. The core of CKF is the spherical-radial cubature rule, which is used to solve the multi-dimensional integral problem in nonlinear Bayesian filtering [24].

The state space form of discrete nonlinear system is described as:

{\begin{cases} x_{k} = f (x_{k - 1}) + w_{k - 1} \\ z_{k} = h (x_{k}) + v_{k} \end{cases}

(28)

where,

x_{k}

and

z_{k}

are the state vector and the measurement vector respectively,

f (x_{k - 1})

is nonlinear state function,

h (x_{k})

is nonlinear measurement vector function,

w_{k} \sim N (0, Q_{k})

is the random system noise,

v_{k} \sim N (0, R_{k})

is the random measurement noise.

According to the extended three-dimension spherical-radial rule, the calculation of a standard Gaussian weighted integral is:

I_{N} (g) = \int_{R} g (x) N (x; 0, I) d x \approx \sum_{i = 1}^{m} ω_{i} g (ξ_{i})

(29)

where,

g (\cdot)

is arbitrary nonlinear function, and the cubature points are defined as follows:

{\begin{cases} ξ_{i} = \sqrt{\frac{m}{2}} {[1]}_{i} \\ ω_{i} = \frac{1}{m}, i = 1, \dots m, m = 2 n \end{cases}

(30)

The CKF filtering algorithm using the cubature points

[ξ_{i}, ω_{i}]

is as follows

▪ Time update:
(1)
Assume that the posterior density function $p (x_{k - 1}) = N ({\hat{x}}_{k - 1 | k - 1}, P_{k - 1 | k - 1})$ is known, the Cholesky Decomposition of error covariance $P_{k - 1 | k - 1}$ is:

$P_{k - 1 | k - 1} = S_{k - 1 | k - 1} S_{k - 1 | k - 1}^{T}$

(31)

(2)
Calculate the cubature points:

$X_{i, k - 1 | k - 1} = S_{k - 1 | k - 1} ξ_{i} + {\hat{x}}_{k - 1 | k - 1}$

(32)

(3)
Propagate cubature points through the state equation:

$X_{i, k | k - 1}^{*} = f (X_{i, k - 1 | k - 1})$

(33)

(4)
Estimate state predictions:

${\hat{x}}_{k | k - 1} = \frac{1}{m} \sum_{i = 1}^{m} X_{i, k | k - 1}^{*}$

(34)

(5)
Estimate the state error covariance predictor:

$P_{k | k - 1} = \frac{1}{m} \sum_{i = 1}^{m} X_{i, k | k - 1}^{*} X_{i, k | k - 1}^{* T} - {\hat{x}}_{k | k - 1} {\hat{x}}_{k | k - 1}^{T} + Q_{k - 1}$

(35)
▪ Measurement update:
(1)
Cholesky decomposition of $P_{k | k - 1}$ :

$P_{k | k - 1} = S_{k | k - 1} S_{k | k - 1}^{T}$

(36)

(2)
Calculate cubature points:

$X_{i, k | k - 1} = S_{k | k - 1} ξ_{i} + {\hat{x}}_{k | k - 1}$

(37)

(3)
Propagate cubature points by the measurement equation:

$Z_{i, k | k - 1} = h (X_{i, k | k - 1})$

(38)

(4)
Measurement prediction:

${\hat{z}}_{k | k - 1} = \frac{1}{m} \sum_{i = 1}^{m} Z_{i, k | k - 1}$

(39)

(5)
Estimate the self-correlation covariance matrix:

$P_{z z, k | k - 1} = \frac{1}{m} \sum_{i = 1}^{m} Z_{i, k | k - 1} Z_{i, k | k - 1}^{T} - {\hat{z}}_{k | k - 1} {\hat{z}}_{k | k - 1}^{T} + R_{k}$

(40)

(6)
Estimate the mutual correlation covariance matrix:

$P_{x z, k | k - 1} = \sum_{i = 1}^{m} ω_{i} X_{i, k | k - 1} Z_{i, k | k - 1}^{T} - {\hat{x}}_{k | k - 1} {\hat{z}}_{k | k - 1}^{T}$

(41)

(7)
Estimate the gain matrix:

$K_{k} = P_{x z, k | k - 1} P_{z z, k | k - 1}^{- 1}$

(42)

(8)
Calculate the state estimation:

${\hat{x}}_{k | k} = {\hat{x}}_{k | k - 1} + K_{k} (z_{k} - {\hat{z}}_{k | k - 1})$

(43)

(9)
Calculate the state error covariance estimation:

$P_{k | k} = P_{k | k - 1} - K_{k} P_{z z, k | k - 1} K_{k}^{T}$

(44)

As can be seen, CKF calculates a set of points with an even number of equal weights according to the spherical-radial cubature rule, which captures the mean and variance of the Gaussian distribution variables completely, and after the transformation of the nonlinear system equation, the precision can reach third-order or higher. So it is not necessary to linearize the nonlinear model, and it is independent of the non-linear equations of the practical system model.

3.2. Sage-Husa Adaptive Filter

In theory, the optimal estimation of the state can be obtained only if the structural parameters and the statistical parameters of the stochastic dynamic system are accurately known. However, in practical application, the above two kinds of parameters are more or less inaccurate, resulting in the accuracy of Kalman filter is reduced, the serious can also cause filtering divergence. Sage and Husa proposed an adaptive filtering algorithm which can estimate the noise parameters of the system in real time by measuring the output, but it is often impossible to estimate all the noise parameters (System noise mean and variance, measurement noise mean and variance). Therefore, only the most commonly used and more effective adaptive algorithm for measurement noise variance matrix is given.

The system is assumed to be linear and described as:

{\begin{cases} x_{k} = Φ_{k / k - 1} x_{k - 1} + Γ_{k - 1} w_{k - 1} \\ z_{k} = H_{k} x_{k} + v_{k} \end{cases}

(45)

There are several conditions that need to be fulfilled by

w_{k}

and

v_{k}

{\begin{cases} E [w_{k}] = 0, & E [w_{k} w_{j}^{T}] = Q_{k} δ_{k j} \\ E [v_{k}] = 0, & E [v_{k} v_{j}^{T}] = R_{k} δ_{k j} \\ E [w_{k} v_{j}^{T}] = 0 \end{cases}

(46)

where, the variance matrix of the measurement noise

R_{k}

is assumed to be unknown and the adaptive estimation method of

R_{k}

is shown as follows.

According to the steps of the Kalman filter, the estimated errors which are also called innovations can be calculated:

\begin{array}{l} ε_{k} = z_{k} - {\hat{z}}_{k | k - 1} \\ = H_{k} x_{k} + v_{k} - H_{k} {\hat{x}}_{k | k - 1} \\ = H_{k} {\tilde{x}}_{k | k - 1} + v_{k} \end{array}

(47)

In consideration of the one step estimated error of state

{\tilde{x}}_{k / k - 1}

and measurement noise

v_{k}

are both zero mean value, we can demonstrate that the mean value of innovation is zero. Furthermore,

{\tilde{x}}_{k / k - 1}

and

v_{k}

are uncorrelated. The following equation can be developed by taking the variances on both sides of Equation (47):

E [ε_{k} ε_{k}^{T}] = H_{k} P_{k / k - 1} H_{k}^{T} + R_{k}

(48)

Rewriting Equation (48), the variance matrix of the measurement noise

R_{k}

can be calculated as follows:

R_{k} = E [ε_{k} ε_{k}^{T}] - H_{k} P_{k / k - 1} H_{k}^{T}

(49)

where,

E [ε_{k} ε_{k}^{T}]

stands for the lumped mean value of random sequence in theory. Nevertheless, it ought to be replaced by time averaged value in adaptive filter method.

R_{k}

can be established by equal linear weighting recursive estimation method as follows:

\begin{array}{l} {\hat{R}}_{k} = \frac{1}{k} \sum_{i = 1}^{k} (ε_{i} ε_{i}^{T} - H_{i} P_{i | i - 1} H_{i}^{T}) \\ = \frac{1}{k} [\sum_{i = 1}^{k - 1} (ε_{i} ε_{i}^{T} - H_{i} P_{i | i - 1} H_{i}^{T}) + (ε_{k} ε_{k}^{T} - H_{k} P_{k | k - 1} H_{k}^{T})] \\ = \frac{1}{k} [(k - 1) {\hat{R}}_{k - 1} + (ε_{k} ε_{k}^{T} - H_{k} P_{k | k - 1} H_{k}^{T})] \\ = (1 - \frac{1}{k}) {\hat{R}}_{k - 1} + \frac{1}{k} (ε_{k} ε_{k}^{T} - H_{k} P_{k | k - 1} H_{k}^{T}) \end{array}

(50)

Besides the equal linear weighting method, in order to decrease the impact of obsolete measurement noise, Equation (50) can be rewritten with fading memory exponent weighted average method as follows:

{\hat{R}}_{k} = (1 - β_{k}) {\hat{R}}_{k - 1} + β_{k} (ε_{k} ε_{k}^{T} - H_{k} P_{k | k - 1} H_{k}^{T})

(51)

β_{k} = \frac{β_{k - 1}}{β_{k - 1} + b}

(52)

where initial value

β_{0} = 1

and

0 < b < 1

denotes the fading factor. When k is enough large,

β_{k} \approx 1 - b

can be obtained within the approximation. The smaller the fading factor b is, the less the influence of obsolete noise. Normally,

b = 0.9 ~ 0.999

As the estimation of

R_{k}

in adaptive filter has an effect on the gain calculation, the filter calculation loop of the adaptive filter is no longer simple and linear as the standard Kalman filter, leading the adaptive filter to be an essentially unusually complex nonlinear system. In theory, it is very difficult to analyze the observability and stability of adaptive filter, so the number of adaptive parameters should be minimized in practical use, which is helpful to ensure the effectiveness of filter.

3.3. ACKF/KF Method

The Sage-Husa adaptive Kalman filter has the same structure as the standard Kalman filter, but the measurement noise covariance is estimated online. The structure of CKF also maintains the standard Kalman filter structure, only adopts the cubature rule to update the calculation of error covariance for the nonlinear system. Both of the Sage-Husa adaptive Kalman filter and CKF are extensions of the standard Kalman filter, which makes it possible to apply Sage-Husa adaptive strategy to CKF, namely the adaptive cubature Kalman filtering (ACKF).

When the difference between the position of DR and SINS is used as the observation, the linear measurement equation can be obtained. Therefore, the ACKF algorithm can be simplified, that is, the time update of the state and the covariance of the state estimation is carried out by the ACKF method for the state equation. For the observation equation, the standard KF is used for measurement update, that is, the ACKF/KF algorithm. This can greatly reduce the calculation of the initial alignment and ensure the accuracy.

In the case of nonlinear state equation, linear measurement equation with unknown measurement noise, ACKF/KF algorithm is as follows:

▪ The system description:

${\begin{cases} x_{k} = f (x_{k - 1}) + w_{k - 1} \\ z_{k} = H_{k} x_{k} + v_{k} \end{cases}$

(53)

${\begin{cases} E [w_{k}] = 0, & E [w_{k} w_{j}^{T}] = Q_{k} δ_{k j} \\ E [v_{k}] = 0, & E [v_{k} v_{j}^{T}] = R_{k} δ_{k j} \\ E [w_{k} v_{j}^{T}] = 0 \end{cases}$

(54)

where, $R_{k}$ is unknown.
▪ Time update:
(1)
Assume that the posterior density function $p (x_{k - 1}) = N ({\hat{x}}_{k - 1 | k - 1}, P_{k - 1 | k - 1})$ is known, the Cholesky Decomposition of error covariance $P_{k - 1 | k - 1}$ is:

$P_{k - 1 | k - 1} = S_{k - 1 | k - 1} S_{k - 1 | k - 1}^{T}$

(55)

(2)
Calculate the cubature points:

$X_{i, k - 1 | k - 1} = S_{k - 1 | k - 1} ξ_{i} + {\hat{x}}_{k - 1 | k - 1}$

(56)

(3)
Propagate cubature points through the state equation:

$X_{i, k | k - 1}^{*} = f (X_{i, k - 1 | k - 1})$

(57)

(4)
Estimate state predictions:

${\hat{x}}_{k | k - 1} = \frac{1}{m} \sum_{i = 1}^{m} X_{i, k | k - 1}^{*}$

(58)

(5)
Estimate the state error covariance predictor:

$P_{k | k - 1} = \frac{1}{m} \sum_{i = 1}^{m} X_{i, k | k - 1}^{*} X_{i, k | k - 1}^{* T} - {\hat{x}}_{k | k - 1} {\hat{x}}_{k | k - 1}^{T} + Q_{k - 1}$

(59)
▪ Measurement update
(1)
Predict the measurement:

${\hat{z}}_{k | k - 1} = H_{k} {\hat{x}}_{k | k - 1}$

(60)

(2)
Calculate the innovation:

$ε_{k} = z_{k} - {\hat{z}}_{k | k - 1}$

(61)

(3)
Estimate the measurement noise:

${\hat{R}}_{k} = (1 - β_{k}) {\hat{R}}_{k - 1} + β_{k} (ε_{k} ε_{k}^{T} - H_{k} P_{k | k - 1} H_{k}^{T})$

(62)

(4)
Estimate the self-correlation covariance matrix:

$P_{z z, k | k - 1} = H_{k} P_{k | k - 1} H_{k}^{T} + {\hat{R}}_{k}$

(63)

(5)
Estimate the mutual correlation covariance matrix:

$P_{x z, k | k - 1} = P_{k | k - 1} H_{k}^{T}$

(64)

(6)
Estimate the gain matrix:

$K_{k} = P_{x z, k | k - 1} P_{z z, k | k - 1}^{- 1}$

(65)

(7)
Calculate the state estimation:

${\hat{x}}_{k | k} = {\hat{x}}_{k | k - 1} + K_{k} (z_{k} - {\hat{z}}_{k | k - 1})$

(66)

(8)
Calculate the state error covariance estimation:

$P_{k | k} = P_{k | k - 1} - K_{k} P_{z z, k | k - 1} K_{k}^{T}$

(67)

According to the structural features of Sage-Husa adaptive filtering and cubature Kalman filtering and the characteristics of system equations, the adaptive filtering and cubature Kalman filtering are effectively combined and simplified, and an improved ACKF/KF algorithm is proposed. The introduction of adaptive adjustment strategy in CKF can not only guarantee the filtering accuracy of the nonlinear system, but also make the filtering immune to the change of measurement noise. The combination of the advantages of the two technologies further improves the accuracy and stability of the filtering algorithm, which has important engineering application value.

4. Simulation and Experiment

To verify the performance of the proposed ACKF/KF algorithm, simulations and experiments are performed in this section.

4.1. Simulation and Analysis

The design of the vehicle motion trajectory as shown in Figure 1, the vehicle maneuvering mode including constant speed, acceleration, yaw and pitch, the simulation time is 900 s, distance is 5.3 km. The initial value of the attitude angle is [0° 0° −117°], the position initial value is longitude 126.6°, Latitude 45.7°. The standard deviation of position random white noise at the beginning is 10 m of positional error, and the standard deviation of position random white noise increases to 30 m during 400 s to 500 s, simulating a section of road with poor traffic.

The SINS is comprised of a triad of orthogonal gyroscopes with drift of 0.05°/h and noise of 0.01°/(h/

\sqrt{Hz}

), and a triad of orthogonal accelerometers with bias of 500 µg and noise of 100 µg/

\sqrt{Hz}

, and the IMU sampling rate is 100 Hz. The odometer scale factor error is 0.002 and the noise of velocity is 0.02 m/s.

Extended Kalman Filter (EKF), Adaptive Extended Kalman filter (AEKF), Cubature Kalman filter (CKF) as the comparisons of the ACKF/KF are set up. The attitude misalignment angle of the simulation is [1° 1° 15°] and the filter parameters are set as follows:

Filter initial value:

x_{0} = 0_{18 \times 1} .

Initial estimate error covariance:

\begin{array}{l} P_{0} = diag [[\begin{matrix} 5^{\circ} & 5^{\circ} & 30^{\circ} \end{matrix}] [\begin{matrix} 1 m / s & 1 m / s \end{matrix}] [\begin{matrix} 50 m & 50 m \end{matrix}] [\begin{matrix} 50 m & 50 m \end{matrix}] \\ {[\begin{matrix} ε_{x} & ε_{y} & ε_{z} \end{matrix}] [\begin{matrix} \nabla_{x} & \nabla_{y} & \nabla_{z} \end{matrix}] [\begin{matrix} 5^{\circ} & 5^{\circ} & 0.01 \end{matrix}]]}^{2} \end{array}

Process noise Covariance:

Q = diag {[\begin{matrix} ε & ε & ε & \nabla & \nabla & 10 m & 10 m & 10 m & 10 m & 0_{9 \times 1} \end{matrix}]}^{2}

Initial measurement noise covariance:

{\hat{R}}_{0} = diag {[\begin{matrix} 10 m & 10 m \end{matrix}]}^{2} .

(68)

The initial alignment simulation was performed by EKF, AEKF, CKF and ACKF/KF respectively under the above simulation conditions, and the alignment results are shown in Figure 2, Figure 3,Figure 4 and Figure 5. Here,

ϕ_{x}

ϕ_{y}

and

ϕ_{z}

mean the pitch, roll, and yaw respectively; the horizontal axis of the graph is time (s); the ordinate is the estimation error of attitude angle (′).

Figure 6, Figure 7 and Figure 8 show the comparison of the four algorithms, in which the pink line denotes the estimation by EKF, the green line denotes the estimation by CKF, the red line denotes the estimation by AEKF, and the blue line denotes the estimation by ACKF/KF. It is obviously that the influence of measurement noise disturbance on the four algorithms is different. As the EKF and CKF algorithms still use the initial value of the measurement noise covariance during the increase of noise because of the lack of estimation of measurement noise, the filtering results are affected and the filtering accuracy is seriously reduced. Moreover, the estimation did not return to the original accuracy after the noise interference. As a result, the EKF and CKF cannot meet the accuracy requirements due to measurement noise interference during the alignment process.

Due to the Sage-Husa adaptive filter, the filter accuracy and stability of ACKF/KF and AEKF are much better than those of EKF and CKF. However, the system of odometer aided SINS initial alignment with large misalignment angle is seriously nonlinear as shown in Section 2, the linearization truncation error in EKF somehow limits its estimation accuracy. The accuracy of the horizontal misalignment angle estimation of ACKF/KF and AEKF is approximately the same, but the accuracy of the ACKF/KF yaw misalignment angle is much higher. The mean and standard deviation of the estimation attitude error are listed in Table 1.

For the complex nonlinear model of the odometer aided SINS initial alignment with large misalignment angle, the EKF only uses the first order Jacobian matrix of the model to linearize, with large computational error. While the cubature points obtained in the CKF method according to the spherical-radial cubature rule can completely capture the mean and variance of Gaussian distribution variables, and the mean and variance can be accurate to the third-order or higher-order term of Taylor series expansion of nonlinear system. The Sage-Husa adaptive algorithm can estimate the noise in real time, thus improving the stability of the filtering results and making the convergence process more stable. The simulation results show that the proposed ACKF/KF algorithm can improve the accuracy and stability of the initial alignment.

4.2. Experiments and Analysis

In order to verify actual performance of the proposed ACKF/KF algorithm, the initial alignment test was conducted by a vehicle equipped with SINS, odometer and GPS. The SINS is comprised of a triad of gyroscopes (drift 0.02°/h, noise 0.01°/(h/

\sqrt{Hz}

) and accelerometers (bias 500 µg, noise 50 µg/

\sqrt{Hz}

) at a sampling rate 100 Hz. The odometer scale factor error is about 0.2%, and the standard variance of measure noise is 0.02 m/s. The GPS is chosen as position reference, with the position accuracy less than 10 m and the velocity accuracy 0.01 m/s. And the attitude reference is given by SINS/GPS integrated navigation. The vehicle test trajectory is shown in Figure 9.

The initial alignment error of pitch, roll and yaw are shown in Figure 10, Figure 11 and Figure 12 respectively, in which the pink, green, red and blue lines denote the estimation by EKF, CKF, AEKF and ACKF/KF respectively. It is obvious that the estimation process of ACKF/KF is much more smoothly than EKF and CKF, and the estimation precision is higher than AEKF. And the difference in Figure 12 is even more pronounced, in which the final estimated results of yaw is 10.7311′ of EKF, 8.2875′ of CKF, 3.7184′ of AEKF and 1.2627′ of ACKF/KF. The initial alignment results of attitude error in details are shown in Table 2. The initial alignment results are consistent with theoretical analysis and simulation results. Therefore, the proposed ACKF/KF method can accomplish the initial alignment with large misalignment angle well without a priori knowledge about the measurement noise, and the alignment speed and precision are significantly improved compared with EKF algorithm.

5. Conclusions

This paper has proposed an improved initial alignment method for land-vehicle SINS in-motion with large misalignment angle. There are two main points in this paper, one of them is the establishment of the linear observation equation of Kalman filter by deducing the odometer/ gyroscope dead reckoning error equation with large misalignment error, which sharply reduces the calculation of the initial alignment. The other is the improved ACKF/KF method, which combines the advantages of CKF and Sage-Husa adaptive filter. The simulation results show that the accuracy, stability and robustness is much better than the EKF, CKF and AEKF when the measurement noise changed during the initial alignment. Experiments show that the ACKF/KF method can accomplish the initial alignment with large misalignment angle in-motion.

Author Contributions

K.G. and S.R. conceived and designed this study. G.Y. and J.Z. performed the experiments. K.G. and Z.W. wrote the paper. S.R. reviewed and edited the manuscript. All authors read and approved this manuscript.

Funding

The above research is supported in part by the National Natural Science Foundation of China (61703123) and in part by the 13th Five-year Equipment Pre-research Foundation (4141708031).

Acknowledgments

The authors would like to thank all the editors and anonymous reviewers for improving this article.

Conflicts of Interest

The authors declare no conflict of interest.

References

Chang, L.; Li, J.; Chen, S. Initial alignment by attitude estimation for strapdown inertial navigation systems. IEEE Trans. Instrum. Meas. 2015, 64, 784–794. [Google Scholar] [CrossRef]
Huang, W.; Fang, T.; Luo, L.; Zhao, L.; Che, F. A damping grid strapdown inertial navigation system based on a Kalman filter for ships in polar regions. Sensors 2017, 17, 1551. [Google Scholar] [CrossRef] [PubMed]
Song, J.W.; Park, C.G. Enhanced pedestrian navigation based on course angle error estimation using cascaded Kalman filters. Sensors 2018, 4, 1281. [Google Scholar] [CrossRef] [PubMed]
Wang, W.; Chen, X. Application of improved 5th-cubature Kalman filter in initial strapdown inertial navigation system alignment for large misalignment angles. Sensors 2018, 18, 659. [Google Scholar] [CrossRef] [PubMed]
Chang, L.B.; Hu, B.Q.; Li, A.; Qin, F.J. Strapdown inertial navigation system alignment based on marginalized unscented Kalman filter. IET Sci. Meas. Technol. 2013, 7, 128–138. [Google Scholar] [CrossRef]
Zhang, Y.; Yu, F.; Gao, W.; Wang, Y. An improved strapdown inertial navigation system initial alignment algorithm for unmanned vehicles. Sensors 2018, 10, 3297. [Google Scholar] [CrossRef] [PubMed]
Zhang, L.; Yang, C.; Chen, Q.W.; Yan, F. Robust H-infinity CKF/KF hybrid filtering method for SINS alignment. IET Sci. Meas. Technol. 2016, 10, 916–925. [Google Scholar] [CrossRef]
Yan, G.M.; Qin, Y.Y. Novel approach to in-flight alignment of micro-mechanical SINS/GPS with heading uncertainty. Chin. J. Sens. Act. 2007, 20, 238–242. [Google Scholar] [CrossRef]
Pan, X.; Wu, Y. Underwater Doppler navigation with self-calibration. J. Navig. 2015, 69, 295–312. [Google Scholar] [CrossRef]
Tang, Y.; Wu, Y.; Wu, M.; Wu, W.; Hu, X.; Shen, L. INS/GPS integration: Global observability analysis. IEEE Trans. Veh. Technol. 2009, 58, 1129–1142. [Google Scholar] [CrossRef]
Ali, J.; Ushaq, M. A consistent and robust Kalman filter design for in-motion alignment of inertial navigation system. Measurement 2009, 42, 577–582. [Google Scholar] [CrossRef]
Skog, I.; Händel, P. In-car positioning and navigation technologies: A survey. IEEE Trans. Intell. Transp. Syst. 2009, 10, 4–21. [Google Scholar] [CrossRef]
Petritoli, E.; Leccese, F. Improvement of altitude precision in indoor and urban canyon navigation for small flying vehicles. In Proceedings of the 2015 IEEE Metrology for Aerospace (MetroAeroSpace), Benevento, Italy, 4–5 June 2015; pp. 56–60. [Google Scholar] [CrossRef]
Ci, W.Y.; Huang, Y.P.; Hu, X. Review of visual odometry algorithms. Appl. Res. Comput. 2018, 36. [Google Scholar] [CrossRef]
Matsuki, H.; Stumberg, L.; Usenko, V.; Stückler, J.; Cremers, D. Omnidirectional DSO: Direct sparse odometry with fisheye cameras. IEEE Robot. Autom. Lett. 2018, 3, 3693–3700. [Google Scholar] [CrossRef]
Gao, W.; Zhang, Y.; Wang, J.G. Research on initial alignment and self-calibration of rotary strapdown inertial navigation systems. Sensors 2015, 15, 3154–3171. [Google Scholar] [CrossRef] [PubMed]
Gao, K.; Ren, S.Q.; Chen, X.J.; Wang, Z.H. An optimization-based initial alignment and calibration algorithm of land-vehicle SINS in-motion. Sensors 2018, 18. [Google Scholar] [CrossRef] [PubMed]
Georgy, J.; Karamat, T.; Iqbal, U.; Noureldin, A. Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter. GPS Solut. 2011, 15, 239–252. [Google Scholar] [CrossRef]
Wang, M.; Wu, W.; Zhou, P.Y.; He, X.F. State transformation extended Kalman filter for GPS/SINS tightly coupled integration. GPS Solut. 2018, 22, 112. [Google Scholar] [CrossRef]
Atia, M.M.; Liu, S.; Nematallah, H.; Karamat, T.B.; Noureldin, A. Integrated indoor navigation system for ground vehicles with automatic 3-D alignment and position initialization. IEEE Trans. Veh. Technol. 2015, 64, 1279–1292. [Google Scholar] [CrossRef]
Julier, S.J.; Uhlman, J.K.; Durrant-Whyte, H.F. A new approach for filtering nonlinear systems. In Proceedings of the IEEE American Control Conference, Seattle, WA, USA, 21–23 June 1995; pp. 1628–1632. [Google Scholar] [CrossRef]
Gao, Y.B.; Liu, M.; Li, G.C.; Guang, X.X. Initial alignment for SINS based on pseudo-earth frame in polar regions. Sensors 2017, 17, 1416. [Google Scholar] [CrossRef] [PubMed]
Wan, L.L.; Wang, J.L.; Lu, L.Q.; Wu, W.Q. A novel scheme for DVL-aided SINS in-motion alignment using UKF techniques. Sensors 2013, 13, 1046–1063. [Google Scholar] [CrossRef]
Arasaratnam, I.; Haykin, S. Cubature Kalman filters. IEEE Trans. Autom. Control 2009, 54, 1254–1269. [Google Scholar] [CrossRef]
Jia, B.; Xin, M.; Cheng, Y. High-degree cubature Kalman filter. Automatica 2013, 49, 510–518. [Google Scholar] [CrossRef]
Gao, W.; Zhang, Y.; Wang, J.G. A strapdown interial navigation system/Beidou/Doppler velocity log integrated navigation algorithm based on a cubature Kalman filter. Sensors 2014, 14, 1511–1527. [Google Scholar] [CrossRef] [PubMed]
Shi, Y.; Han, C.Z. Adaptive UKF method with applications to target tracking. Acta Autom. Sin. 2011, 37, 755–759. [Google Scholar] [CrossRef]
Wu, Y.; Wu, M.; Hu, X.; Hu, D. Self-calibration for land navigation using inertial sensors and odometer: Observability analysis. In Proceedings of the AIAA Guidance, Navigation, and Control Conference, Chicago, IL, USA, 10–13 August 2009. [Google Scholar] [CrossRef]
Wang, J.G.; Gopaul, N.; Scherzinger, B. Simplified algorithms of variance component estimation for static and kinematic GPS single point positioning. J. Glob. Position. Syst. 2009, 8, 43–52. [Google Scholar] [CrossRef]
Wang, J.G.; Gopaul, S.N.; Guo, J.M. Adaptive Kalman filtering based on posteriori variance-covariance components estimation. In Proceedings of the CPGPS 2010 Technical Forum, Shanghai, China, 17–20 August 2010. [Google Scholar] [CrossRef]
Dong, Q.H.; Li, Y.B.; Sun, Q.; Zhang, Y. An adaptive initial alignment algorithm based on variance component estimation for a strapdown inertial navigation system for AUV. Symmetry 2017, 9, 129. [Google Scholar] [CrossRef]
Narasimhappa, M.; Rangababu, P.; Sabat, S.L.; Nayak, J. A modified Sage-Husa adaptive Kalman filter for denoising Fiber Optic Gyroscope signal. In Proceedings of the 2012 Annual IEEE India Conference (INDICON), Kochi, India, 7–9 December 2012; pp. 1266–1271. [Google Scholar] [CrossRef]
Jin, M.; Zhao, J.; Jin, J.; Yu, G.; Li, W. The adaptive Kalman filter based on fuzzy logic for inertial motion capture system. Measurement 2014, 49, 196–204. [Google Scholar] [CrossRef]

Figure 1. The vehicle’s simulation trajectory.

Figure 2. The initial alignment simulation results of EKF method. The three figures are the attitude error of x axis (pitch), y axis (roll) and z axis (yaw) respectively.

Figure 3. The initial alignment simulation results of AEKF method. The three figures are the attitude error of x axis (pitch), y axis (roll) and z axis (yaw) respectively.

Figure 4. The initial alignment simulation results of CKF method. The three figures are the attitude error of x axis (pitch), y axis (roll) and z axis (yaw) respectively.

Figure 5. The initial alignment simulation results of ACKF/KF method. The three figures are the attitude error of x axis (pitch), y axis (roll) and z axis (yaw) respectively.

Figure 6. The comparison of estimation error of pitch angle. The pink line denotes the estimation by EKF, the green line denotes the estimation by CKF, the red line denotes the estimation by AEKF, and the blue line denotes the estimation by ACKF/KF.

Figure 7. Comparison of estimation error of roll angle. The pink line denotes the estimation by EKF, the green line denotes the estimation by CKF, the red line denotes the estimation by AEKF, and the blue line denotes the estimation by ACKF/KF.

Figure 8. Comparison of estimation error of yaw angle. The pink line denotes the estimation by EKF, the green line denotes the estimation by CKF, the red line denotes the estimation by AEKF, and the blue line denotes the estimation by ACKF/KF.

Figure 9. The vehicle test trajectory.

Figure 10. The estimated error of pitch angles. The pink line denotes the estimation by EKF, the green line denotes the estimation by CKF, the red line denotes the estimation by AEKF, and the blue line denotes the estimation by ACKF/KF.

Figure 11. The estimated error of roll angles. The pink line denotes the estimation by EKF, the green line denotes the estimation by CKF, the red line denotes the estimation by AEKF, and the blue line denotes the estimation by ACKF/KF.

Figure 12. The estimated error of yaw angles. The pink line denotes the estimation by EKF, the green line denotes the estimation by CKF, the red line denotes the estimation by AEKF, and the blue line denotes the estimation by ACKF/KF.

Table 1. Estimation of attitude error.

Algorithm	Mean	Standard Deviation
EKF	[16.2312′ 13.1301′ 23.7061′] ^T	[0.4373′ 0.5588′ 0.6972′] ^T
AEKF	[7.0033′ 1.3920′ −3.5554′] ^T	[0.1118′ 0.1490′ 0.5211′] ^T
CKF	[12.9795′ 8.1662′ 22.2739′] ^T	[0.4318′ 0.5020′ 0.6329′] ^T
ACKF/KF	[6.8852′ 1.2172′ −0.7645′] ^T	[0.2279′ 0.0787′ 0.7646′] ^T

Table 2. Initial alignment results of attitude error.

Algorithm	Mean	Standard Deviation
EKF	[0.139′ 1.7329′ 10.7311′] ^T	[0.2296′ 0.2498′ 0.1677′] ^T
AEKF	[0.6441′ 1.9502′ 8.2875′] ^T	[0.1748′ 0.1984′ 0.1723′] ^T
CKF	[0.5891′ 1.9932′ 3.7184′] ^T	[0.0206′ 0.0920′ 0.0379′] ^T
ACKF/KF	[0.4495′ 1.8672′ 1.2627′] ^T	[0.0437′ 0.0853′ 0.0352′] ^T

© 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Gao, K.; Ren, S.; Yi, G.; Zhong, J.; Wang, Z. An Improved ACKF/KF Initial Alignment Method for Odometer-Aided Strapdown Inertial Navigation System. Sensors 2018, 18, 3896. https://doi.org/10.3390/s18113896

AMA Style

Gao K, Ren S, Yi G, Zhong J, Wang Z. An Improved ACKF/KF Initial Alignment Method for Odometer-Aided Strapdown Inertial Navigation System. Sensors. 2018; 18(11):3896. https://doi.org/10.3390/s18113896

Chicago/Turabian Style

Gao, Kang, Shunqing Ren, Guoxing Yi, Jiapeng Zhong, and Zhenhuan Wang. 2018. "An Improved ACKF/KF Initial Alignment Method for Odometer-Aided Strapdown Inertial Navigation System" Sensors 18, no. 11: 3896. https://doi.org/10.3390/s18113896

APA Style

Gao, K., Ren, S., Yi, G., Zhong, J., & Wang, Z. (2018). An Improved ACKF/KF Initial Alignment Method for Odometer-Aided Strapdown Inertial Navigation System. Sensors, 18(11), 3896. https://doi.org/10.3390/s18113896

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Menu

An Improved ACKF/KF Initial Alignment Method for Odometer-Aided Strapdown Inertial Navigation System

Abstract

1. Introduction

2. Nonlinear Initial Alignment Equation

2.1. SINS Error Equation with Large Misalignment Angle

2.2. Odometer/Gyroscopes Dead Reckoning Error Equation

2.3. Kalman Filter Equation

3. Improved Adaptive CKF/KF Method

3.1. Cubature Kalman Filter

3.2. Sage-Husa Adaptive Filter

3.3. ACKF/KF Method

4. Simulation and Experiment

4.1. Simulation and Analysis

4.2. Experiments and Analysis

5. Conclusions

Author Contributions

Funding

Acknowledgments

Conflicts of Interest

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI