Sage Journals: Discover world-class research

Abstract

It is critical to measure the roll angle of a spinning missile quickly and accurately. Magnetometers are commonly used to implement these measurements. At present, the estimation of roll angle parameters is usually performed with the unscented Kalman filter algorithm. In this paper, the two-step adaptive augmented unscented Kalman filter algorithm is proposed to calibrate the biaxial magnetometer and circuit measurements quickly, which allows accurate estimates of the missile roll angle. Unlike the existing algorithms, the state vector of the algorithm is based on the missile roll angle parameters and the error factors caused by the magnetometer and the measurement circuit errors. Next, a two-step fast fitting algorithm is used to fit the initial value. After satisfying the stop rule, the state vector of the filter is configured to estimate the roll angle parameters and the calibration parameters. This method is evaluated by running numerous simulations. In the experiment, the algorithm completes the calibration of the magnetometer and the measurement circuit 1 s after the missile launches, with a sampling rate of 1 ms and an output roll attitude angle with a 0.0015 rad precision. The conventional unscented Kalman filter algorithm requires more time to achieve such a high accuracy. The simulation results demonstrate that the proposed two-step augmented unscented Kalman filter outperforms the conventional unscented Kalman filter in its estimation accuracy and convergence characteristics.

Keywords

Magnetometer roll angle spinning missile two-step adaptive unscented Kalman filter

I. Introduction

To achieve an accurate approach of a target after launch, the flight path of a rotating missile can be changed by applying force (e.g. caused by gas jets or steering gear, etc.) to the missile body. However, both gas jets and the deflection of the steering gear must be applied at the right moment. Otherwise, a spinning missile will not be able to make the correct adjustment. Thus, the onboard computer is required to calculate the missile’s angular displacement and angular velocity in a very short time.¹

The magnetometer is widely used because of its low cost, high reliability, and other characteristics. However, these sensors are not error free because of their biases, scaling errors, and non-orthogonality, which limit the accuracy of the roll angle measurements.² Therefore, the magnetometer must be calibrated before correctly measuring the missile roll angle. Moreover, in order to allow the missile to adjust its altitude immediately after launch, the calibration process must be completed as quickly as possible.

The calibration can be realized using different mathematical approaches to estimate the parameters both offline and online.³ At present, researchers have proposed various approaches to reducing the measurement errors of the magnetometers. Dong et al.⁴ proposed the reason for affecting the accuracy of the magnetometer measurement. Soken⁵ proposed an estimation algorithm based on a reconfigurable unscented Kalman filter (UKF). This algorithm reduces the computational cost, but does not speed up the solution. Da Forno et al.⁶ used both the extended Kalman filter (EKF) and UKF algorithms for the online estimation of magnetometers. This work considers that the UKF has certain advantages over the EKF. Zhang et al.⁷ presented a perfect offline estimation algorithm. In the study of Hu and Pang,⁸ a genetic algorithm is proposed to calibrate the magnetometer error, and wavelets are proposed for noise cancellation. These two algorithms are considered to be effective for the calibration of the magnetometer. Pang et al.⁹ used a non-magnetic rotating platform and proton magnetometers to calibrate the magnetometers based on a differential algorithm. GH Elkaim and CC Foster proposed the use of an ellipse and ellipsoid fitting algorithm to calibrate the magnetometer. That algorithm uses the least square fitting result as the final result, but the precision can be improved further.¹⁰

This paper aims to introduce a novel adaptation strategy for an UKF algorithm that is used for the rapid estimation of the roll angle of the spinning missile. The initial values of the filter are known to affect the convergence speeds and accuracies of the filters. However, due to the uncertainty of the information to be measured, the value of the initial value cannot be accurately obtained with the traditional UKF. To solve the above problems, this paper presents the augmented unscented Kalman filter (AAUKF). This algorithm solves some of the problems in previous papers. First, the process of calibrating magnetometers offline is too complex to be implemented in large quantities. Second, using the UKF algorithm for online calibration, the convergence of the results is slow. Third, fitting the data using an ellipsoid or ellipse, the accuracy of the results is limited by the amount of data. Compared with the algorithm proposed in the previous paper,^5–7 the fast convergence of the fitting algorithm and the real-time solution of the UKF algorithm are combined. Without affecting the accuracy of the solution, the solution speed has been greatly improved. It should be noted that in the numerical simulation experiments in this paper, the consideration of noise is only considered as Gaussian white noise. For this system, the noise disturbance includes $C_{s i e}$ and $\vec{H_{h d}^{b}}$ . $C_{s i e}$ represents the sum of soft iron errors that are fixed to the body frame that serve to scale the sensor outputs. $\vec{H_{h d}^{b}}$ represents the sum of all permanent magnetic fields and shifts the sensor by a constant amount. In Zhu and colleagues,^11–15 there is a good description and solution to the disturbance of noise. We will perform further research in the following work.

II. Angle Measurement Model Based on Magnetometer

When using the biaxial magnetometer to measure the roll angle of a rolling missile, a circuit system equipped with a magnetometer is installed on the cross section of the missile.¹⁶ Through the conversion of the relationships among the angles, we can obtain the roll attitude angle of the missile.

