Open AccessCommunication

Robust SCKF Filtering Method for MINS/GPS In-Motion Alignment

Huanrui Zhang

and

Xiaoyue Zhang

School of Instrumentation and Optoelectronic Engineering, Beihang University, Beijing 100191, China

Author to whom correspondence should be addressed.

Sensors 2021, 21(8), 2597; https://doi.org/10.3390/s21082597

Submission received: 27 February 2021 / Revised: 29 March 2021 / Accepted: 3 April 2021 / Published: 7 April 2021

(This article belongs to the Section Physical Sensors)

Download

Browse Figures

Review Reports Versions Notes

Abstract

This paper presents a novel multiple strong tracking adaptive square-root cubature Kalman filter (MSTASCKF) based on the frame of the Sage–Husa filter, employing the multi-fading factor which could automatically adjust the Q value according to the rapidly changing noise in the flight process. This filter can estimate the system noise in real-time during the filtering process and adjust the system noise variance matrix Q so that the filtering accuracy is not significantly reduced with the noise. At the same time, the residual error in the filtering process is used as a measure of the filtering effect, and a multiple fading factor is introduced to adjust the posterior error variance matrix in the filtering process, so that the residual error is always orthogonal and the stability of the filtering is maintained. Finally, a vibration test is designed which simulates the random noise of the short-range guided weapon in flight through the shaking table and adds the noise to the present simulation trajectory for semi-physical simulation. The simulation results show that the proposed filter can significantly reduce the attitude estimation error caused by random vibration.

Keywords:

robust filtering algorithm; MIMU; in-motion alignment

1. Introduction

With the development of Micro-Electro-Mechanical System (MEMS) technology, the MEMS inertial measurement unit (MIMU) is gaining attention due to its integrability and miniaturization in several fields such as guided weaponry, unmanned automatic vehicles (UAVs), and robots [1,2,3,4,5,6]. As a dead-reckoning method, the initial alignment accuracy has a vital impact to the final inertial navigation results, while there are only tens of seconds for a tactical weapon. Thus, achieving rapid, accurate, and robust initial alignment is essential to the application of the MIMU. As for the MIMU, it cannot detect the Earth’s rotation properly with high gyro noise, so alignment methods for the high-accuracy Inertial Navigation System (INS) are not suitable for it. A widely used method to achieve initial alignment for the MIMU is to use an extra sensor to provide additional information as a reference for initial alignment during the flight process. The in-flight alignment has been investigated as one of the key technologies for guided weapons and UAV applications in MIMUs in the past 10 years.

The numerous methods to solve in-flight alignment for the MIMU could be mainly divided into two categories, namely the optimization-based approach and the Kalman filter approach. The first one usually turns the problem of alignment into a Whaba’s problem. To solve the problem, several algorithms have been developed, such as the q-method, Quaternion ESTimator (QUEST), and Recursive QUEST (REQUEST) [7,8]. These methods calculate the optimal quaternion by constructing the optimal observation vectors through different filtering methods [9,10,11,12,13]. The latter method is to obtain the misalignment angle between the navigation frame to the computing frame of the INS by constructing an appropriate model for filtering. Owing to the nonlinearity of the misalignment model and the large noise caused by environment, the nonlinear filtering methods occupy a great proportion of the in-motion alignment methods to achieve stability, accuracy, and robustness.

In order to handle with the strong nonlinear system, several filtering methods based on the Kalman filter have been proposed, such as the extended Kalman filter (EKF) [14,15], unscented Kalman filter (UKF) [16,17,18], Gauss–Hermite quadrature filter (GHQF) [19,20], sparse grid quadrature filter (SGQF) [21,22,23], and cubature Kalman filter (CKF) [24,25]. The EKF approximates the nonlinear model to a linear model through Taylor expansion to deal with nonlinear problems. The advantage of this is that the filter structure is simple and the Kalman filter framework can be directly applied. Fang proposed an innovative adaptive extended Kalman filter method for the aerial alignment of airborne position and azimuth measurement systems [14]. This method improves the real-time performance of the algorithm and reduces the interference from GPS measurement noise. However, EKF can only achieve first-order accuracy and cannot handle strong nonlinear problems. In order to solve this problem, UKF is proposed. UKF assumes that the nonlinear system is a Gaussian probability density function (PDF) distribution and creates a series of sigma points based on the unscented transform rule to approximate the distribution of the system. It can achieve the second-order Taylor expansion polynomial. Saman Mukhtar Siddiqui applied the initialization process of the Central Difference Unscented Kalman filter (CDUKF) to the square-root unscented Kalman filter and verified the advantages of CDUKF in calculation time and accuracy in the on-board experiment to solve the alignment problem of misalignment angles within 30° [26]. Wang Dingjie proposed an adaptive unscented Kalman filter for a small UAV MEMS navigation system. With an initial error angle of 30°, the posture is completed in 75 s, which is more accurate than EKF and traditional UKF [27]. However, there is also a problem: For systems with a dimension higher than 3, the weight of the sigma points could be negative, and the filter process will be unstable. As a matter of fact, this problem is caused by the Unscented Transformation (UT) rule. To avoid the negativity of the sigma points, GHQF, SGQF, and CKF have been proposed using different integrated rules rather than the UT rule, which not only solves the mentioned problem but also maintains the accuracy of the second-order Taylor expansion. The GHQF utlizes the Gauss–Hermite quadrature (GHQ) rule to conduct the filter process with the quadrature sampling points that it can be accurate to the arbitrary order Taylor expansion polynomial [23]. However, the drawback of the GHQ rule is that it needs a large number of the sampling points due to the application of the direct tensor product. Thus, the GHQF would also have problems in application in high-dimensional systems. CKF with the cubature rule guarantees the positive weight, and its number of sampling points is 2n. It could also achieve a higher accuracy while applying the high-degree cubature rule with less computational burden [24]. Compared with CKF, the square-root CKF (SCKF) has more advantage not only in convergence speed but also in calculation efficiency [25]. To achieve a better performance of robustness during the in-flight process, we added several methods, such as Sage–Husa noise statistic and fading factors, to the frame of SCKF.

