Application of Interactive Multiple Model Adaptive Five-Degree Cubature Kalman Algorithm Based on Fuzzy Logic in Target Tracking

Abstract

Aiming at the shortcomings of low precision, hysteresis, and poor robustness of the general interactive multimodel algorithm in the “snake-like” maneuver tracking of anti-ship missiles, an interactive multimodel adaptive five-degree cubature Kalman algorithm based on fuzzy logic (FLIMM5ACKF) is proposed. The algorithm mainly includes adaptive five-degree cubature Kalman algorithm (A5CKF) and fuzzy logic algorithm (FL). A5CKF uses the Sage–Husa noise estimation principle to propose a state error covariance adaptive five-degree cubature Kalman algorithm to improve the performance of state estimation. Then, the fuzzy logic algorithm (FL) is added to the model probability update module to control the model probability update module. Finally, by setting the same tracking model simulation analysis, the algorithm has better convergence speed, tracking effect and robustness than the interactive multimodel cubature Kalman algorithm (IMMCKF), the interactive multimodel five-degree cubature Kalman algorithm (IMM5CKF) and the interactive multimodel adaptive five-degree cubature Kalman (IMMA5CKF).

1. Introduction

With the development of modern military, anti-ship missiles have various forms of maneuvering, and the most typical one is the “snake-like” maneuver [1], because the maneuvering method of anti-ship missiles is relatively variable, a model can no longer meet the needs of anti-ship missile tracking. Therefore, the multiple model algorithm is proposed, for example, the multiple model (MM) algorithm proposed by Magill [2], the GPB algorithm proposed by Ackerson and Fu [3], and the interactive multimodel (IMM) algorithm proposed by Blom [4,5], among these algorithms, the most widely used is IMM algorithm [6,7,8], which takes into account the characteristics of the model and considers that only one model matches the motion state at a certain time, it reduces the error of a single model, improves the effect of target tracking. However, IMM algorithm also has some shortcomings, in order to solve the problems that the traditional IMM algorithm has, such as low filtering precision, slow convergence speed, and linear filtering in target tracking, this article will improve the IMM algorithm.

Nowadays, the research of nonlinear filtering algorithms has attracted more and more researchers’ attention. In the aspect of nonlinear filtering algorithm, the most widely used is the extended Kalman algorithm (EKF) [9,10], its core idea is to use linearization to approximate nonlinearization according to the first-order Taylor series expansion, however, the Jacobian matrix of nonlinear functions is difficult to find in many practical problems. In order to better improve the estimation performance, Julier et al. proposed the Unscented Kalman Filter (UKF) [11,12], which has better performance than EKF. In order to seek better filtering effects, the cubature Kalman algorithm (CKF) based on the third-order spherical-radial cubature rule was proposed by Haykin et al. [13,14]; it is different from UKF, CKF has a strict mathematical formula to prove, it has been shown that CKF has better performance than UKF when the dimension of the system is greater than three [15]. In order to pursue better performance of CKF, 5CKF was proposed in the literature [16,17,18], and the simulation shows that 5CKF performance is better than CKF. For pursuing a better estimation performance of 5CKF filtering algorithm, combined with the improvement of Sage–Husa noise estimation in the literature [19], and the combination with the corresponding filtering algorithm in the literature [20,21,22,23], the adaptive five-degree cubature Kalman algorithm based on error covariance estimation is proposed, and it is used as a filtering algorithm under the IMM framework.

In the IMM algorithm, the transition probability between models is performed by the Markov transition matrix. When the target is tracked at a certain time, it works by a specific model, and its probability is approximately 1, other approximations are 0. However, this method requires a certain amount of time, and there is hysteresis, resulting in a decrease in tracking effect. For solving the problem of slow convergence caused by hysteresis, a fuzzy logic (FL) is proposed [24], and combine the FL algorithm with the IMM algorithm, and the corresponding fuzzy rules are formulated, when the model is transformed, the FL algorithm is used to judge whether the probability of the model is 1 or 0 to accelerate the update of the probability, this will speed up the convergence rate, the related references also proves the effectiveness of the combined algorithm [20,25].

Although the IMMCKF algorithm and the IMM5CKF algorithm have already achieved good results [14,18], they still cannot effectively solve the problem of low filtering precision and slow convergence in the tracking process, therefore, an interactive multimodel adaptive five-degree cubature Kalman algorithm based on fuzzy logic is proposed in this paper, it uses the maximum likelihood function obtained by the improved A5CKF algorithm in the parallel filtering, updates the probability through the FL algorithm, and finally obtains the result through the output data fusion. Finally, by setting the same simulation model analysis, compared with IMMCKF [14], IMMA5CKF, and IMM5CKF [18], FLIMMA5CKF has better tracking effect and robustness, and the hysteresis is also improved. The rest of the sections are arranged as follows. Section 2 introduces fuzzy logic algorithms, Section 3 introduces adaptive five-degree cubature Kalman algorithm, Section 4 introduces FLIMMA5CKF algorithm, Section 5 performs missile dynamics modeling, Section 6 provides simulation experiments with the analysis of the results, and the conclusions are provided in Section 7.