Figure 1 indicates the angle measurement model based on the magnetometer. The red block represents the circuit equipped with a biaxial magnetometer. $O - x y_{n} z$ is the measurement coordinate system^17,18 and $\vec{O X}$ is the axis vector of the missile. $\vec{O y}$ and $\vec{O z}$ are the two sensitive axes of the biaxial magnetometer, respectively. There is a non-orthogonal error angle $δ$ of the sensitive axes between $\vec{O y}$ and $\vec{O y_{n}}$ . $\vec{O Z_{m}}$ is the missile zero reference axis and $\vec{O Z_{r}}$ is the zero reference axis of the inertial space. $\vec{H}$ is the vector physical quantity to be measured. Additionally $a_{H}$ is the azimuthal angle for the $\vec{H}$ relative to the $\vec{O Z_{r}}$ axis. $γ$ is the attitude of the missile relative to the inertial space. $φ_{0}$ is the mounting angle of the sensor. $φ$ is the roll angle of the missile. When the missile rotates, the pulsating direct current (DC) signals and $z$ can be measured and converted into digital signals, which are then received by the data acquisition system.

Figure 1.

Angle measurement model based on the magnetometer.

The measurement values of the magnetometer channels $Y$ and $Z$ are digitized and defined as $y$ and $z$ as follows