The rest of this paper is organized as follows. An Optimisation-Based initial Alignment (OBA) method based on the Rodrigues parameter for random misalignment angle is introduced in Section 2, where the state model and the measurement equation are used in the simulation. Then, the specific formulation of MSTASCKF is presented in Section 3. In Section 4, a semi-physical simulation is carried out to evaluate the performance of the proposed algorithm under the vibration condition. The alignment results are compared with the conventional SCKF. Finally, the conclusions are drawn in Section 5.

2. Alignment Model for Random Misalignment Angle

In this section, a dynamic alignment model in the form of a random misalignment angle [28] is introduced.

The coordinate systems are defined as follows:

Body frame (b-frame): The origin of the coordinate system is at the center of mass of the carrier, the Y axis is the longitudinal axis of the carrier, the X axis points to the right of the carrier and is perpendicular to the Y axis, and the Z axis and X and Y axes form a right-handed coordinate system.

Navigation frame (n-frame): The origin of the coordinate system is at the center of mass of the carrier, the X axis points to the geographic north direction, the Y axis points to the geographic east direction, and the Z axis points to the up direction.

Inertial navigation frame (

i_{n}

-frame): An inertial coordinate system, which coincides with the initial navigation frame.

Inertial body frame (

i_{b}

-frame): The inertial coordinate system, which coincides with the body frame at the initial time.

The attitude matrix can be decomposed into three parts based on the chain rule as follows:

C_{b}^{n} (t) = C_{i n}^{n} (t) C_{i b}^{i n} C_{b}^{i b} (t)

(1)

where

C_{i n}^{n} (t)

C_{b}^{i b} (t)

denotes the time-varying transformation matrix from the

i_{n}

-frame to the n-frame and the transformation matrix from the b-frame to the

i_{b}

-frame, respectively.

C_{i b}^{i n}

is a constant transformation matrix from the

i_{b}

-frame to the

i_{n}

-frame.

From Equation (1), we can see that the attitude matrix

C_{b}^{n} (t)

can be divided into three parts:

C_{i n}^{n} (t)

represents the time-varying attitude due to the rotation of the n-frame caused by the earth rotation rate and the vehicle’s transport rate related to earth, which can be calculated with the change of latitude and the longitude.

C_{b}^{i b} (t)

describes the time-varying attitude due to the rotation of the vehicle from the b-frame to the

i_{b}

-frame, which can be computed with the angular rate measured by the gyro. It is easy to derive the recursive form:

C_{b}^{i b} (t_{k}) = C_{b}^{i b} (t_{k - 1}) C_{b (t_{k})}^{b (t_{k - 1})}, C_{b}^{i b} (0) = I

(2)

The

C_{b (t_{k})}^{b (t_{k - 1})}

represents the rotation of the b-frame from time

t_{k - 1}

to time

t_{k}

. It is appropriate to assume the rotation is tiny while the sampling rate of INS is high.

C_{b (t_{k})}^{b (t_{k - 1})} = [I - (- ω_{b} (t_{k})) \times]

(3)

As for the last part,

C_{i b}^{i n}

can be derived by solving the attitude determination problem using the relationship between vectors:

V^{i n} = C_{i b}^{i n} V^{i b}

(4)

where the

V^{i n}

and

V^{i b}

are the integration of the specific force in the

i_{n}

-frame and the

i_{b}

-frame, respectively. We assume the specific force detected by accelerometer at time

t_{k}

in the

i_{b}

-frame is given by following expression:

{\hat{V}}^{i b} (t_{k}) = V^{i b} (t_{k}) + δ V^{i b} (t_{k})

(5)

where the

δ V^{i b} (t_{k})

is the integration error of

{\hat{V}}^{i b} (t_{k})

Equation (4) could be rewritten as:

V^{i n} (t_{k}) = C_{i b}^{i n} ({\hat{V}}^{i b} (t_{k}) - δ V^{i b} (t_{k}))

(6)

The attitude matrix

C_{i b}^{i n}

can be written in the form of the Rodrigues vector [29] as:

C_{i b}^{i n} = {[I + (l \times)]}^{- 1} [I - (l \times)]

(7)

where the

l \times

is the skew-symmetric matrix of the Rodrigues vector.

Substituting Equation (7) to Equation (6) leads to:

{\hat{V}}^{i b} (t_{k}) - V^{i n} (t_{k}) = ({\hat{V}}^{i b} (t_{k}) + V^{i n} (t_{k})) \times l + l \times δ V^{i b} (t_{k}) + w_{t_{k}}

(8)

where the

w_{t_{k}}

is the inertial sensor error.

Defining the sum and difference as follows:

D_{t_{k}} = {\hat{V}}^{i b} (t_{k}) - V^{i n} (t_{k}), S_{t_{k}} = {\hat{V}}^{i b} (t_{k}) + V^{i n} (t_{k})

(9)

We could obtain:

D_{t_{k}} = S_{t_{k}} \times l + l \times δ V^{i b} (t_{k}) + w_{t_{k}}

(10)