2. Fuzzy Logic Algorithm

The fuzzy logic algorithm is used to update the model probability [24], so that the model probability is quickly converted to accelerate the response speed of the filtering system. Figure 1 shows the structure of the fuzzy logic algorithm in the model probability update module.

In this paper, we use two kinds of maneuver models of constant velocity and “snake-like” to simulate, so the input of the filter of the model probability update module is ${\mathrm{\Lambda }}_1$ and ${\mathrm{\Lambda }}_2$. Firstly, the model probabilities ${\widehat{\mu }}_1$ and ${\widehat{\mu }}_2$, corresponding to the two models, are calculated according to the IMM algorithm to calculate the model probability.

$${\overline{c}}_j=\sum p_{ji}{\mu }_{l-1}^i$$

(1)

$${\mu }_l^j=\frac{1}{c}{\mathrm{\Lambda }}_l^i{\overline{c}}_j$$

(2)

$$\mathrm{c}=\sum _{j=1}^r{\mathrm{\Lambda }}_l^i{\overline{c}}_j$$

(3)

It can be derived from Formulas (1)–(3):

$$\{\begin{array}{c}{\widehat{\mu }}_l^1=\frac{{\mathrm{\Lambda }}_l^1{\overline{c}}_1}{{\mathrm{\Lambda }}_l^1{\overline{c}}_1+{\mathrm{\Lambda }}_l^2{\overline{c}}_2}\\ {\widehat{\mu }}_l^2=\frac{{\mathrm{\Lambda }}_l^2{\overline{c}}_2}{{\mathrm{\Lambda }}_l^1{\overline{c}}_1+{\mathrm{\Lambda }}_l^2{\overline{c}}_2}\end{array}$$

(4)

In the formula

$$\{\begin{array}{c}{\overline{c}}_1=p_{11}{\mu }_{l-1}^1+p_{21}{\mu }_{l-1}^2\\ {\overline{c}}_2=p_{12}{\mu }_{l-1}^1+p_{22}{\mu }_{l-1}^2\end{array}$$

(5)

where $p_{ji}$ is the transition probability of the model $j$ to $i$, and ${\mu }^i$ is the probability of the first model, ${\mu }^j\left(l\right)$ is the probability of IMM update, and c is the normalization constant.

Analyze model 1 (uniform linear motion), assuming that the input variables obtained by the model probability update module are $I_1$ and $I_2$, and the resulting output variable is $\mu$, let

$$\{\begin{array}{c}{\mathrm{I}}_1={\mu }_{1,\mathrm{k}-1}\\ {\mathrm{I}}_2={\widehat{\mu }}_{1,\mathrm{k}}-{\mu }_{1,\mathrm{k}-1}\\ {\mu }_{1,\mathrm{k}}=\mu \end{array}$$

(6)

(1)

The domain of input variables and output variables:

$$\{\begin{array}{c}{\mathrm{I}}_1:\left[0,1\right]\\ {\mathrm{I}}_2:\left[-1,1\right]\\ \mu :\left[0,1\right]\end{array}$$

(7)

(2)

Fuzzy set of input variables and output variables:

$$\{\begin{array}{c}{\mathrm{I}}_1:\left\{Small\left(S\right),Medium\left(M\right),Big\left(B\right)\right\}\\ {\mathrm{I}}_2:\left\{Negative\left(N\right), Zero\left(Z\right), Positive\left(P\right)\right\}\\ \mu :\left\{Small\left(S\right),Medium\left(M\right),Big\left(B\right)\right\}\end{array}$$

(8)

(3)

Determining the membership function:

Refer to Figure 2, Figure 3 and Figure 4 to formulate fuzzy rules, ${\mathrm{I}}_1$ represents the probability of model 1 at $\left(l-1\right)$ moment, ${\mathrm{I}}_2$ represents the difference between the updated probability and the pre-update probability after updating at the moment of l. $\mu$ is the probability of time $\mathrm{l}$, then, analyze these three variables: if ${\mathrm{I}}_2$ is a negative value and is denoted as N, then the probability $\mu$ at k moment must be smaller than the model probability ${\mathrm{I}}_1$ at $\left(l-1\right)$ moment; if ${\mathrm{I}}_2$ is equal to 0, it is recorded as Z, then the probability $\mu$ at k moment should be equal to the model probability ${\mathrm{I}}_1$ at $\left(l-1\right)$ moment; if ${\mathrm{I}}_2$ is a positive value and is denoted as P, then the probability $\mu$ at l moment must be greater than the model probability ${\mathrm{I}}_1$ at $\left(l-1\right)$ moment. Their fuzzy logic rules are shown in

Table 1. Fuzzy logic rules.

Rule Number	${\mathbf{I}}_1$	${\mathbf{I}}_2$	$\mathbf{\mu }$
1	S	N	S
2	S	Z	S
3	S	P	M
4	M	N	S
5	M	Z	M
6	M	P	B
7	B	N	M
8	B	Z	B
9	B	P	B

:

When the output of the fuzzy logic system is defuzzified, the median method is used to defuzzify and obtain the actual probability of the model.

3. Adaptive Five-Degree Cubature Kalman

In order to pursue the better performance of the 5CKF filtering algorithm [16,17], the error covariance is improved based on the principle of Sage–Husa noise estimator, and a five-degree cubature Kalman algorithm based on error covariance adaptive is proposed.

Assume random variable x, its mean is $\overline{x}=E\left[x\right]$, $\mathrm{P}=\mathrm{cov}\left[x\right]$ is its covariance matrix, the A5CKF algorithm consists of two parts: time update and measurement update.

3.1. Time Update

(1) Performing Cholesky decomposition on the covariance matrix $P_{l-1|l-1}$:

$$P_{l-1|l-1}=S_{l-1|l-1}{S}_{l-1|l-1}^T$$

(9)

(2) Calculate the cubature point:

$$X_{l-1|l-1}^{0i}={\widehat{x}}_{l-1|l-1}$$

(10)

$$X_{l-1|l-1}^{1i}=\sqrt{\left(n+2\right)}{S}_{l-1|l-1}{e}_i+{\widehat{x}}_{l-1|l-1}$$

(11)

$$X_{l-1|l-1}^{2i}=-\sqrt{\left(n+2\right)}{S}_{l-1|l-1}{e}_i+{\widehat{x}}_{l-1|l-1}$$

(12)

$$X_{l-1|l-1}^{3i}=\sqrt{\left(n+2\right)}{S}_{l-1|l-1}{s}_i^{+}+{\widehat{x}}_{l-1|l-1}$$

(13)

$$X_{l-1|l-1}^{4i}=-\sqrt{\left(n+2\right)}{S}_{l-1|l-1}{s}_i^{+}+{\widehat{x}}_{l-1|l-1}$$

(14)

$$X_{l-1|l-1}^{5i}=\sqrt{\left(n+2\right)}{S}_{l-1|l-1}{s}_i^{-}+{\widehat{x}}_{l-1|l-1}$$

(15)

$$X_{l-1|l-1}^{6i}=-\sqrt{\left(n+2\right)}{S}_{l-1|l-1}{s}_i^{-}+{\widehat{x}}_{l-1|l-1}$$

(16)

$$s_i^{+}=\left\{\sqrt{\frac{1}{2}}\left(e_j+e_i\right)\right\}:j<i,j,i=1,2,\dots ,n$$

(17)

$$s_i^{-}=\left\{\sqrt{\frac{1}{2}}\left(e_j-e_i\right)\right\}:j<i,j,i=1,2,\dots ,n$$

(18)

$${\omega }_0=2/\left(n+2\right)$$

(19)

$${\omega }_1=\frac{4-n}{2{\left(n+2\right)}^2},i=1,2,\dots ,n$$

(20)

$${\omega }_2=\frac{1}{{\left(n+2\right)}^2},i=1,2,\dots ,n\left(n-1\right)/2$$

(21)

where the dimension of the system is n; the weight of the cubature point is ${\omega }_0$, ${\omega }_1$, and ${\omega }_2$; and the unit vector is $e_i$.

(3) Propagating the cubature point:

Substituting the cubature point into the equation of state to obtain the cubature point after propagation.

$$X_{l|l-1}^{\ast ,0i}=f\left(X_{l-1|l-1}^{oi}\right)$$

(22)

$$X_{l|l-1}^{\ast ,1i}=f\left(X_{l-1|l-1}^{1i}\right)$$

(23)

$$X_{l|l-1}^{\ast ,2i}=f\left(X_{l-1|l-1}^{2i}\right)$$

(24)

$$X_{l|l-1}^{\ast ,3i}=f\left(X_{l-1|l-1}^{3i}\right)$$

(25)

$$X_{l|l-1}^{\ast ,4i}=f\left(X_{l-1|l-1}^{4i}\right)$$

(26)

$$X_{l|l-1}^{\ast ,5i}=f\left(X_{l-1|l-1}^{5i}\right)$$

(27)

$$X_{l|l-1}^{\ast ,6i}=f\left(X_{l-1|l-1}^{6i}\right)$$

(28)

(4) Prediction of state values:

Calculate the predicted value by combining the transmitted sample points with the corresponding weighted values.

$${\widehat{x}}_{l|l-1}={\omega }_0{X}_{l|l-1}^{\ast ,0i}+{\omega }_1{X}_1^{\ast }+{\omega }_2{X}_2^{\ast }$$

(29)

$$X_1^{\ast }={\sum }_{i=1}^n\left(X_{l|l-1}^{\ast ,1i}+X_{l|l-1}^{\ast ,2i}\right)$$

(30)

$$X_2^{\ast }=\sum _{i=1}^{n\left(n-1\right)/2}\left(X_{l|l-1}^{\ast ,3i}+X_{l|l-1}^{\ast ,4i}+X_{l|l-1}^{\ast ,5i}+X_{l|l-1}^{\ast ,6i}\right)$$

(31)

(5) Prediction of the state error covariance matrix:

$$P_{l|l-1}={\omega }_0{X}_{l|l-1}^{\ast ,0i}{X}_{l|l-1}^{\ast T,0i}+P_1+P_2$$

(32)

$$P_1={\omega }_1\sum _{i=1}^n\left(X_{l|l-1}^{\ast ,1i}{X}_{l|l-1}^{\ast T,1i}+X_{l|l-1}^{\ast ,2i}{X}_{l|l-1}^{\ast T,2i}\right)$$

(33)

$$P_2={\omega }_2\sum _{i=1}^{n(n-1)/2}\left(X_{l|l-1}^{\ast ,3i}{X}_{l|l-1}^{\ast T,3i}+X_{l|l-1}^{\ast ,4i}{X}_{l|l-1}^{\ast T,4i}+X_{l|l-1}^{\ast ,5i}{X}_{l|l-1}^{\ast T,5i}+X_{l|l-1}^{\ast ,6i}{X}_{l|l-1}^{\ast T,6i}\right)$$

(34)

3.2. Measurement Update

(1) Performing Cholesky decomposition on the prediction covariance matrix $P_{l|l-1}$:

$$P_{l|l-1}=S_{l|l-1}{S}_{l|l-1}^T$$

(35)

(2) Calculate the cubature point:

$$X_{l|l-1}^{0i}={\widehat{x}}_{l|l-1}$$

(36)

$$X_{l|l-1}^{1i}=\sqrt{\left(n+2\right)}{S}_{l|l-1}{e}_i+{\widehat{x}}_{l|l-1}$$

(37)

$$X_{l|l-1}^{2i}=-\sqrt{\left(n+2\right)}{S}_{l|l-1}{e}_i+{\widehat{x}}_{l|l-1}$$

(38)

$$X_{l|l-1}^{3i}=\sqrt{\left(n+2\right)}{S}_{l|l-1}{s}_i^{+}+{\widehat{x}}_{l|l-1}$$

(39)

$$X_{l|l-1}^{4i}=-\sqrt{\left(n+2\right)}{S}_{l|l-1}{s}_i^{+}+{\widehat{x}}_{l|l-1}$$

(40)

$$X_{l|l-1}^{5i}=\sqrt{\left(n+2\right)}{S}_{l|l-1}{s}_i^{-}+{\widehat{x}}_{l|l-1}$$

(41)

$$X_{l|l-1}^{6i}=-\sqrt{\left(n+2\right)}{S}_{l|l-1}{s}_i^{-}+{\widehat{x}}_{l|l-1}$$

(42)

(3) Propagating the cubature point:

Substituting the cubature point into the observation equation to obtain the cubature point after propagation.

$$Z_{l|l-1}^{0i}=h\left(X_{l|l-1}^{0i}\right)$$

(43)

$$Z_{l|l-1}^{1i}=h\left(X_{l|l-1}^{1i}\right)$$

(44)

$$Z_{l|l-1}^{2i}=h\left(X_{l|l-1}^{2i}\right)$$

(45)

$$Z_{l|l-1}^{3i}=h\left(X_{l|l-1}^{3i}\right)$$

(46)

$$Z_{l|l-1}^{4i}=h\left(X_{l|l-1}^{4i}\right)$$

(47)

$$Z_{l|l-1}^{5i}=h\left(X_{l|l-1}^{5i}\right)$$

(48)

$$Z_{l|l-1}^{6i}=h\left(X_{l|l-1}^{6i}\right)$$

(49)

(4) Predicted observation:

Calculating the observed values by weighted summation of the propagated cubature points.

$${\widehat{z}}_{l|l-1}={\omega }_0{Z}_{l|l-1}^{0i}+{\omega }_1{Z}_1+{\omega }_2{Z}_2$$

(50)

$${\mathrm{Z}}_1=\sum _{i=1}^n\left(Z_{l|l-1}^{1i}+Z_{l|l-1}^{2i}\right)$$

(51)

$${\mathrm{Z}}_2=\sum _{i=1}^{n\left(n-1\right)/2}\left(Z_{l|l-1}^{3i}+Z_{l|l-1}^{4i}+Z_{l|l-1}^{5i}+Z_{l|l-1}^{6i}\right)$$

(52)

(5) Prediction of the error covariance matrix:

Calculating the predicted error covariance matrix by weighted summation.

$$P_{l|l-1}^{zz}={\omega }_0{Z}_{l|l-1}^{0i}{Z}_{l|l-1}^{0i,T}+P_{zz1}+P_{zz2}+R_k$$

(53)

$$P_{zz1}={\omega }_1\sum _{i=1}^n\left(Z_{l|l-1}^{1i}{Z}_{l|l-1}^{1i,T}+Z_{l|l-1}^{2i}{Z}_{l|l-1}^{2i,T}\right)$$

(54)

$$P_{zz2}={\omega }_2\sum _{i=1}^{n(n-1)/2}\left(Z_{l|l-1}^{3i}{Z}_{l|l-1}^{3i,T}+Z_{l|l-1}^{4i}{Z}_{l|l-1}^{4i,T}+Z_{l|l-1}^{5i}{Z}_{l|l-1}^{5i,T}+Z_{l|l-1}^{6i}{Z}_{l|l-1}^{6i,T}\right)$$

(55)

(6) Calculation of the cross-covariance matrix:

Calculating the cross-covariance matrix by weighted summation.

$$P_{l|l-1}^{xz}={\omega }_0{X}_{l|l-1}^{0i}{Z}_{l|l-1}^{0i,T}+P_{xz1}+P_{xz2}-{\widehat{x}}_{l|l-1}{\widehat{z}}_{l|l-1}^T$$

(56)

$$P_{xz1}={\omega }_1\sum _{i=1}^n\left(X_{l|l-1}^{1i}{Z}_{l|l-1}^{1i,T}+X_{l|l-1}^{2i}{Z}_{l|l-1}^{2i,T}\right)$$

(57)

$$P_{xz1}={\omega }_1\sum _{i=1}^{n(n-1)/2}\left(X_{l|l-1}^{3i}{Z}_{l|l-1}^{3i,T}+X_{l|l-1}^{4i}{Z}_{l|l-1}^{4i,T}+X_{l|l-1}^{5i}{Z}_{l|l-1}^{5i,T}+X_{l|l-1}^{6i}{Z}_{l|l-1}^{6i,T}\right)$$

(58)

(7) Estimate the Kalman gain:

$$K_l=P_{l|l-1}^{xz}{P}_{l|l-1}^{zz,-1}$$

(59)

(8) Estimate the updated state:

$${\widehat{x}}_{l|l}={\widehat{x}}_{l|l-1}+K_l\left(z_l-{\widehat{z}}_{l|l-1}\right)$$

(60)

(9) Update of covariance matrix:

$$P_1=P_{l|l-1}-K_l{P}_{l|l-1}^{zz}{K}_l^T$$

(61)

(10) Calculation of residuals:

$${\epsilon }_l^j=z_l-{\widehat{z}}_{l|l-1}^j$$

(62)

(11) Recursive estimation $P_{l|l}$ statistical characteristics:

$$P_{l|l}=\left(l{P}_1+K_l{\epsilon }_l^j{\epsilon }_l^{j,T}{K}_l^T\right)/\left(l+1\right)$$

(63)

4. Algorithm Overall Framework

In order to obtain good filtering accuracy and response speed, A5CKF and FL algorithms are added in IMM, and an interactive multimodel adaptive five-degree cubature Kalman algorithm based on FL algorithm is proposed, which has the advantages of three algorithms. In the FLIMMA5CKF algorithm, the A5CKF filter is used for parallel filtering, the obtained estimation values are weighted and fused, and finally the state estimation is output. The execution steps of the FLIMMA5CKF algorithm and the IMM algorithm are basically the same: it is divided into four steps: input interaction, parallel filtering, update probability, and output data fusion. Figure 5 below is the frame diagram of FLIMMA5CKF.

4.1. Input Interaction

Estimating the current time measurement value $Z_l$ according to the system state estimation and covariance estimation at the previous moment, and then performing a new initialization calculation on the model, wherein the new initial value is obtained by the Markov matrix between different models. Set the state optimal estimate and the estimated covariance matrix at model time $\left(l-1\right)$:

$${\mu }_{l-1|l-1}^{j|i}=p_{ji}{\mu }_{l-1}^j/c_i$$

(64)

$${\widehat{X}}_{l-1|l-1}^{0i}=\sum _i{\widehat{X}}_{l-1|l-1}^j{\mu }_{l-1|l-1}^{j|i}$$

(65)

$$p_{l-1|l-1}^{0i}=\sum _i\left\{\begin{array}{c}{p}_{l-1|l-1}^j+\left[{\widehat{X}}_{l-1|l-1}^j-{\widehat{X}}_{l-1|l-1}^{0i}\right]\\ \ast {\left[{\widehat{X}}_{l-1|l-1}^j-{\widehat{X}}_{l-1|l-1}^{0i}\right]}^T\end{array}\right\}{\mu }_{l-1|l-1}^{j|i}$$

(66)

$$c_i=\sum p_{ji}{\mu }_{l-1}^j$$

(67)

The following is the corresponding meaning: at time $\left(l-1\right)$, for model $j$, its conditional probability is ${\mu }_{l-1|l-1}^{j|i}$, its probability is ${\mu }_{l-1}^j$, its mathematical expectation is ${\widehat{X}}_{l-1|l-1}^{0i}$, and its error covariance is $p_{l-1|l-1}^{0i}$.

4.2. Parallel Filtering

The choice of filtering algorithm can also directly determine the tracking effect. Therefore, the parallel filtering module selects the A5CKF algorithm proposed in Section 3 of this paper, which has better estimation effect. Then, based on the previously obtained a priori information combined with the execution steps of A5CKF, finally, we can get updated state variables and recursive estimation $P_{l|l}$ statistical characteristics.

4.3. Update Probability

Model updating is also the key factor to determine the effectiveness of the algorithm. It uses maximum likelihood function combined with FL algorithm in this paper, and uses the principle of fuzzy logic to control the updating of model probability.

The likelihood function:

$${\mathrm{\Lambda }}_{l-1}^i=N\left(z;z_{l|l-1}^i,{\epsilon }_l^i\right)=\frac{1}{\sqrt{2\pi v_l^i}}exp\left\{-\frac{1}{2}{\epsilon }_l^{i,T}{S}_{i,l}^{-1}{\epsilon }_l^i\right\}$$

(68)

where the filter residual is represented by ${\epsilon }_l^i=z-z_{l|l-1}^i$, and the covariance matrix of the residual is $S_{i,l}$

4.4. Output Data Fusion

According to the results of each model and their weights, the final output results are obtained.

$${\widehat{X}}_{l|l}=\sum _i{\widehat{X}}_{l|l}^i{\mu }_l^i$$

(69)

$$P_{l|l}=\sum \left\{P_{l|l}^i+\left[{\widehat{X}}_{l|l}^i-X_{l|l}\right]{\left[{\widehat{X}}_{l|l}^i-X_{l|l}\right]}^T\right\}{\mu }_l^i$$

(70)

5. Missile Dynamics Modeling

5.1. Analysis of The Motion Characteristics of Missiles

The missile’s motion can be seen as a six-degrees-of-freedom rigid body motion. The general situation is to describe the missile’s motion by solving the missile’s six-degree-of-freedom full-quantity equation [20]. Since the computational complexity of the high-order solution is large, and in most cases, the aerodynamic data of the missile cannot be obtained. It is necessary to adopt a simplified dynamic model. Therefore, according to the law of missile movement, the lift, thrust and side force of the missile are planned. The position and attitude of the missile are obtained from the equation of motion of the missile’s centroid, and then the missile’s track is generated. The centroid motion equation of the missile is as follows:

$$\{\begin{array}{c}\dot{v}=g_{x2}+a_{x2}\\ v\dot{\theta =g_{y2}+a_{y2}}\\ -{\dot{\psi }}_{v}cos{\theta }_v=g_{z2}+a_{z2}\\ \dot{x}=vcos\theta cos{\psi }_v\\ \dot{y}=vsin\theta \\ \dot{z}=-vcos\theta sin{\psi }_v\end{array}$$

(71)

$$\{\begin{array}{c}{g}_{x2}=-gsin\theta \\ g_{y2}=-gcos\theta \\ g_{z2}=0\\ a_{x2}=0\\ a_{y2}=-g_{y2}\\ a_{z2}=a_{0}cos\left(\omega t\right)\end{array}$$

(72)

where $v$ is the missile velocity; $\theta$ is the ballistic inclination of the missile; ${\psi }_v$ is the ballistic angle of the missile, x, y, and z are components in the inertial coordinate system; $g_{x2}$, $g_{y2}$, and $g_{z2}$ are the components of gravity acceleration in the ballistic coordinate system; and $a_{x2}$, $a_{y2}$, and $a_{z2}$ are the components of the ballistic overload in the inertial coordinate system.

Firstly, calculating the actual movement position of the missile using Runge–Kutta algorithm, and then the coordinate transformation is performed to convert the speed coordinate system to the projectile coordinate system for simulation.

5.2. Snake-Like Maneuvering

The “snake-like” is a maneuvering model: it is a special turning motion, when moving half a circle, its angular velocity direction will be opposite, and its radius is also changing over time [25]. Suppose the target moves in a sinusoidal motion in the horizontal plane. The target makes a uniform linear motion in the X axis, the speed is V, the speed of the Y axis is 0, and the sine motion is performed in the Z-axis.

The discretization form of the discretization available state equation is:

$$X_{k+1}=F{X}_k+G{W}_k$$

(73)

Deriving the discretized process noise input matrix G and the state transition matrix F:

$$\mathrm{G}={\left[\begin{array}{ccccccccc}\frac{T^3}{6}& 0& 0& \frac{T^2}{2}& 0& 0& T& 0& 0\\ 0& \frac{T^3}{6}& 0& 0& \frac{T^2}{2}& 0& 0& T& 0\\ 0& 0& \frac{\omega T-sin\left(\omega T\right)}{{\omega }^3}& 0& 0& \frac{1-cos\left(\omega T\right)}{{\omega }^2}& 0& 0& \frac{sin\left(\omega T\right)}{\omega }\end{array}\right]}^T$$

(74)

$$\mathrm{F}=\left[\begin{array}{ccccccccc}1& 0& 0& T& 0& 0& \frac{T^2}{2}& 0& 0\\ 0& 1& 0& 0& T& 0& 0& \frac{T^2}{2}& 0\\ 0& 0& 1& 0& 0& \frac{sin\left(\omega T\right)}{\omega }& 0& 0& \frac{1-cos\left(\omega T\right)}{{\omega }^2}\\ 0& 0& 0& 1& 0& 0& T& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& T& 0\\ 0& 0& 0& 0& 0& cos\left(\omega T\right)& 0& 0& \frac{sin\left(\omega T\right)}{\omega }\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& -\omega sin\left(\omega T\right)& 0& 0& cos\left(\omega T\right)\end{array}\right]$$

(75)

6. Results and Discussion

Setting the same simulation conditions, the missile performs uniform linear motion in 0–15 s and snake motion in 16–60 s. Comparative analysis was performed using FLIMMA5CKF with IMMCKF, IMM5CKF, and IMMA5CKF algorithms.

The nonlinear observation equation of the target is described as follows:

$$\mathrm{Z}\left(t\right)=h\left[X\left(t\right)\right]+V\left(t\right)$$

(76)

where $Z\left(t\right)$ is the observation vector, $V\left(t\right)$ is the measurement noise, and $h[\ast ]$ is the observation function. The value is:

$$\mathrm{h}\left[X\left(t\right)\right]=\left[\begin{array}{c}arctan\frac{y}{\sqrt{x^2+z^2}}\\ arctan\frac{-z}{x}\end{array}\right]$$

(77)

The performance of the four algorithms can be measured by the displacement, velocity, acceleration Error and RMSE at time l:

$$\mathrm{Error}=\frac{{\sum }_{i=1}^l\frac{1}{m}{\sum }_{j=1}^m\left(X_m^l-{\widehat{X}}_m^l\right)}{k}$$

(78)

$$\mathrm{RMSEs}=\sqrt{\frac{{\sum }_{j=1}^m\frac{1}{l}{\sum }_{i=1}^l{\left(X_m^l-{\widehat{X}}_m^l\right)}^2}{m}}$$

(79)

where m is the number of Monte Carlo simulations, n is the total number of sampling points, $X_{m,k}$ is the true value, and ${\widehat{X}}_{m,l}$ is the estimated value.

Assume that the target’s maneuvering frequency is $\mathsf{\omega }=0.2\mathsf{\pi }$, the filtering period is 0.1 s, the Runge–Kutta method step size is 0.01 s, the initial probability of both models is 0.5, the initial state of the target is $X\left(0\right)={\left[\begin{array}{ccccccccc}\mathrm{90,000}& 2000& 50& -1450& 0& 0& 0& 0& -20\end{array}\right]}^T$, the initial state error covariance matrix is $\mathrm{P}\left(0\right)=diag\left\{\left[\begin{array}{ccccccccc}100& 100& 100& 25& 25& 25& 1& 1& 1\end{array}\right]\right\}$, measuring noise standard deviation is 1 mrad, the standard noise standard deviation is 10, the Monte Carlo simulations are 50 times, and the transfer matrix between models is:

$$P_{ij}=\left[\begin{array}{cc}0.999& 0.001\\ 0.001& 0.999\end{array}\right]$$

(80)

Figure 6, Figure 7 and Figure 8 are comparisons of error estimates for tracking targets using the improved FLIMMA5CKF algorithm, IMMA5CKF algorithm, IMM5CKF algorithm [18], and IMMCKF algorithm [14], respectively.

From Figure 6, Figure 7 and Figure 8, combined with the data in

Table 2. X, Y, and Z direction maximum error.

Max Error	IMMCKF	IMM5CKF	IMMA5CKF	FLIMMA5CKF
$P_x$	265.62	258.53	39.81	26.65
$P_y$	48.75	43.50	9.66	5.78
$P_z$	38.80	38.81	45.82	50.25
$v_x$	15.28	14.96	4.10	2.68
$v_y$	7.24	6.21	2.03	1.47
$v_z$	39.69	39.78	40.16	40.62
$a_x$	−0.87	−0.85	−0.45	−0.42
$a_y$	0.81	0.79	0.31	0.26
$a_z$	19.62	19.65	19.70	19.89
time (s)	8.253	17.356	18.031	28.311

, it can be seen that the estimation error of the IMMA5CKF algorithm on the X-axis and the Y-axis is significantly smaller than the IMM5CKF algorithm of Zhu [18], and the IMMCKF algorithm of Li [14], which proves the high-precision characteristics of the A5CKF algorithm proposed in this paper, it not only adapts the error covariance, but also adapts the noise. Comparing the FLIMMA5CKF algorithm with IMMA5CKF, we can find that the estimation error of the FLIMMA5CKF algorithm is smaller than that of the IMMA5CKF algorithm. It proves that the FL algorithm proposed in this paper not only improves the accuracy of the algorithm, but also accelerates the convergence speed. In the Z-axis direction, the FLIMMA5CKF algorithm is slightly worse than the other three algorithms before 35 s, but after 35 s, the error reduction is obvious and more stable, in contrast, the other three algorithms have varying degrees of mutation or jump, in Figure 6, the late estimation error is approximately $\pm 1$ m, in Figure 7, the late speed estimation error is ~0.4 $\mathrm{m}/\mathrm{s}$, in Figure 8, and the late acceleration estimation error is ~0.2 $\mathrm{m}/{\mathrm{s}}^2$.

Figure 9, Figure 10 and Figure 11 shows the RMSE of the four algorithms. As can be seen from Figure 9, Figure 10 and Figure 11, when the missile moves linearly at a constant speed, there is no significant difference in the tracking effect of several algorithms, but after 16 s, the missile begins to accelerate in a snake-like manner, as can be seen from Figure 9, Figure 10 and Figure 11. Compared with the IMMCKF algorithm [14], the IMM5CKF algorithm [18], and the IMMA5CKF algorithm, the FLIMMA5CKF algorithm has a great improvement in tracking performance and stability. It can be seen from the Z-axis of Figure 9, Figure 10 and Figure 11, FLIMMA5CKF has a better convergence speed than the other three algorithms, no peaks appear after 40 s, and the robustness is better. Figure 12 shows the model probability update graphs of the four algorithms, FLIMMA5CKF algorithm can better control model probability update due to the addition of fuzzy logic algorithm.

It can be seen from the data in

Table 3. Average RMSE comparison of four algorithms.

RMSE	IMMCKF	IMM5CKF	IMMA5CKF	FLIMMA5CKF
$P_x$	71.13	65.35	19.24	14.38
$P_y$	15.09	14.29	7.19	6.26
$P_z$	8.62	8.59	8.56	8.65
$v_x$	6.86	6.35	2.41	1.67
$v_y$	3.06	2.96	2.21	1.87
$v_z$	6.08	5.96	5.93	5.89
$a_x$	0.37	0.36	0.22	0.16
$a_y$	0.58	0.57	0.49	0.39
$a_z$	3.19	3.16	3.15	3.12
time (s)	8.253	17.356	18.031	28.311

that the performance of the FLIMMA5CKF algorithm in this paper is significantly higher than that of the IMMCKF algorithm [14], IMM5CKF algorithm [18], and IMMA5CKF algorithm in the X-axis and Y-axis directions. In the Z-axis direction, the algorithm of this paper is better than the other three, but the advantages of the algorithm are not particularly large. The shortcoming is that the FLIMMA5CKF algorithm in this paper is more complex than the other three algorithms, and the simulation time of the algorithm is relatively long.

7. Conclusions

Aiming at addressing the problem of poor filtering, slow convergence, and poor robustness in the IMM algorithm, this paper proposes the FLIMMA5CKF algorithm. The A5CKF algorithm is effective for improving the estimation effect, the FL algorithm can solve the problem of convergence speed very well, and these two algorithms are added to the IMM algorithm. By setting the same simulation model analysis, compared with IMMCKF, IMM5CKF, and IMMA5CKF, the proposed algorithm has a good filtering effect, and the hysteresis problem is also significantly improved. According to the analysis, in future research, the algorithm can take advantage of its accuracy to be used in actual positioning, tracking or navigation systems.