{\begin{matrix} y = Y_{0} + k A \sin (φ - φ_{0} - δ) \\ z = Z_{0} + A \cos (φ - φ_{0}) \end{matrix}

(1)

α_{H} = φ + γ

(2)

where $Y_{0}$ and $Z_{0}$ are the DC biases of the two channels. Here $A$ is the half-peak-to-peak value, which is an output signal of the channels $Y$ and $Z$ . $k$ is the sensitivity-proportional coefficient of the channels $Y$ and $Z$ .

Defining $φ_{s} = φ - φ_{0}$ , we can obtain

φ_{s} = \arctan (\frac{(y - Y_{0})}{k (z - Z_{0}) \cos δ} - \tan δ)

(3)

Finally, the missile attitude angle $γ$ can be obtained from the missile roll angle $φ_{s}$

γ = α_{H} - φ_{0} - φ_{s}

(4)

The estimation of the parameters via the filtering method is a substantial process of transforming the parameter estimation problem into state estimations, which can be achieved by customizing unknown parameters to be additional state variables, thus increasing the variables of the system state vector.¹⁹ Specifically, such a method can be effectively applied to nonlinear problems instead of linearizing the model. This nonlinearity manifests itself in the state products.²⁰

The system equation can be represented in its discrete forms, which is shown in formula (5), as can the measurement equation, which is shown in formula (6)

X_{k + 1} = f (X_{k}, u_{k}) + W_{k}

(5)

Z_{k + 1} = h (X_{k + 1}) + V_{k + 1}

(6)

where $X_{k + 1}$ is the state vector and the $Z_{k + 1}$ is the measurement vector. Here, $f$ is the linear vector function and $h$ is the nonlinear vector function. $W_{k}$ is the noise vector of the system equation, and $V_{k}$ is the noise vector of the measurement equation, while $u_{k}$ is the deterministic control value. At the same time, $W_{k}$ and $V_{k}$ are independent and meet the following conditions

E [W_{k}] = 0, Cov [W_{k}, W_{j}] = E [W_{k} W_{j}^{T}] = Q_{k} δ_{k j}

(7)

E [V_{k}] = 0, Cov [V_{k}, V_{j}] = E [V_{k} V_{j}^{T}] = R_{k} δ_{k j}

(8)

Cov [W_{k}, V_{j}] = E [W_{k} V_{j}^{T}] = 0

(9)

where $Q_{k}$ is the variance matrix of the system noise sequence and is assumed to be a nonnegative definite matrix; $R_{k}$ is the variance matrix of the measurement noise sequence and is assumed to be a positive definite matrix; and $δ_{k j}$ is the deterministic control value.²¹

The parameter vector estimated during the filtering process is $X_{k}$

X_{k} = {[\begin{matrix} A & Y_{0} & Z_{0} & k & δ & Φ & Φ^{'} & Φ^{″} \end{matrix}]}^{T}

(10)

The parameters $A, Y_{0}, Z_{0}, k, δ$ are the parameters of the reaction magnetometer and measuring circuit error characteristics, which are favorable for observing the convergence of the filter. $Y_{0}, Z_{0}, k, δ$ are the specific characteristics of the circuit system. Once the magnetometer and the measurement circuit are defined, the four parameters are determined to complete the calibration of the magnetometer and measurement circuit

To accurately describe the rolling motion of the missile, and considering the small changes in the acceleration of the missile flight, the missile rolling motion is approximately expressed as an accelerating motion with a slow acceleration

{(φ_{s})}_{i} \approx {(φ_{s T})}_{i} = Φ + Φ^{'} t_{i} + \frac{1}{2} Φ^{″} t_{i}^{2}

(11)

In formula (11), $Φ, Φ^{'}, Φ^{″}$ are the roll angle parameters that must be fitted, which indicate the angular displacement, angular velocity, and angular acceleration.

III. Two-Step Adaptive Adjustment of the Initial Value of the Filter

Within a missile attitude measurement algorithm, especially for short-range missiles, the missile’s attitude angle must be solved for within a short period of time after the launch. Otherwise, the missile will not be able to adjust its trajectory based on the target position and the current attitude. However, the initial data acquired by the magnetometer are mixed with considerable and unknown noise and outliers.²² Directly using these initial data as the initial value of the filter will cause the filtering process to slow down the convergence rate or even be to not converge.²³ Therefore, a relatively accurate initial value of the state vector can effectively speed up the convergence rate of the filter. However, the initial value of the state vector is not determined due to the different sensors, different installation methods, and different missiles, which cause the state vector to have different real values. This difference requires a faster calculation method to obtain the closest possible value to the exact initial value of the state vector in a short time.

A. First step: adaptive processing of the calibration parameters of magnetometers

In the state vector $X_{k} = {[\begin{matrix} A & Y_{0} & Z_{0} & k & δ & Φ & Φ^{'} & Φ^{″} \end{matrix}]}^{T},$ $Y_{0}, Z_{0}, k, δ$ are the unique parameters of the magnetometer and the operational amplifying circuit, which once determined will not be easily changed. In addition, the approximate values of these parameters can be calculated quickly from the digital quantities obtained from the biaxial magnetometer. When the biaxial magnetometer measures the rotational attitude of the spinning missile, two sensitive axes rotate with the missile along the direction perpendicular to the missile axis.²⁴ Thus, the trajectory data acquired by the two axes are two trigonometric functions with a difference of $θ$ . Thus, $θ$ is the angle between the two sensitive axes.

The trajectory equation measured by the Y and Z axes of the biaxial magnetometer is

{\begin{cases} y_{i} = Y_{0} + k A_{i} \sin (φ (t_{i}) + θ) \\ z_{i} = Z_{0} + A_{i} \sin (φ (t_{i})) \\ θ = δ + π / 2 \end{cases}

(12)

where $y_{i}, z_{i}$ are the output values of the magnetometer at time $t_{i}$ . The subscript $i$ indicates the ith sampling period corresponding to the data.

Considering the calculation quantities and speeds, the value of the non-orthogonal error angle is generally not more than 0.025% of the full angle. This value will not affect the overall convergence rate of the UKF, as opposed to the other estimated parameters. Thus, we assume $δ_{e} = 0$ in the initial value of the vector.

The simplified formula (12) is as follows

{y_{i}}^{2} - 2 Y_{0} y_{i} + k^{2} {z_{i}}^{2} - 2 k^{2} Z_{0} z_{i} - k^{2} A^{2} + Y_{0}^{2} + k^{2} Z_{0}^{2} = 0

(13)

Formula (13) is the final trajectory equation of the magnetic vector signal. Thus, the trajectory of the magnetic vector is an ellipse.

The standard binomial of an ellipse is

y^{2} + a y + b z^{2} + c z + d = 0

(14)

Comparing formulas (13) and (14), the following relation holds

{\begin{cases} - 2 Y_{0} = a \\ k^{2} = b \\ - 2 k^{2} Z_{0} = c \\ - k^{2} A^{2} + Y_{0}^{2} + k^{2} Z_{0}^{2} = d \end{cases}

(15)

The least squares method is adopted to solve for the ellipse parameters $a, b, c, d$ . Suppose that the fitting date sequence is $t_{i}, y_{i}, z_{i}$ .

Let the residual formula be

ε = \sum_{i = 0}^{n - 1} {[{y_{i}}^{2} + a y_{i} + b {z_{i}}^{2} + c z_{i} + d]}^{2}

(16)

If there is

\frac{\partial ε}{\partial a} = \frac{\partial ε}{\partial b} = \frac{\partial ε}{\partial c} = \frac{\partial ε}{\partial d} = 0

(17)

The elliptical parameters $a, b, c, d$ can be solved by directly using the linear system of equations from formula (21), followed by the calculation of the system parameters $A_{i}, Y_{0}, Z_{0} k$ .

In the first step, the adaptive process of the parameters can speed up the convergence rate of the UKF, but this process also takes time. Moreover, with regard to the fitting that is taking place, the improvement of the fitting accuracy will not be proportional to the consumption of the fitting time. Therefore, the correct judgement condition is needed to terminate this step.

In the time period of $t r {t_{0}, t_{1}, \dots, t_{i}}$ , the state vector ${(X_{f})}_{i} = [\begin{matrix} A_{i} & {(Y_{0})}_{i} & {(Z_{0})}_{i} & k_{i} \end{matrix}]$ that corresponds to the fitting parameters is obtained. Here, the difference between the fitting values is

{(Λ_{f})}_{i} = {(X_{f})}_{i} - {(X_{f})}_{i - 1}

(18)

The covariance of the fitting parameters is

\begin{matrix} {(τ_{f})}_{i} = {(Λ_{f})}_{i} {(Λ_{f})}_{i}^{T} \\ = ({(X_{f})}_{i} - {(X_{f})}_{i - 1}) {({(X_{f})}_{i} - {(X_{f})}_{i - 1})}^{T} \end{matrix}

(19)

When ${(τ_{f})}_{i} < {(τ_{f})}_{\min}$ , each parameter in the state vector of the fitted parameter must be optimized to a certain degree, where ${(τ_{f})}_{\min}$ is a small pre-set value. Thus, the optimal result $A_{e}, {(Y_{0})}_{e}, {(Z_{0})}_{e}, k_{e}$ of the state parameter adaptation is obtained in the first step.

B. Second step: adaptive process of the parameters of rolling motion

In the first step, the calibration parameters of the magnetometer and the measuring circuit are obtained.²⁵ According to these parameters and the measurement sequence $y_{i}, z_{i}$ of the biaxial magnetometer, the roll angle sequence ${(φ_{s})}_{i}$ is obtained for $i = 0, n - 1$

{(φ_{s})}_{i} = \arctan (\frac{y_{i} - {(Y_{0})}_{e}}{k_{e} (z_{i} - {(Z_{0})}_{e})})

(20)

From equation (12), we can see that the missile roll angle approximation is

{(φ_{s T})}_{i} = Φ + Φ^{'} t_{i} + \frac{1}{2} Φ^{″} {t_{i}}^{2}

(21)

Then, the approximation is fit using the least square method. First, define the residuals $ε$ as

ε = \sum_{i = 0}^{n - 1} {({(φ_{s})}_{i} - {(φ_{s T})}_{i})}^{2}

(22)

Then, there is an optimal value of $Φ, Φ^{'}, Φ^{″}$ wherein the residual $ε$ is set to its minimum value

\frac{\partial ε}{\partial Φ} = \frac{\partial ε}{\partial Φ^{'}} = \frac{\partial ε}{\partial Φ^{″}} = 0

(23)

The roll angle $Φ, Φ^{'}, Φ^{″}$ can be solved for directly using the linear system of equations from formula (23). As in the first step, the precision of the parameter fitting in the second step is not proportional to the time of the fitting after a certain period of time. Therefore, the second step also requires a similar stop strategy.

In the period of time $t r {t_{0}, t_{1}, \dots, t_{i}}$ , the state vector ${(X_{s})}_{i} = [\begin{matrix} Φ_{i} & {Φ^{'}}_{i} & {Φ^{″}}_{i} \end{matrix}]$ that corresponds to the fitting parameters is obtained. Here, the difference between each fitting values is

{(Λ_{s})}_{i} = {(X_{s})}_{i} - {(X_{s})}_{i - 1}

(24)

The covariance of the fitting parameters is

\begin{matrix} {(τ_{s})}_{i} = {(Λ_{s})}_{i} {(Λ_{s})}_{i}^{T} \\ = ({(X_{s})}_{i} - {(X_{s})}_{i - 1}) {({(X_{s})}_{i} - {(X_{s})}_{i - 1})}^{T} \end{matrix}

(25)

When ${(τ_{s})}_{i} < {(τ_{s})}_{\min}$ , each parameter in the state vector of the fitted parameter must be optimized to a certain degree, where ${(τ_{s})}_{\min}$ is a small pre-set value. Thus, the optimal result $Φ_{e}, {Φ^{'}}_{e}, {Φ^{″}}_{e}$ of the state parameter adaptation is obtained in the second step.

After the two-step adaptive adjustment of the state parameter, the optimal initial value $E (X) = {[\begin{matrix} A_{e} & {(Y_{0})}_{e} & {(Z_{0})}_{e} & k_{e} & δ_{e} & Φ_{e} & {Φ^{'}}_{e} & {Φ^{″}}_{e} \end{matrix}]}^{T}$ of the enhanced Kalman state vector is obtained.

IV. Augmented Unscented Kalman Filtering for Additive Process Noise

Both the UKF and standard Kalman filter belong to the linear minimum variance estimation method that is based on the model.²⁶ The difference between them is the method of obtaining the optimal gain matrix. The UKF determines the optimal gain matrix based on the estimated and measured covariance matrices.²⁷ Then, the covariance matrix is calculated from the doubled $σ$ sample points, which are determined according to the system and measurement equations. In the process of calculating the optimal gain matrix, the UKF does not require any additional conditions for the system equations and the measurement equations. Therefore, the UKF is superior to the standard Kalman filter under nonlinear conditions, and thus, it overcomes problems inherent to the EKF, which has a considerable error and has difficulty calculating the Jacobian matrix in the process of nonlinearity.²⁸ Nevertheless, the UKF is very sensitive to errors in the model, which hinders its application. Therefore, this paper proposes a new UKF algorithm: the two-step AAUKF, which allows for an additive noise algorithm that can predict the model error in the filter process to overcome the original model shortcomings. The driving noise of the system is augmented to increase the input information of the system. The algorithm effectively improves the model resistance to the error interference and the robustness of the filtering process effectively in comparison with the traditional UKF.

The system state is extended by augmenting the dimension of the state vector in the UKF model. Because of the driving influence of noise on the system motion model mentioned above, the driving noise is considered in the system state. Therefore, the augmented state vector and covariance matrix can be depicted in formulas (26) and (27)

X_{k}^{a x} = {[\begin{matrix} X_{k}^{T} & W_{k}^{T} & V_{k}^{T} \end{matrix}]}^{T}

(26)

P_{k}^{a x} = [\begin{matrix} P_{k} & 0 & 0 \\ 0 & Q_{k} & 0 \\ 0 & 0 & R_{k} \end{matrix}]

(27)

where the superscript ax denotes the augmented state vector, the dimension of $X_{k}^{a x}$ is $L = n + r + m$ , the dimension of $Q_{k}$ is $r$ , and $m$ is the dimension of $R_{k}$ .

The state vector and the covariance matrix of the filter are initialized as follows

X_{0} = E [X]

(28)

p_{0} = E {[(X - X_{0}) (X - X_{0})]}^{T}

(29)

where $E [(X - X_{0}) {(X - X_{0})}^{T}]$ is the expectation operator, $(X - X_{0})$ is the estimated error of the initial value, $X$ is the true value of the position, and $X_{0}$ is the estimated initial value. In the following, we use the notation “tilde” (~) and “hat” (^) to denote the predicted and corrected variables, respectively.

The initial estimation of the extended state vectors is as follows

{\hat{X}}_{0}^{a x} = E {[\begin{matrix} X_{0}^{T} & 0_{n \times 1}^{T} & 0_{m \times 1}^{T} \end{matrix}]}^{T}

(30)

The initial estimation of the extended covariance matrix is as follows

{\hat{P}}_{0}^{a x} = E [\begin{matrix} {\hat{P}}_{0} & 0 & 0 \\ 0 & Q_{0} & 0 \\ 0 & 0 & R_{0} \end{matrix}]

(31)

The unscented transformation is a type of efficient method for the estimation of the mean and covariance of a random variable during a nonlinear transformation.²⁹ The $σ$ sample point of the extended state vector is constructed according to ${\hat{X}}_{k}^{a x}$ and ${\hat{P}}_{k}^{a x}$ .

For the $σ$ point number of $L$ , through a nonlinear transformation (polar coordinate measured), the cloud measurement transformation points are obtained

χ_{k}^{a x, 0} = {\hat{X}}_{k}^{a x}

(32)

χ_{k}^{a x, i} = {\hat{X}}_{k}^{a x} + {(\sqrt{(L + λ) {\hat{P}}_{k}^{a x}})}_{i} i = 1, \dots, L

(33)

χ_{k}^{a x, i} = {\hat{X}}_{k}^{a x} - {(\sqrt{(L + λ) {\hat{P}}_{k}^{a x}})}_{i} i = L + 1, \dots, 2 L

(34)

The weights W are calculated as follows

{\begin{cases} W_{0}^{m} = \frac{λ}{L + λ} \\ W_{0}^{c} = \frac{λ}{L + λ} + (1 - α^{2} + β) \\ W_{i}^{m} = W_{i}^{c} = \frac{1}{2 (L + λ)} \end{cases}

(35)

{\begin{cases} λ = α^{2} (L + κ) - L \\ L = n^{a x} \end{cases}

(36)

where the subscripts 0 and i are the corresponding estimated states, and the superscripts m and c represent the weights of the mean and covariance, respectively. Here, $α$ is a very small positive value with a range of $10^{- 4} \leq α \leq 1$ (e.g. $α = 10^{- 3}$ ); the value of $β$ is related to the distribution of $X$ ; for a normal distribution, $β = 2$ is the optimal value;³⁰ $κ$ is the scaling parameter, which is used for fine-tuning, although the heuristic choice is to set that parameter as $κ + n = 3$ ; and $n^{a x}$ is the total number of states to be estimated.

The time propagation equation is as follows

γ_{X, k + 1 | k}^{a x, i} = f (χ_{X, k}^{a x, i}, χ_{W, k}^{a x, i}) i = 0, 1, 2, \dots, 2 L

(37)

γ_{Z, k + 1 | k}^{a x, i} = h (γ_{X, k + 1 | k}^{a x, i}, χ_{V, k}^{a x, i}) i = 0, 1, 2, \dots, 2 L

(38)

The state estimation equation is as follows

{\tilde{X}}_{k + 1 | k} = \sum_{i = 0}^{2 L} W_{i}^{m} γ_{X, k + 1 | k}^{a x, i}

(39)

{\tilde{P}}_{X, k + 1 | k} = \sum_{i = 0}^{2 L} W_{i}^{c} (γ_{X, k + 1 | k}^{a x, i} - {\tilde{X}}_{k + 1 | k}) {(γ_{X, k + 1 | k}^{a x, i} - {\tilde{X}}_{k + 1 | k})}^{T} + Q_{k}

(40)

The measurement estimation equation is as follows

{\tilde{Z}}_{k + 1 | k} = \sum_{i = 0}^{2 L} W_{i}^{m} γ_{Z, k + 1 | k}^{a x, i}

(41)

{\tilde{P}}_{Z, k + 1 | k} = \sum_{i = 0}^{2 L} W_{i}^{c} (γ_{Z, k + 1 | k}^{a x, i} - {\tilde{Z}}_{k + 1 | k}) {(γ_{Z, k + 1 | k}^{a x, i} - {\tilde{Z}}_{k + 1 | k})}^{T} + R_{k}

(42)

The cross-covariance matrix is as follows

{\tilde{P}}_{X Z, k + 1 | k} = \sum_{i = 0}^{2 L} W_{i}^{c} [γ_{X, k + 1 | k}^{a x, i} - {\tilde{X}}_{k + 1 | k}] {[γ_{Z, k + 1 | k}^{a x, i} - {\tilde{Z}}_{k + 1 | k}]}^{T}

(43)

Calculate the gain of the UKF

K_{k + 1} = {\tilde{P}}_{X Z, k + 1 | k} \cdot {\tilde{P}}_{Z, k + 1 | k}^{- 1}

(44)

Finally, the updated states and covariance matrix are determined as follows

{\hat{X}}_{k + 1} = {\tilde{X}}_{k + 1 | k} + K_{k + 1} (Z_{k + 1} - {\tilde{Z}}_{k + 1 | k})

(45)

{\hat{P}}_{k + 1} = {\tilde{P}}_{X, k + 1 | k} - K_{k + 1} \cdot {\tilde{P}}_{Z, k + 1 | k} \cdot {K_{k + 1}}^{T}

(46)

where ${\hat{X}}_{k + 1}$ is the estimated state vector and ${\hat{P}}_{k + 1}$ is the estimated covariance matrix.

V. Experimental Verification

A. Simulation of the algorithm

The simulation is used to model the rolling motion of a spinning missile, the measurement of the magnetometer, and the measuring circuit because the actual missile roll angle and the magnetometer and the measurement circuit error parameters cannot be accurately known. In the simulation process, all of the magnetometer and the measurement circuit error values are manually predefined. Only in this way can the final processing result be compared with the correct result to evaluate the merits of the algorithm.³¹

According to the actual case of a spinning missile moving from its launch to a stable flight, the rolling angular displacement, velocity, and acceleration of the missile as well as the change in the component amplitude of the geomagnetic field vector on the sensitive axis are simulated. In this simulation, we set the Y-axis baseline as $Y_{0} = 0.476 F S$ , with the nominal value deviation of $2.4 %$ ; we set the Z-axis baseline to $Z_{0} = 0.507 F S$ , with the deviation from the nominal value being $0.6 %$ . The scale factor is $k = 1.07$ , with the nominal value of the deviation being $7 %$ ; the non-orthogonal error angle is set to 6°, with the nominal value of the deviation being $1.67 %$ . Based on formula (6), increasing the measurement noise by $1.5 %$ allows for the measurement value with the biaxial magnetometer sampling interval of 1 ms. The evaluation is implemented by comparing the errors between the estimated parameters and the predefined parameters using two different calibration approaches: the two-step AAUKF and traditional UKF.

In Figure 2(a)–(d) denotes the estimation of the magnetometer calibration parameters using the AAUKF algorithm and the UKF algorithm. The comparison indicates that when the AAUKF algorithm is used, the magnetometer amplitude, scaling factor, Y-axis, and Z-axis baselines can be obtained in 0.12, 0.13, 0.16, and 0.12 s, respectively. Their estimated accuracy reached 0.2% Full Scale (FS), 0.3%, 0.2% FS, and 0.4% FS, respectively. However, to obtain the same precision results using the UKF algorithm, then the amount of time required is 40, 30, 55, and 48 s, respectively. In both algorithms, the baseline estimation time for the Y-axis is the longest—0.16 and 55 s for the AAUKF and UKF, respectively. This finding shows that the calculation speed of the AAUKF algorithm is almost 300 times faster than that of the UKF algorithm on the magnetometer calibration parameters.

Figure 2.

Error of the parameter estimation. (a) Error of the amplitude estimation; (b) error of the scale factor estimation; (c) error of the Y-axis baseline estimation; (d) error of the Z-axis baseline estimation; (e) error of the missile roll angular displacement estimation; (f) error of the missile roll angular velocity estimation; (g) error of the missile roll angular acceleration estimation.

In Figure 2(e)–(g) , respectively, show that the angular displacement, roll angular velocity, and roll angular acceleration of the spinning missile can be obtained at 0.13, 0.4, and 0.45 s using the AAUKF algorithm. Their estimation accuracy is 0.005 rad, 0.1 rad/s, and 1.5 rad/s², respectively. However, if we use the UKF algorithm and want to obtain the same accuracy, the results will take approximately 41, 38, and 40 s. Again, this time is approximately 100 times longer than that when the AAUKF algorithm is used. This result means that in estimating the roll angle information of the missile, the AAUKF algorithm can correct the magnetometer and the measurement circuit in less time than the UKF algorithm to obtain accurate roll angle parameters, which allows the missile state to be controlled as soon as possible, and thus allows accurate focus on the target.

B. Semi-physical simulation experiments

To verify the validity of the algorithm on the magnetometer, the magnetic shielding device of the Helmholtz coils and simulative alternating magnetic field generator are used for the simulation experiments. As shown in Figure 3 , the measurement components of the magnetometer are placed in the simulative alternating magnetic field of the Helmholtz coils.³² Instead of magnetometer rotation, we set the signal generator to output the Alternating Current to generate a customized rotating magnetic field.

Figure 3.

Experimental platform of the simulative alternating magnetic field: (a) the simulative alternating magnetic field generator and (b) the experimental system.

In this experiment, we used the same magnetometer, signal processing circuit, and onboard computer. However, there are two types of calculation programs, the AAUKF algorithm and UKF algorithm, in the onboard computer.

The biaxial magnetometer obtains the magnetic field components of the Y and Z sensitive axes according to the changes in the magnetic field, and it corrects the magnetometer and the measuring circuit through the AAUKF algorithm and the UKF algorithm in the onboard computer, it thus achieves the missile’s roll angular displacement, roll angular velocity and roll angular acceleration measurements. The obtained parameters are compared with the angular displacement, angular velocity, and angular acceleration of the rotating magnetic field to obtain the estimation error of the filtering algorithm.

Figure 4(a)–(c) shows that the fitting process of the initial values of the angular displacement, angular velocity, and angular acceleration is completed in approximately 0.1 s when using the AAUKF algorithm. In 0.3–0.4 s, the estimation of the roll motion parameters is completed. A comparison is made with the AAUKF algorithm. The convergence of the estimation process of the roll motion parameters is very slow when using the UKF algorithm, which is approximately 100 times the amount of time for using the AAUKF algorithm.

Figure 4.

Error of the parameter estimation: (a) error of the roll angular displacement estimation, (b) error of the roll angular velocity estimation, and (c) error of the roll angular acceleration estimation.

The result of this measurement is similar to that of the numerical simulation. The slightly different convergence time is due to the different true values of the calibration parameters of the magnetometer and the signal processing circuit. Measurement results show that the AAUKF algorithm has better results in solving similar problems. During the measurement, the true value of the calibration parameters of the magnetometer is unknown, so the algorithm cannot be evaluated by the error of the filtering result. However, in the simulation process, the calibration parameters of the magnetometer are artificially set and have a certain true values. The effectiveness of the algorithm can be confirmed by the filtering error of the calibration parameters.

VI. Conclusion

In this study, a two-step AAUKF algorithm is proposed to quickly calibrate the magnetometer and estimate the roll angle of a missile. The algorithm consists of three steps. The first step uses an elliptical equation to fit the correction parameters of the magnetometer and the measuring circuit. When the correction parameters satisfy the stop rule, the second step begins. Using the calculation results and the measurement of the magnetometer in the first step, the roll angle displacement, angular velocity, and angular acceleration of the missile are fitted. Similarly, after satisfying the fitting stop rule, the third step begins. Eight state parameters of the missile roll angle that are measured using a magnetometer are estimated by the Kalman algorithm. In contrast to the existing algorithms, the proposed algorithm can estimate the characteristic parameters of the magnetometer and the measurement circuit; at the same time, this new algorithm can monitor the validity of the missile roll angle parameter to ensure the correct convergence of the estimated parameters. The algorithm performs a fast processing of the initial value of the filter to ensure that the missile can obtain its roll attitude in a short period after its launch to enable the missile to be controlled quickly and effectively. Simulation and experimental results show that when using a dual-axis magnetometer to measure the missile roll angle, the algorithm can complete the corrections of the magnetometer and measurement circuit in approximately 1 s and provide the angle of the roll attitude with an accuracy of not less than 0.0015 rad and that of the angular velocity of the roll with an accuracy of no less than 0.1 rad/s. In addition, the proposed scheme and algorithm are equally applicable to rapid and accurate measurements of other carrier roll angles.

Footnotes

Funding

The author(s) received no financial support for the research, authorship, and/or publication of this article.

References

Han

. A missile-borne angular velocity sensor based on triaxial electromagnetic induction coils. Sensors 2016; 16(10): 1625–37.

Huang

Chou

Zhang

. A high-performance angular speed measurement method based on adaptive hysteresis switching techniques. Mechanical Systems and Signal Processing 2015; 65(10): 282–95.

Santhosh

Roy

. A practically validated adaptive calibration technique using optimized artificial neural network for level measurement by capacitance level sensor. Measurement and Control 2015; 48(7): 217–24.

Dong

Chou

Fang

. Magnetometer calibration algorithm based on ellipsoid constraint. High Technology Letters 2015; 21(4): 407–13.

Soken

. UKF-based reconfigurable attitude parameters estimation and magnetometer calibration. IEEE Transactions on Aerospace and Electronic Systems 2010; 48(3): 2614–27.

Da Forno

Reali

Bristot

Debei

. Autonomous navigation of MegSat1: Attitude, sensor bias and scale factor estimation by EKF and magnetometer-only measurement. In Proceedings of the 22nd AIAA international communications satellite systems conference and exhibit, Monterey, CA, 9–12 May 2004.

Zhang

Z-Q

Meng

X-L

J-K

. Quaternion-based Kalman filter with vector selection for accurate orientation tracking. IEEE Transactions on Instrumentation and Measurement 2012; 61(10): 2817–24.

Pang

. Magnetometer calibration improvement using wavelet and genetic algorithm. IEEJ Transactions on Electrical and Electronic Engineering 2016; 11(2): 130–137.

Pang

Zhu

Pan

Zhang

. The component compensation of geomagnetic field vector measurement system. Journal of Magnetism and Magnetic Materials 2015; 381(10): 390–5.

10.

Foster

Elkaim

. Extension of a two-step calibration methodology to include nonorthogonal sensor axes. IEEE Transactions on Aerospace and Electronic Systems 2008; 44(3): 1070–8.

11.

Wang

Zhu

. Finite-time stabilization of high-order stochastic nonlinear systems in strict-feedback form. Automatica 2015; 54: 284–91.

12.

Wang

Zhu

. Global stabilization of stochastic nonlinear systems via C¹ and C^∞ controllers. IEEE Transactions on Automatic Control 2017; 62(11): 5880–7.

13.

Zhu

Zhang

. pth moment exponential stabilisation of hybrid stochastic differential equations by feedback controls based on discrete-time state observations with a time delay. IET Control Theory & Applications 2017; 11(12): 1992–2003.

14.

Wang

Zhu

. Stability analysis of Markov switched stochastic differential equations with both stable and unstable subsystems. Systems & Control Letters 2017; 105: 55–61.

15.

Zhu

. Razumikhin-type theorem for stochastic functional differential equations with Levy noise and Markov switching. International Journal of Control 2017; 9(8): 1703–12.

16.

Tadej

Samo

Janez

Marko

. Magnetometer calibration using Kalman filter covariance matrix for online estimation of magnetic field orientation. IEEE Transactions on Instrumentation and Measurement 2014; 63(8): 2013–20.

17.

Liu

Zhang

. Novel calibration algorithm for a three-axis strapdown magnetometer. Sensors 2014; 14(8): 8485–504.

18.

Chang

. Fast two-position initial alignment for SINS using velocity plus angular rate measurements. Advances in Space Research 2015; 56(7): 1331–42.

19.

Dunik

Simandl

Straka

. Unscented Kalman filter: Aspects and adaptive setting of scaling parameter. IEEE Transactions on Automatic Control 2012; 57(9): 2411–16.

20.

Lei

Xueshuai

Yongping

. Attitude determination method by fusing single antenna GPS and low cost MEMS sensors using intelligent Kalman filter algorithm. Mathematical Problems in Engineering 2017; 2017: 4517673.

21.

Zhao

Gao

Zhang

Sun

. Robust predictive augmented unscented Kalman filter. International Journal of Control, Automation, and Systems 2014; 12(5): 996–1004.

22.

Chen

Niu

. The research of geomagnetic measuring model of projectile roll attitude. Journal of Projectiles, Rockets, Missiles and Guidance 2016; 6(3): 35–42.

23.

Han

Wang

. Extended Kalman filter-based gyroscope-aided magnetometer calibration for consumer electronic devices. Sensors 2017; 17(1): 1625–37.

24.

Sakamoto

Pacheco

Kitano

Tittmann

. Geometrical parameter analysis of a high-sensitivity fiber optic angular displacement sensor. Applied Optics 2014; 53(9): 8436–43.

25.

Yan

Chen

Yang

. Application of Kalman filter restraining outliers in estimation of rockets impact point. Journal of Projectiles, Rockets, Missiles and Guidance 2016; 36(3): 94–8.

26.

Yalcin

Balcilar

Koyun

. Design and implementation of Kalman filter for fuel pipe damping test machine working based on variable frequency vibration principle. Measurement and Control 2017; 50(9–10): 199–205.

27.

Miao

Zhang

Fang

Lei

. Design of orientation estimation system by inertial and magnetometers. Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering 2014; 228(7): 1105–13.

28.

Wang

. An improved PDR/magnetometer/floor map integration algorithm for ubiquitous positioning using the adaptive unscented Kalman filter. ISPRS International Journal of Geo-Information 2015; 4(10): 2638–59.

29.

Jwo

Chung

. Fuzzy adaptive unscented Kalman filter for ultra-tight GPS/INS integration. In International symposium on computational intelligence and design, Hangzhou, China, 29–31 October 2010, pp.229–35. New York: IEEE.

30.

Song

. Adaptive unscented Kalman filter for estimation of modelling errors for helicopter. In IEEE international conference on robotics and biomimetics, Guilin, China, 19–23 December 2009, pp.2463–7. New York: IEEE.

31.

Hajiyev

. Stopping rules formation and faults detection in parametric identification problems. Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering 2001; 215(4): 357–64.

32.

Yaghobi

. Out-of-step protection of generator using analysis of angular velocity and acceleration data measured from magnetic flux. Electric Power Systems Research 2016; 132(6): 9–21.

Two-Step Adaptive Augmented Unscented Kalman Filter for Roll Angles of Spinning Missiles Based on Magnetometer Measurements

Abstract

Keywords

I. Introduction

II. Angle Measurement Model Based on Magnetometer

III. Two-Step Adaptive Adjustment of the Initial Value of the Filter

A. First step: adaptive processing of the calibration parameters of magnetometers

B. Second step: adaptive process of the parameters of rolling motion

IV. Augmented Unscented Kalman Filtering for Additive Process Noise

V. Experimental Verification

A. Simulation of the algorithm

B. Semi-physical simulation experiments

VI. Conclusion

Footnotes

Funding

References