Equation (10) is the measurement equation of our filtering model. Once the optimal estimation of the Rodrigues vector

l

is obtained, we can derive

C_{i b}^{i n}

from Equation (7).

Considering the bias and the random noise of the gyro and the accelerometer, the output of gyro and accelerometer can be derived as:

{\hat{ω}}^{b} = ω^{b} + ε^{b} + w_{g}^{b}

(11)

{\hat{f}}^{b} = f^{b} + \nabla^{b} + w_{a}^{b}

(12)

Referring to the error model of INS under n-frame, the error equation of the attitude and velocity can be obtained:

{\dot{φ}}^{i b} = - {\hat{C}}_{b}^{i b} (ε^{b} + w_{g}^{b}), φ^{i b} (0) = 0

(13)

δ {\dot{V}}^{i b} = - {\hat{f}}^{i b} \times φ^{i b} + {\hat{C}}_{b}^{i b} (\nabla^{b} + w_{a}^{b}), δ {\dot{V}}^{i b} (0) = 0

(14)

The gradient of the Rodrigues vector is zero, since the attitude matrix

C_{i b}^{i n}

is a constant matrix.

\dot{l} = 0

(15)

The gyroscope drifts

ε^{b}

and the accelerometer bias

\nabla^{b}

are irrelevant with time, so we can derive:

\begin{array}{l} ε^{b} = 0 \\ \nabla^{b} = 0 \end{array}

(16)

The state vector was selected as follows:

X = {[\begin{matrix} l & φ^{i b} & δ V^{i b} & ε^{b} & \nabla^{b} \end{matrix}]}^{T}

(17)

3. Mstasckf Method

In this section, we demonstrate the algorithm design ideas and the overall process of the algorithm. The algorithm was based on the frame of square-root Kalman filter and the Sage–Husa noise estimator. The aim to our work was to add a criterion to the Sage–Husa noise estimator to ensure the calculated system error variance remains positive, which could make the filter stable. Besides that, a multiple fading factor was implemented to make the residual orthogonal, which could accelerate system convergence and improve the accuracy.

3.1. Sage–Husa Noise Estimator

The Sage–Husa filtering method was originally proposed by P.A Sage and G.W Husa in 1969 [30]. The core idea of the algorithm is to construct a real-time filtered noise statistic based on maximum a posteriori information (MAP) to count the system noise and measurement noise in the filtering process.

For a nonlinear system as shown in (18):

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

(18)

where the

w_{k - 1}

and

v_{k}

are the system noise and measurement noise, respectively.

f (*)

and

h (*)

denote the nonlinear state model and measurement model, respectively.

Definite the system’s posterior error variance matrix at the initial moment as:

P_{0} = E [(x_{0} - {\bar{x}}_{0}) {(x_{0} - {\bar{x}}_{0})}^{T}]

(19)

where the

{\bar{x}}_{0}

is the mean value of the initial state vector

x_{0}

. At time

t_{k}

, assuming the system noise

q_{k}

, the measurement noise

r_{k}

, the system noise covariance matrix

Q_{k}

, and the measurement noise covariance matrix

R_{k}

are unknown. The state quantity set and quantity measurement set from the initial time to k time are, respectively,

X_{k} = [x_{0}, x_{1}, \dots, x_{k}]

Z_{k} = [z_{1}, z_{2}, \dots, z_{k}]

It can be seen from the Bayes formula that the probability of estimating the state vector at time

t_{k}

, the system noise, the measurement noise, and its noise variance matrix from the measurement sequence from the initial time to time

t_{k}

can be expressed as:

\begin{array}{l} p (X_{k}, q_{k}, Q_{k}, r_{k}, R_{k} | Z_{k}) = J \\ = p (Z_{k} | X_{k}, q_{k}, Q_{k}, r_{k}, R_{k}) p (X_{k} | q_{k}, Q_{k}, r_{k}, R_{k}) \times p (Z_{k}) \end{array}

(20)

where

p (Z_{k})

has nothing to do with noise estimation. Then, (20) can be reduced to the problem of solving the maximum value of probability density as follows:

p (Z_{k} | X_{k}, q_{k}, Q_{k}, r_{k}, R_{k}) \times p (X_{k} | q_{k}, Q_{k}, r_{k}, R_{k}) \times p (q_{k}, r_{k}, Q_{k}, R_{k})

(21)

Considering that

C o v (q_{k}, r_{k}) = 0

and each measurement can be regarded as independent.

{\hat{q}}_{k} = \frac{1}{k} \sum_{j = 1}^{k} [{\hat{x}}_{j | k} - f_{j - 1} (x_{j - 1})]

(22)

{\hat{Q}}_{k} = \frac{1}{k} \sum_{j = 1}^{k} [{\hat{x}}_{j k} - f_{j - 1} (x_{j - 1}) - q_{k}] \times {[{\hat{x}}_{j k} - f_{j - 1} (x_{j - 1}) - q_{k}]}^{T}

(23)

{\hat{r}}_{k} = \frac{1}{k} \sum_{j = 1}^{k} [z_{j} - h (x_{j})]

(24)

{\hat{R}}_{k} = \frac{1}{k} \sum_{j = 1}^{k} \{[z_{j} - h ({\hat{x}}_{j | j - 1}) - r_{k}] \times {[z_{j} - h ({\hat{x}}_{j}) - r_{k}]}^{T}\}

(25)

When the noise changes with time, the filter estimates and measurements at the more recent time can better reflect the current noise situation. Therefore, it is necessary to adjust the weights of the filter estimates and measurements at different times in the noise statistics process to obtain more accurate noise statistics.

Define the weighting coefficient set

\{λ_{i}\}

λ_{i} = λ_{i - 1} b

d_{k} = \frac{1 - b}{1 - b^{k}}

i = 0, 1, 2, \dots, k

, b as a constant value between 0 to 1. Hence, we can derive a discrete noise estimator for time-varying noise as:

{\hat{q}}_{k} = (1 - d_{k}) {\hat{q}}_{k - 1} + d_{k} ({\hat{x}}_{k} - Φ_{k l k - 1} X_{k - 1})

(26)

{\hat{r}}_{k} = (1 - d_{k}) {\hat{r}}_{k - 1} + d_{k} (z_{k} - H_{k} {\hat{X}}_{k / k - 1})

(27)

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

(28)

{\hat{Q}}_{k} = (1 - d_{k}) {\hat{Q}}_{k - 1} + d_{k} (K_{k} v_{k} v_{k}^{T} K_{k}^{T} + P_{k} - Φ_{k / k - 1} P_{k - 1} Φ_{k / k - 1}^{T})

(29)

where

Φ_{k l k - 1}

is the one-step prediction matrix from time k − 1 to time k and H_k is the measurement model of the system.

v_{k} = z_{k} - H_{k} {\hat{X}}_{k | k - 1} - r_{k}

is the residual of the measurement updating process.

3.2. SCKF with Sage–Husa Noise Estimator

There is a problem when applying the Sage–Husa noise estimator to the SCKF frame. Under the framework of square-root volume Kalman filtering, it is necessary to use the Cholesky decomposition form of the system noise variance matrix and the measurement noise variance matrix to perform filtering iterations during the filtering process [24], which requires us to always obtain positive definite in the noise estimator. Otherwise, the filter will be unstable.

Define the second part of (29) and (28) as:

J_{q} = K_{k} v_{k} v_{k}^{T} K_{k}^{T} + P_{k} - Φ_{k / k - 1} P_{k - 1} Φ_{k / k - 1}^{T}

(30)

J_{r} = v_{k} v_{k}^{T} - H_{k} P_{k | k - 1} H_{k}^{T}

(31)

When the initial system noise covariance is positive, the sign of

{\hat{Q}}_{k}

is only related to the sign of

J_{q}

, and the same situation occurs for

{\hat{R}}_{k}

. To ensure the stability of the filter, we used the equation as follows to compute

{\hat{Q}}_{k}

and

{\hat{R}}_{k}

in the condition

J_{q} < 0

J_{r} < 0

{\hat{Q}}_{k} = (1 - d_{k}) {\hat{Q}}_{k - 1} + d_{k} (K_{k} v_{k} v_{k}^{T} K_{k}^{T} + P_{k})

(32)

{\hat{R}}_{k} = (1 - d_{k}) {\hat{R}}_{k - 1} + d_{k} v_{k} v_{k}^{T}

(33)

Unfortunately, when the covariance matrix is computed with (32) or (33), the result is no longer unbiased. Thus, to make up to the accuracy loss caused by the bias, a multiple fading factor was introduced.

3.3. Multiple Fading Factor Calculation

The multiple fading factors method is based on the orthogonality principle of the residual. The computation process of multiple fading factors under SCKF could be summarized as follows, and readers can refer to the methodology described by the authors of [31] for more analysis and additional derivations.

Definite the multiple fading factor sequence as:

γ_{k} = d i a g [γ_{k}^{1}, γ_{k}^{2}, \dots, γ_{k}^{n}]

(34)

where n is the dimension of the state vector.

γ_{k}^{i} = \{\begin{array}{l} γ_{k}^{0 i}, & γ_{k}^{0 i} > 1 \\ 1, & γ_{k}^{0 i} \leq 1 \end{array} i = 1, \dots, n

(35)

γ_{k}^{0 i} = \frac{tr [N_{k}]}{\sum_{i = 1}^{n} α_{i} M_{k}^{i n}}

(36)

N_{k} = V_{k} - {[P_{x z, k | k - 1}^{(l)}]}^{T} {[{(P_{k | k - 1}^{(l)})}^{- 1}]}^{T} Q_{k - 1} [{(P_{k | k - 1}^{(l)})}^{- 1}] [P_{x z k | k - 1}^{(l)}] - R_{k}

(37)

M_{k} = (P_{k | k - 1}^{(l)} - Q_{k - 1}) [{(P_{k | k - 1}^{(l)})}^{- 1}] [P_{x z, k | k - 1}^{(l)}] {[P_{x z, k | k - 1}^{(l)}]}^{T} {[{(P_{k | k - 1}^{(l)})}^{- 1}]}^{T}

(38)

3.4. MSTASCKF Update

Step 1: Initialization

S_{k - 1} = c h o l (P_{k - 1})

(39)

where

S_{k - 1}

is the lower triangle matrix of the result of the Cholesky decomposition.

Step 2: Prediction

In the prediction step, MSTASCKF computes the mean

x_{k | k - 1}

and

S_{k | k - 1}^{(l)}

, the square-root form of the associated covariance without the correction of the multiple fading factors, using the cubature rule, in which each cubature point has the same weight.

Obtain the cubature points:

X_{i, k - 1 | k - 1} = S_{k - 1} ξ_{i} + x_{k - 1}

(40)

Propagate the cubature points through the state model:

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

(41)

Estimate the predicted state:

x_{k | k - 1} = \sum_{i = 1}^{m} ω_{i} X_{i, k | k - 1}^{*}

(42)

X_{k | k - 1}^{*} = {(\sqrt{ω_{i}} (X_{i, k | k - 1} - x_{k | k - 1}))}_{i = 1}^{m}

(43)

In the proposed algorithm, the weights of the cubature points are equal, which is

ω_{i} = \frac{1}{n}

Square-root factor of the predicted error covariance:

S_{k | k - 1}^{(l)} = q r (\begin{matrix} X_{k | k - 1}^{*} & S_{Q_{k - 1}} \end{matrix})

(44)

where

S_{Q_{k - 1}}

is the square-root of the system noise

Q_{k - 1}

at time k − 1 and

q r ()

is the QR decomposition function to obtain the square-root factor of the matrix.

Step 3: Calculating the multiple fading factor

Obtain the cubature points:

X_{i, k | k - 1}^{(l)} = S_{k | k - 1}^{(l)} ξ_{i} + x_{k | k - 1}

(45)

Propagate the cubature points through measurement model:

Z_{i, k | k - 1}^{* (l)} = h (X_{i, k | k - 1}^{(l)})

(46)

Obtain the predicted measurement:

z_{k | k - 1}^{(l)} = \sum_{i = 1}^{m} ω_{i} Z_{i, k | k - 1}^{* (l)}

(47)

Z_{k | k - 1}^{* (l)} = {(\sqrt{ω_{i}} (Z_{i, k | k - 1}^{(l)} - z_{k | k - 1}^{(l)}))}_{i = 1}^{m}

(48)

S_{z z, k | k - 1}^{(l)} = q r (\begin{matrix} Z_{k | k - 1}^{* (l)} & S_{R_{k}} \end{matrix})

(49)

P_{x z, k | k - 1} = χ_{k | k - 1}^{(l)} Z_{k | k - 1}^{* (l)}^{T}

(50)

Obtain the multiple fading factor with (35)–(38) and adjust

S_{k | k - 1}

with multiple fading factors:

S_{k | k - 1} - \sqrt{γ_{k}} S_{k | k - 1}^{(l)}

(51)

The l in the upper right corner of the symbol indicates that this quantity has not been corrected by multiple fading factors.

Step 4: Measurement updating

Create cubature points:

X_{i, k | k - 1} = S_{k | k - 1} ξ_{i} + x_{k | k - 1}

(52)

Propagate the cubature points through measurement model:

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

(53)

z_{k | k - 1} = \sum_{i = 1}^{m} ω_{i} Z_{i, k | k - 1}^{*}

(54)

Z_{k | k - 1}^{*} = {(\sqrt{ω_{i}} (Z_{i, k | k - 1} - z_{k | k - 1}))}_{i = 1}^{m}

(55)

S_{z z, k | k - 1} = q r (\begin{matrix} Z_{k | k - 1}^{*} & S_{R_{k}} \end{matrix})

(56)

χ_{k | k - 1} = {(\sqrt{ω_{i}} (X_{i, k | k - 1} - x_{k | k - 1}))}_{i = 1}^{m}

(57)

P_{x z, k | k - 1} = χ_{k | k - 1} Z_{k | k - 1}^{*}^{T}

(58)

Obtain the Kalman gain:

W_{k} = P_{x z, k | k - 1} / S_{z z, k | k - 1} / S_{z z, k | k - 1}^{T}

(59)

Update the state estimation:

x_{k} = x_{k | k - 1} + W_{k} (z_{k} - z_{k | k - 1})

(60)

Obtain the square-root of noise covariance estimation:

S_{k} = q r (\begin{matrix} χ_{k | k - 1} - W_{k} Z_{k | k - 1}^{*} & W_{k} S_{R_{k}} \end{matrix})

(61)

Step 5: Estimation of the noise covariance

Obtain

J_{q}

and

J_{r}

Compute the system noise covariance and the measurement noise covariance based on the sign of

J_{q}

and

J_{r}

using (28), (29) or (32), and (33).

4. Semi-Physical Simulation Results and Discussion

In order to assess the performance of the MSTASCKF during the in-flight alignment for GPS/MIMU under vibration, a semi-physical simulation is described in this section. A trajectory maintaining the IMU data and GPS data of typical short-range guided weapon was created, and an experiment to simulate the flight vibration was carried out to obtain simulated vibration noise data. Then, the vibration noise was added to the IMU data to simulate the vibration in-flight. The performance of MSTASCKF was compared with the traditional SCKF, which validated the efficiency of the two strategies added in the MSTASCKF.

4.1. Vibration Experiment

Guided weapons generally experience storage, transportation, and flight environments after their production cycle. During flight, the high-speed turbulent flow field formed around the projectile due to high-speed flight induces complex vibration, overload, high temperatures, and other environments [32]. The vibration environment has a greater impact on the structure of the projectile and may even cause deformation of the projectile or shedding of solder joints on the circuit board. The vibration of guided weapons during flight can be divided into two categories: 0–2000 HZ, low-frequency noise excited by vibration mainly transmitted by mechanical structures, and 10–10,000 HZ noise excited by frequency range. Among them, vibration excitation noise has an obvious effect on the mass-spring structure, and excitation noise mainly has an obvious effect on the plate and shell structure. Therefore, the main focus of this paper was the impact characteristics of the frequency range 0–2000 HZ by vibration excitation noise on the output of the inertial navigation system.

In order to obtain the output data of MIMU under the vibration environment, the following random vibration test was designed. The power spectrum density and frequency range of the vibration are as shown in the Figure 1. The experimental platform is shown in Figure 2, which is composed of a MIMU (MTI-1 from the XSENS company), a vibration table, and the tooling for the vibration table. The specification of the MIMU is listed in Table 1.

Since the MTI-1 MIMU cannot be sensitive to the Earth’s rotation angular rate, its gyro output under static conditions can be regarded as zero bias and random noise. Vibration noise can be obtained by subtracting the average value of the data at rest when the vibration table is turned on. Thus, the process of the experiment was arranged as in Figure 3 to obtain both the static output and the vibration output of the three axes. The processed vibration noise data in the experiment are shown in Figure 4.

4.2. Simulation Condition

Generally, the flight time of short-range guided weapons is within 60 s, and the flight distance is within 20 km. According to the trajectory characteristics of short-range guided weapons, the trajectory attitude and speed used in the semi-physical simulation are shown in the Table 2. The attitude and the velocity of the trajectory are shown in Figure 5.

The trajectory was set for 30 s, and the initial attitude angle was set as

ψ = 60^{\circ}, θ = 45^{\circ}, γ = 0^{\circ}

, representing the yaw, pitch, and roll, respectively. The yaw change was positive from north to east and negative from west to north, the pitch change was positive from the horizontal section of the projectile upward and negative from the downward direction, the hour hand was positive and the counterclockwise was negative, and the projectile speed was the projectile speed at the end of the current time period.

In the simulation, the three-axis gyro data were all added with a 100°/h bias. The speed error of the GPS was 0.1 m/s. Thus, the initial state covariance was set as follows.

\{\begin{matrix} P_{0} = P_{0} \otimes I_{3} \\ P_{0} = d i a g ([P_{l}, P_{φ^{i_{b}}}, P_{δ V^{i_{b}}}, P_{ε}, P_{\nabla}]) \end{matrix}

(62)

where

P_{l_{j}} = {(1)}^{2}

P_{φ_{j}^{i_{b}}} = {(1 e - 4^{'})}^{2}

P_{δ V_{j}^{j_{b}}} = {(0.1 m / s)}^{2}

P_{ε_{j}} = {(100^{°} / h)}^{2}, P_{\nabla_{j}} = {(3 mg)}^{2}

R = diag ({[\begin{array}{l} 0.1 & 0.1 & 0.1 \end{array}]}^{2})

4.3. Simulation Results under Vibration Noise

The in-flight alignment progresses were calculated using MSTASCKF and traditional SCKF, respectively. The attitude error is shown in Figure 6. The specific value of the estimated attitude error and its RMS of the proposed method and SCKF is shown in Table 3.

The figure shows that the MSTASCKF has an advantage in convergence speed in the alignment process. The estimation of yaw, pitch, and roll converged within 15 s and finally reached the accuracy of 0.08°, 0.05°, and 0.25°, respectively. The SCKF had a worse performance, which can be easily observed as it converged slower and only met the accuracy of −2.85°, 0.95°, 1.65° for the yaw, pitch, and roll under the vibration noise.

To further compare the stability of the MSTASCKF and SCKF, the RMS of the attitude is shown in the Figure 7.

We can see that the proposed method had a better RMS performance than the SCKF. The RMS values of the three attitudes of MSTASCKF and SCKF at the end of the alignment process were

[\begin{matrix} {1.13}^{\circ} & {0.14}^{\circ} & {5.35}^{\circ} \end{matrix}]

and

[\begin{matrix} {2.5}^{\circ} & {0.99}^{\circ} & {6.12}^{\circ} \end{matrix}]

, respectively. The RMS of the yaw and roll was much larger than the pitch. This was caused by the estimation scheme, since the MSTASCKF and SCKF had the same trend. The first estimation of roll and pitch had such a large deviation that the subsequent RMS value was affected.

In fact, the main metrics of alignment performance was the final accuracy at the end of the alignment process rather than the accuracy during the process.

The attitude error is shown in the Table 4 when the alignment time was limited to 10 s and 20 s. These data could also show that the MSTASCKF had an advantage in the convergence speed and the estimation stability.

4.4. Discussion

In this section, a semi-physical simulation for in-flight alignment under vibration noise was carried out to evaluate the performance of the proposed algorithm. Compared with conventional SCKF, it is apparent that the proposed method had a better performance not only in the convergence speed in the beginning of the process but also in the accuracy and steady state. For the proposed filter, the results are as follows.

We proposed a robust SCKF algorithm, which could significantly increase the in-flight alignment accuracy under the vibration noise.

Compared with conventional SCKF filter, the estimation performance was better. The attitude error was smaller and the RMS was also smaller. The convergence time of the proposed method was no longer than 10 s. Thus, the proposed method can meet the rapidity and accuracy requirements of in-flight alignment.

5. Conclusions

The rapid and accurate in-flight alignment has a vital impact on the performance of the guided weapon applied MIMU. To improve the in-flight alignment performance of MIMU/GPS system under flight vibration, this paper proposed a MSTASCKF filter based on the Sage–Husa noise estimator and multiple fading factors. The filter solved the instability problem of SCKF caused by the Sage–Husa estimator and improved the accuracy. The main work of this paper was as follows.

An experimental vibration environmental test was conducted to maintain the simulated vibration noise data during the flight and a semi-physical simulation was carried out to evaluate the performance of the proposed algorithm. The filtering method had an advantage in the convergence speed, accuracy, and stability compared to SCKF.

Author Contributions

H.Z. conceived and designed the experiment. H.Z. and X.Z. wrote and revised the manuscript. All authors have read and agreed to the published version of the manuscript.

Funding

As part of the research project, this work was supported by the Top Talent Project at Beihang University and National Natural Science Foundation of China (61703024).

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Not applicable.

Conflicts of Interest

The authors declare no conflict of interest.

References

Gao, C.; Zhao, G.; Lü, J. In-flight misalignment attitude estimation for UAV based on GPS and UKF/MPF filter. In Proceedings of the IEEE Chinese Guidance, Navigation and Control Conference, Yantai, China, 8–10 August 2014. [Google Scholar]
Kaygısız, B.H.; Şen, B. In-motion Alignment of a Low-cost GPS/INS under Large Heading Error. J. Navig. 2015, 68, 355–366. [Google Scholar] [CrossRef] [Green Version]
Chung, H.-Y.; Hou, C.-C.; Chen, Y.-S. Indoor Intelligent Mobile Robot Localization Using Fuzzy Compensation and Kalman Filter to Fuse the Data of Gyroscope and Magnetometer. IEEE Trans. Ind. Electron. 2015, 62, 6436–6447. [Google Scholar] [CrossRef]
Costanzi, R.; Fanelli, F.; Monni, N.; Ridolfi, A.; Allotta, B. An Attitude Estimation Algorithm for Mobile Robots Under Unknown Magnetic Disturbances. IEEE/ASME Trans. Mechatron. 2016, 21, 1900–1911. [Google Scholar] [CrossRef]
Wendel, J.; Meister, O.; Schlaile, C.; Trommer, G.F. An integrated GPS/MEMS-IMU navigation system for an autonomous helicopter. Aerosp. Sci. Technol. 2006, 10, 527–533. [Google Scholar] [CrossRef]
Wang, D.; Chen, L.; Wu, J. Novel in-flight coarse alignment of low-cost strapdown inertial navigation system for unmanned aeri-al vehicle applications. Trans. Jpn. Soc. Aeronaut. Space Sci. 2016, 59, 10–17. [Google Scholar] [CrossRef] [Green Version]
Choukroun, D.; Bar-Itzhack, I.Y.; Oshman, Y. Optimal-REQUEST Algorithm for Attitude Determination. J. Guid. Control. Dyn. 2004, 27, 418–425. [Google Scholar] [CrossRef]
Shuster, M.D.; Oh, S.D. Three-axis attitude determination from vector observations. J. Guid. Control. 1981, 4, 70–77. [Google Scholar] [CrossRef]
Wu, Y.; Pan, X. Velocity/Position Integration Formula Part I: Application to In-Flight Coarse Alignment. IEEE Trans. Aerosp. Electron. Syst. 2013, 49, 1006–1023. [Google Scholar] [CrossRef] [Green Version]
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]
Chang, L.; Hu, B.; Li, Y. Backtracking Integration for Fast Attitude Determination-Based Initial Alignment. IEEE Trans. Instrum. Meas. 2015, 64, 795–803. [Google Scholar] [CrossRef]
Li, J.; Xu, J.; Chang, L.; Zha, F. An Improved Optimal Method for Initial Alignment. J. Navig. 2014, 67, 727–736. [Google Scholar] [CrossRef]
Xie, L.; Lu, J. Optimisation-based Transfer Alignment and Calibration Method for Inertial Measurement Vector Integration Matching. J. Navig. 2017, 70, 595–606. [Google Scholar] [CrossRef]
Jiancheng, F.; Sheng, Y. Study on Innovation Adaptive EKF for In-Flight Alignment of Airborne POS. IEEE Trans. Instrum. Meas. 2011, 60, 1378–1388. [Google Scholar] [CrossRef]
Xi, Y.; Zhang, X.; Li, Z.; Zeng, X.; Tang, X.; Cui, Y.; Xiao, H. Double-ended travelling-wave fault location based on residual analysis using an adaptive EKF. IET Signal Process. 2018, 12, 1000–1008. [Google Scholar] [CrossRef]
Li, W.; Jia, Y. Consensus-Based Distributed Multiple Model UKF for Jump Markov Nonlinear Systems. IEEE Trans. Autom. Control 2011, 57, 227–233. [Google Scholar] [CrossRef]
Xing, Z.; Xia, Y. Comparison of centralised scaled unscented Kalman filter and extended Kalman filter for multisensor data fusion architectures. IET Signal Process. 2016, 10, 359–365. [Google Scholar] [CrossRef]
Chang, G.; Xu, T.; Wang, Q. Alternative framework for the iterated unscented Kalman filter. IET Signal Process. 2017, 11, 258–264. [Google Scholar] [CrossRef]
Arasaratnam, I.; Haykin, S.; Elliott, R.J. Discrete-Time Nonlinear Filtering Algorithms Using Gauss–Hermite Quadrature. Proc. IEEE 2007, 95, 953–977. [Google Scholar] [CrossRef]
Singer, H. Conditional Gauss–Hermite Filtering With Application to Volatility Estimation. IEEE Trans. Autom. Control 2015, 60, 2476–2481. [Google Scholar] [CrossRef]
Jia, B.; Xin, M.; Cheng, Y. Sparse Gauss-Hermite Quadrature Filter with Application to Spacecraft Attitude Estimation. J. Guid. Control Dyn. 2011, 34, 367–379. [Google Scholar] [CrossRef]
Jia, B.; Xin, M.; Cheng, Y. Sparse-grid quadrature nonlinear filtering. Automatica 2012, 48, 327–341. [Google Scholar] [CrossRef]
Jia, B.; Xin, M. Vision-Based Spacecraft Relative Navigation Using Sparse-Grid Quadrature Filter. IEEE Trans. Control Syst. Technol. 2012, 21, 1595–1606. [Google Scholar] [CrossRef]
Arasaratnam, I.; Haykin, S. Cubature Kalman Filters. IEEE Trans. Autom. Control 2009, 54, 1254–1269. [Google Scholar] [CrossRef] [Green Version]
Zhang, T.; Xu, X. A new method of seamless land navigation for GPS/INS integrated system. Measurement 2012, 45, 691–701. [Google Scholar] [CrossRef]
Siddiqui, S.M. Integrated Navigation and Self alignment using Square Root Unscented Kalman Filtering. In Proceedings of the Interna-tional Bhurban Conference on Applied Sciences and Technology, Islamabad, Pakistan, 15–19 January 2013; IEEE: New York, NY, USA, 2013; pp. 73–76. [Google Scholar]
Wang, D.; Lv, H.; Wu, J. In-flight initial alignment for small UAV MEMS-based navigation via adaptive unscented Kalman filtering approach. Aerosp. Sci. Technol. 2017, 61, 73–84. [Google Scholar] [CrossRef]
Cui, X.; Mei, C.; Qin, Y.; Yan, G.; Fu, Q. In-motion Alignment for Low-cost SINS/GPS under Random Misalignment Angles. J. Navig. 2017, 70, 1224–1240. [Google Scholar] [CrossRef]
Mortari, D.; Markley, F.L.; Singla, P. Optimal Linear Attitude Estimator. J. Guid. Control Dyn. 2007, 30, 1619–1627. [Google Scholar] [CrossRef]
Sage, A.P.; Husa, G.W. Adaptive filtering with unknown prior statistics. In Proceedings of the 1969 Joint Automatic Control Conference, Washington, DC, USA, 7–9 May 1969. [Google Scholar]
Cheng, S.; Li, L.; Chen, J. Fusion Algorithm Design Based on Adaptive SCKF and Integral Correction for Side-Slip Angle Observation. IEEE Trans. Ind. Electron. 2018, 65, 5754–5763. [Google Scholar] [CrossRef]
Liu, Q.; Chen, Y.; Tian, G.; Li, M. Laboratory Test Methods of Vibration Environment for the Flighting Missiles. Equip. Environ. Eng. 2016, 13, 68–75. [Google Scholar]

Figure 1. The power spectrum density and frequency range of the vibration experiment.

Figure 2. The experimental platform.

Figure 3. The vibration experiment process.

Figure 4. The vibration noise data of the vibration experiment of each axis.

Figure 5. The real-time trajectory of the simulation. (a) The real-time attitude of the simulated trajectory. (b) The real-time velocity of the simulated trajectory in the direction of east, north, and up, respectively.

Figure 6. The attitude error comparison between MSTASCKF and SCKF.

Figure 7. The RMS of the attitude error of MSTASCKF and SCKF.

Table 1. Specifications of MTI-1.

Parameters	Accelerometer	Gyroscope
Dynamic range	±16 g	±2000°/s
Update rate	100 Hz	100 Hz
Bias	3 mg	0.15°/s
Bias stability	0.03 mg	10°/h
Random walk	120 μg/√Hz	0.07°/s/√Hz

Table 2. The specific value of the attitude and velocity used in the trajectory.

Time	Yaw Change	Pitch Change	Speed
1–3 s	5°	0°	200 m/s
4–17 s	−30°	−45°	135 m/s
18–30 s	25°	−45°	50 m/s

Table 3. The attitude error and RMSE comparison between MSTASCKF and SCKF.

	MSTASCKF		SCKF
	Attitude Error	RMS	Attitude Error	RMS
Yaw	0.08°	1.13°	−2.85°	2.5°
Pitch	0.05°	0.14°	0.96°	0.99°
Roll	0.25°	5.35°	1.66°	6.12°

Table 4. Thea attitude error comparison between MSTASCKF and SCKF when the alignment time was 10 s and 20 s.

	Attitude Error
	MSTASCKF		SCKF
Time	10 s	20 s	10 s	20 s
Yaw	0.29°	0.15°	−1.60°	−2.73°
Pitch	−0.02°	−0.08°	0.82°	1.25°
Roll	0.32°	0.19°	2.89°	2.05°

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

© 2021 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 (https://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Zhang, H.; Zhang, X. Robust SCKF Filtering Method for MINS/GPS In-Motion Alignment. Sensors 2021, 21, 2597. https://doi.org/10.3390/s21082597

AMA Style

Zhang H, Zhang X. Robust SCKF Filtering Method for MINS/GPS In-Motion Alignment. Sensors. 2021; 21(8):2597. https://doi.org/10.3390/s21082597

Chicago/Turabian Style

Zhang, Huanrui, and Xiaoyue Zhang. 2021. "Robust SCKF Filtering Method for MINS/GPS In-Motion Alignment" Sensors 21, no. 8: 2597. https://doi.org/10.3390/s21082597

APA Style

Zhang, H., & Zhang, X. (2021). Robust SCKF Filtering Method for MINS/GPS In-Motion Alignment. Sensors, 21(8), 2597. https://doi.org/10.3390/s21082597

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

Article Menu

Robust SCKF Filtering Method for MINS/GPS In-Motion Alignment

Abstract

1. Introduction

2. Alignment Model for Random Misalignment Angle

3. Mstasckf Method

3.1. Sage–Husa Noise Estimator

3.2. SCKF with Sage–Husa Noise Estimator

3.3. Multiple Fading Factor Calculation

3.4. MSTASCKF Update

4. Semi-Physical Simulation Results and Discussion

4.1. Vibration Experiment

4.2. Simulation Condition

4.3. Simulation Results under Vibration Noise

4.4. Discussion

5. Conclusions

Author Contributions

Funding

Institutional Review Board Statement

Informed Consent Statement

Data Availability Statement

Conflicts of Interest

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI