Improved Spatial Registration and Target Tracking Method for Sensors on Multiple Missiles

Abstract

Inspired by the problem that the current spatial registration methods are unsuitable for three-dimensional (3-D) sensor on high-dynamic platform, this paper focuses on the estimation for the registration errors of cooperative missiles and motion states of maneuvering target. There are two types of errors being discussed: sensor measurement biases and attitude biases. Firstly, an improved Kalman Filter on Earth-Centered Earth-Fixed (ECEF-KF) coordinate algorithm is proposed to estimate the deviations mentioned above, from which the outcomes are furtherly compensated to the error terms. Secondly, the Pseudo Linear Kalman Filter (PLKF) and the nonlinear scheme the Unscented Kalman Filter (UKF) with modified inputs are employed for target tracking. The convergence of filtering results are monitored by a position-judgement logic, and a low-pass first order filter is selectively introduced before compensation to inhibit the jitter of estimations. In the simulation, the ECEF-KF enhancement is proven to improve the accuracy and robustness of the space alignment, while the conditional-compensation-based PLKF method is demonstrated to be the optimal performance in target tracking.

1. Introduction

The spatial registration is vital for current and near-future cooperative combat missions through the vehicle network to estimate and compensate the sensor errors by measuring the common target [1,2,3].

Depending on the dimension of the error model, the algorithm is usually divided into a two-dimensional (2-D) system and a three-dimensional (3-D) system [4]. Many typical 2-D algorithms have been proposed in literatures. e.g., Real Time Quality Control (RTQC) [5], Least Square (LS) [6], Generalized Least Square (GLS) [7], Exact Maximum Likelihood (EML) [8]. Compared to 3-D algorithms, the former wherein has relatively lower computational complexity at the expense of accuracy. Meanwhile, a number of online registration algorithms for 3-D sensors have been put forward. Unfortunately, there are few approaches taking both measurement errors and orientation errors into consideration. The traditional Earth-Centered Earth-Fixed coordinate (ECEF) method [9] capable of solving the spatial registration problem concentrates on the measurement biases, yet ignores the attitude errors of sensors themselves when the measurement states are transformed from separate reference frame to public system. The neglect of measurement deviations in the Kalman-filter-based method [10] proposed for attitude biases makes it deficient way as well. Although the algorithm [11] combines the two types of errors for estimation, it only fits for the situation where the attitude angles are small and the sensors are close to each other.

Besides, there is another branch of research that integrates the registration with target tracking. The paper [12] presents an improved method based on the state value and space deviation of federated filtering of unscented Kalman filter and standard Kalman filter, conducting registration and tracking simultaneously. However, this method is thought to accounts for more computation [13]. For this reason, the processes of those two are usually performed separately in current investigation. Moreover, most methods referred above are only suitable for sensors on stationary base which would be deemed points in the relative motion between them and targets. Failure of them to adapts for the sensors having their own attitude and position changes (e.g., radar seeker on high-speed moving and turning missiles) is considered as the deficiency.

Target tracking, based on sensor measurement information, is a technique for estimation of the state (position, speed) of a moving target at the current or future time [14,15]. Filtering is the cornerstone of tracking algorithm with the existence of systematic and measured noise. The typical filters have been greatly adopted for more than half a century: Kalman Filter (KF) is first exploited by [16] to solve the problem of tracking for linear system. Extended Kalman Filter (EKF) [17] transforms nonlinear model to linear one and then takes KF to estimate the states, which easily diverges under strongly nonlinearity. On this basis, [18] puts forwards Unscented Kalman Filter (UKF) which is apt for both linear and nonlinear tracking, but compromised by the heavy computation. To make up the disadvantage of the methods above, the Pseudo Linear Kalman Filter (PLKF) [19] which takes the models in [20] preserves the nonlinearity in the system. With the development of computers and data links during the last ten years, tracking performance is increased through the design and implementation of systems using data collected by a network of spatially distributed sensors. Consequently, the spatial registration has gradually become the pre-requisite for information process and data fusion for multiple sensors.

In the case of two missiles cooperative target tracking, this paper employs an improved Kalman Filter on Earth-Centered Earth-Fixed coordinate (ECEF-KF) algorithm which transforms the measurements to public target from body systems into the ECEF system so as to isolate the motion of sensors. Taking advantage of the difference of transformed values, the systemic biases and the attitude errors are estimated simultaneously. Subsequently, the results of registration algorithm adopted are fed back to compensate the inputs of linear PLKF and nonlinear UKF which are used for target tracking. It is found in this paper that the convergence of ECEF-KF and the effectiveness of compensation depend on the quality of the 3-D measurement data that can be adversely affected by the movement of missiles and target. More specifically, the 3-D information may loses one degree of freedom on any dimension and turns to be 2-D with the approach of vehicles, ruining the registration results. For sake of the possible divergence, a Low-pass First Order Filter (LFOF) with position-judgement condition is introduced to detect and inhibit divergent trend, furtherly improves the accuracy of the target tracking. The paper researches theoretically in the information processing methods, which conduces furtherly to the application of sensor in the engineering problems.

The main contributions are listed below:

• A new spatial registration algorithm is first proposed for sensors on high-speed moving vehicles, realizing the simultaneous estimation for system and attitude biases which are compensated to the biased measurements of the tracking schemes. The accuracy and robustness of the estimation of target state are effectively improved.
• Inspired by the ideal of integral controller, a Low-pass filter is used when the position relationship between missiles and target meets the special condition to inhibit the jitter of estimations. This skill improve the adaptability of tracking system without time-delay caused by the common integral controllers.

This paper is organized as follows. The definition of each coordinate system and their transform relationships are given in Section 2. Section 3 develops the model of original and improved ECEF-KF algorithm. The compensation-based PLKF and UKF methods with strategies of divergence detection and inhibition is introduced in Section 4. Section 5 presents the performance of improved ECEF-KF under two cases of motion of missiles and compares PLKF methods and UKF methods. The concluding remarks are given in Section 6.

2. Coordinate Defination and Transformation

Before the model development, the reference frames will be introduced in this section.

2.1. Definition of Coordinate System

In the spatial alignment of multiple missiles, each member has its own reference frame or coordinate system. Among which, the measurements of sensors on the platform are obtained in the body coordinate system, the attitude angles and the position are usually described in the Local East, North, Up system and ECEF coordinate system respectively. The definition of these coordinate systems referred are listed as follows (see in Figure 1).

(1) ECEF Coordinate $O{X}_e{Y}_e{Z}_e$: As defined in WGS-84 coordinate [21], the origin is located in the earth’s Center of Mass (CM). The $Z_e$ axis points to the direction of Conventional Terrestrial Pole (CTP). The $X_e$ axis points to the intersection point of the zero-meridian plane and the CTP equator defined in Bureau International deI’Heure (BIH) 1984 [22]. The $Y_e$ axis and the $Z_e$ axis are perpendicular to the $X_e$ axis, forming the right-hand coordinate system.

(2) Local East, North and Up (LENU) Coordinate $O{X}_d{Y}_d{Z}_d$: The origin is the projection of the platform’s CM to the surface of the earth, the $X_d$, $Y_d$ and $Z_d$ axis points to the east, north and up respectively [23].

(3) Body Coordinate $O{X}_b{Y}_b{Z}_b$: The origin is located in the CM of platform. The $X_b$ axis coincides with the longitudinal axis of platform, pointing to the head as positive. The $Y_b$ axis coincides with the vertical axis of platform, pointing to the up as positive. And the $Z_b$ is determined by the right-hand rule.

2.2. Transformation between Reference Frames

(1) ECEF and LENU Coordinate: Let the column vectors ${\mathit{r}}_e={\left[\begin{array}{ccc}{x}_e& y_e& z_e\end{array}\right]}^T$ and ${\mathit{r}}_d={\left[\begin{array}{ccc}{x}_d& y_d& z_d\end{array}\right]}^T$ (the superscript T denotes matrix transposition) represent the ECEF coordinate and LENU coordinate respectively. The transformation from the later to the former is given by

$${\mathit{r}}_e={\mathit{C}}_d^e{\mathit{r}}_d$$

(1)

where ${\mathit{C}}_d^e$ is the $3\times 3$ orthogonal matrix given by [24]

$$\left[\begin{array}{ccc}-sin\lambda & -sin\phi cos\lambda & cos\phi cos\lambda \\ cos\lambda & -sin\phi sin\lambda & cos\phi sin\lambda \\ 0& cos\phi & sin\phi \end{array}\right]$$

(2)

$\lambda$ and $\phi$ denote the longitude and latitude of platform. Besides, if the longitude $\lambda$, latitude $\phi$ and height h are known, its ECEF coordinate can be calculated by [25]

$$\begin{array}{c}{x}_e=\left(C+h\right)cos\left(\phi \right)cos\left(\lambda \right)\hfill \\ y_e=\left(C+h\right)cos\left(\phi \right)sin\left(\lambda \right)\hfill \\ z_e=\left(C\left(1-e^2\right)+h\right)sin\left(\phi \right)\hfill \\ C=R_e/\sqrt{1-e^2{sin}^2\left(\phi \right)}\hfill \end{array}$$

(3)

where $R_e$ is the equatorial radius, e is the eccentricity of the earth.

(2) LENU and Body Coordinate: Let the column vectors ${\mathit{r}}_b={\left[\begin{array}{ccc}{x}_b& y_b& z_b\end{array}\right]}^T$ represents the body coordinates. Likewise, the transformation from the body coordinate to the LENU coordinate is represented by a matrix ${\mathit{C}}_b^d$ in [26]

$$\left[\begin{array}{ccc}cos\theta cos\psi & sin\theta & -cos\theta sin\psi \\ -sin\theta cos\psi cos\gamma +sin\psi sin\gamma & cos\theta cos\gamma & sin\theta sin\psi cos\gamma +cos\psi sin\gamma \\ sin\theta sin\gamma cos\psi +cos\gamma sin\psi & -cos\theta sin\gamma & -sin\theta sin\psi sin\gamma +cos\gamma cos\psi \end{array}\right]$$

(4)

where $\psi$, $\theta$ and $\gamma$ are the yaw, pitch and roll angle of the platform respectively.

3. Registration Algorithm

The algorithm is based on the following assumptions:

• The attitude biases and the measured deviations are considered as constant and the attitude biases are assumed as small values.
• The coupling between biases is ignored.
• The position errors of sensors are not considered. It means the positions are known exactly with other possible assistant device (e.g., GPS).

3.1. Attitude and Sensor Measurement Errors

The ECEF-KF algorithm is introduced to estimate attitude and sensor measurement biases which are modeled as additive constant biases to the reported values. That is

$$\left\{\begin{array}{c}{\tilde{\psi }}_k={\psi }_k+\Delta {\psi }_k\hfill \\ {\tilde{\theta }}_k={\theta }_k+\Delta {\theta }_k\hfill \\ {\tilde{\gamma }}_k={\gamma }_k+\Delta {\gamma }_k\hfill \end{array}\right.$$

(5)

where ${\tilde{\psi }}_k$, ${\tilde{\theta }}_k$, ${\tilde{\gamma }}_k$ are the reported values of the k th sensor’s yaw, pitch and roll angles. ${\psi }_k$, ${\theta }_k$, ${\gamma }_k$ are the true values and $\Delta {\psi }_k$, $\Delta {\theta }_k$, $\Delta {\gamma }_k$ are the bias errors of these angles. Likewise, the measurement erros are modeled as

$$\left\{\begin{array}{c}{\tilde{r}}_k=r_k+\Delta r_k\\ {\tilde{\alpha }}_k={\alpha }_k+\Delta {\alpha }_k\\ {\tilde{\beta }}_k={\beta }_k+\Delta {\beta }_k\end{array}\right.$$

(6)

where $r_k$, ${\alpha }_k$, ${\beta }_k$ represent the true values of the kth sensor’s slope distance (range), the elevation and azimuth angle of line of sight respectively. It is notable that the measurement errors include constant biases and random noises, that is

$$\left\{\begin{array}{c}\Delta r_k=\Delta r_{km}+\Delta r_{kn}\\ \Delta {\alpha }_k=\Delta {\alpha }_{km}+\Delta {\alpha }_{kn}\\ \Delta {\beta }_k=\Delta {\beta }_{km}+\Delta {\beta }_{kn}\end{array}\right.$$

(7)

where $\Delta r_{km}$, $\Delta {\alpha }_{km}$, $\Delta {\beta }_{km}$ are the constant biases, and $\Delta r_{kn}$, $\Delta {\alpha }_{kn}$, $\Delta {\beta }_{kn}$ denote the random noises.

3.2. Traditional ECEF-KF Algorithm

Given the true measurements of range r, elevation $\alpha$ and azimuth $\beta$ of radar sensor which are defined under body system, the non-linear relationship between target state $(x_b,y_b,z_b)$ and measurement is (see in Figure 2)

$$\begin{array}{c}\alpha =arcsin\frac{y_b}{\sqrt{x_b^2+y_b^2+z_b^2}}\hfill \\ \beta =-arctan\frac{z_b}{x_b}\hfill \\ r=\sqrt{x_b^2+y_b^2+z_b^2}\hfill \end{array}$$

(8)

then the coordinate of target can be written as

$$\left\{\begin{array}{c}{x}_b=rcos\alpha cos\beta \hfill \\ y_b=rsin\alpha \hfill \\ z_b=-rcos\alpha sin\beta \hfill \end{array}\right.$$

(9)

Substitute (6) in (9)

$$\left\{\begin{array}{c}{x}_b=\left(\tilde{r}-\Delta r\right)cos\left(\tilde{\alpha }-\Delta \alpha \right)cos\left(\tilde{\beta }-\Delta \beta \right)\hfill \\ y_b=\left(\tilde{r}-\Delta r\right)sin\left(\tilde{\alpha }-\Delta \alpha \right)\hfill \\ z_b=-\left(\tilde{r}-\Delta r\right)cos\left(\tilde{\alpha }-\Delta \alpha \right)sin\left(\tilde{\beta }-\Delta \beta \right)\hfill \end{array}\right.$$

(10)

Let ${\mathit{X}}_b={\left[\begin{array}{ccc}{x}_b& y_b& z_b\end{array}\right]}^T$, $\mathit{\xi }={\left[\begin{array}{ccc}\Delta r& \Delta \alpha & \Delta \beta \end{array}\right]}^T$. Since the measurement errors are small values as assumed, the Equation (10) can be expressed as the first-order linearity of the errors

$${\mathit{X}}_b={\tilde{\mathit{X}}}_b+\mathit{J}\mathit{\xi }$$

(11)

where ${\tilde{\mathit{X}}}_b$ is the reported coordinate of target, $\mathit{J}$ is the Jacobian matrix of ${\mathit{X}}_b$ with respect to $\mathit{\xi }$ given by

$$\mathit{J}=\left[\begin{array}{ccc}\frac{\partial x_b}{\partial \Delta r}& \frac{\partial x_b}{\partial \Delta \alpha }& \frac{\partial x_b}{\partial \Delta \beta }\\ \frac{\partial y_b}{\partial \Delta r}& \frac{\partial y_b}{\partial \Delta \alpha }& \frac{\partial y_b}{\partial \Delta \beta }\\ \frac{\partial z_b}{\partial \Delta r}& \frac{\partial z_b}{\partial \Delta \alpha }& \frac{\partial z_b}{\partial \Delta \beta }\end{array}\right]$$

(12)

Now, here are two sensors A and B. (${\lambda }_a$,${\phi }_a$,$h_a$) and (${\lambda }_b$,${\phi }_b$,$h_b$) are their longitude, latitude and height coordinates. Let ${\mathit{X}}_{ea}={\left[\begin{array}{ccc}{x}_{ea}& y_{ea}& z_{ea}\end{array}\right]}^T$ and ${\mathit{X}}_{eb}={\left[\begin{array}{ccc}{x}_{eb}& y_{eb}& z_{eb}\end{array}\right]}^T$ be the ECEF coordinates of two sensors respectively and ${\mathit{X}}_{et}={\left[\begin{array}{ccc}{x}_{et}& y_{et}& z_{et}\end{array}\right]}^T$ represents the ECEF coordinate of target. ${\mathit{X}}_{ba}={\left[\begin{array}{ccc}{x}_{ba}& y_{ba}& z_{ba}\end{array}\right]}^T$ and ${\mathit{X}}_{bb}={\left[\begin{array}{ccc}{x}_{bb}& y_{bb}& z_{bb}\end{array}\right]}^T$ are the body coordinates of target from two sensors respectively. Thus, the following relationships are given.

$$\left[\begin{array}{c}{x}_{et}\\ y_{et}\\ z_{et}\end{array}\right]=\left[\begin{array}{c}{x}_{ea}\\ y_{ea}\\ z_{ea}\end{array}\right]+\left[\begin{array}{ccc}-sin{\lambda }_a& -sin{\phi }_{a}cos{\lambda }_a& cos{\phi }_{a}cos{\lambda }_a\\ cos{\lambda }_a& -sin{\phi }_{a}sin{\lambda }_a& cos{\phi }_{a}sin{\lambda }_a\\ 0& cos{\phi }_a& sin{\phi }_a\end{array}\right]\left[\begin{array}{c}{x}_{ba}\\ y_{ba}\\ z_{ba}\end{array}\right]$$

(13)

$$\left[\begin{array}{c}{x}_{et}\\ y_{et}\\ z_{et}\end{array}\right]=\left[\begin{array}{c}{x}_{eb}\\ y_{eb}\\ z_{eb}\end{array}\right]+\left[\begin{array}{ccc}-sin{\lambda }_b& -sin{\phi }_{b}cos{\lambda }_b& cos{\phi }_{b}cos{\lambda }_b\\ cos{\lambda }_b& -sin{\phi }_{b}sin{\lambda }_b& cos{\phi }_{b}sin{\lambda }_b\\ 0& cos{\phi }_b& sin{\phi }_b\end{array}\right]\left[\begin{array}{c}{x}_{bb}\\ y_{bb}\\ z_{bb}\end{array}\right]$$

(14)

From the two equations above, we obtain the following equality

$${\mathit{X}}_{ea}+{\mathit{R}}_a{\mathit{X}}_{ba}={\mathit{X}}_{eb}+{\mathit{R}}_b{\mathit{X}}_{bb}$$

(15)

where ${\mathit{R}}_a$ and ${\mathit{R}}_b$ denote the rotational matrices in (13) and (14). ${\mathit{X}}_{ba}$ and ${\mathit{X}}_{bb}$ can be expanded to the linear form like (11), that is

$$\begin{array}{cc}\hfill \left({\mathit{X}}_{ea}+{\mathit{R}}_a{\tilde{\mathit{X}}}_{ba}\right)+{\mathit{R}}_a{\mathit{J}}_a{\mathit{\xi }}_a& =\left({\mathit{X}}_{eb}+{\mathit{R}}_b{\tilde{\mathit{X}}}_{bb}\right)+{\mathit{R}}_b{\mathit{J}}_b{\mathit{\xi }}_b\hfill \\ \hfill {\tilde{\mathit{X}}}_{ea}-{\tilde{\mathit{X}}}_{eb}& =-{\mathit{R}}_a{\mathit{J}}_a{\mathit{\xi }}_a+{\mathit{R}}_b{\mathit{J}}_b{\mathit{\xi }}_b\hfill \end{array}$$

(16)

where ${\tilde{\mathit{X}}}_{ea}$ and ${\tilde{\mathit{X}}}_{eb}$ represent the ECEF coordinates of target reported by sensors A and B. In order to estimate the measurement errors of sensors A and B, the state vector is developed as

$$\mathit{\chi }\left(k\right)={\left[\begin{array}{cccccc}\Delta r_{am}& \Delta {\alpha }_{am}& \Delta {\beta }_{am}& \Delta r_{bm}& \Delta {\alpha }_{bm}& \Delta {\beta }_{bm}\end{array}\right]}^T$$

(17)

where $\Delta r_{am}$, $\Delta {\alpha }_{am}$, and $\Delta {\beta }_{am}$ are the measurment biases of sensor A, $\Delta r_{bm}$, $\Delta {\alpha }_{bm}$, $\Delta {\beta }_{bm}$ denote the measurment biases of sensor B. Since these terms are constant, the discrete equation of the state can be written as

$$\begin{array}{c}\mathit{\chi }\left(k\right)=\mathit{F}\left(k\right)\mathit{\chi }\left(k-1\right)+\mathit{W}\left(k\right)\hfill \\ \mathit{F}\left(k\right)={\left[\begin{array}{ccc}1& 0& 0\\ 0& \ddots & 0\\ 0& 0& 1\end{array}\right]}_{6\times 6}\hfill \end{array}$$

(18)

where $\mathit{W}\left(k\right)$ is zero mean white Gaussian process noise with covariance $\mathit{Q}\left(k\right)$. $\mathit{F}\left(k\right)$ is the state transition matrix. In (16), let $\mathit{Z}={\tilde{\mathit{X}}}_{ea}-{\tilde{\mathit{X}}}_{eb}$, ${\mathit{H}}_1=-{\mathit{R}}_a{\mathit{J}}_a$, ${\mathit{H}}_2={\mathit{R}}_b{\mathit{J}}_b$, $\mathit{H}=\left[\begin{array}{cc}{\mathit{H}}_1& {\mathit{H}}_2\end{array}\right]$. Therefore, the measurement equation can be expressed as

$$\mathit{Z}\left(k\right)=\mathit{H}\left(k\right)\mathit{\chi }\left(k\right)+\mathit{V}\left(k\right)$$

(19)

where $\mathit{V}\left(k\right)$ is zero mean white Gaussian process noise with covariance $\mathit{R}\left(k\right)$. Kalman filtering techniques [27] using (18) and (19) can be applied to estimate sensor errors online.

3.3. Improved ECEF-KF Algorithm

From the above derivation, it can be seen that the traditional ECEF-KF method only fits for sensors on stationary base without attitude errors. For the sensors on high-speed moving missiles, the position’s and attitude’s change of missiles will influence the actual measurements, and the oriented biases of each partner which may cause the misalignment of tracking or even the loss of target must not be ignored. Considering this situation, an improved method making minor modifications on the original algorithm, is proposed to realize the simultaneous estimation of attitude and measurement errors.

Here assuming two missiles A and B as well. Considering the attitude change, (13) and (14) are modified as

$${\mathit{X}}_{et}={\mathit{X}}_{ea}+{\mathit{C}}_{d,a}^e{\mathit{C}}_{b,a}^d{\mathit{X}}_{ba}$$

(20)

$${\mathit{X}}_{et}={\mathit{X}}_{eb}+{\mathit{C}}_{d,b}^e{\mathit{C}}_{b,b}^d{\mathit{X}}_{bb}$$

(21)

where ${\mathit{C}}_{d,a}^e$ and ${\mathit{C}}_{d,b}^e$ equal with ${\mathit{R}}_a$ and ${\mathit{R}}_b$ in (15) are rotation matrices from the LENU coordinate system to the ECEF coordinate system (2) of missiles A and B. ${\mathit{C}}_{b,a}^d$ and ${\mathit{C}}_{b,b}^d$ represent rotation matrices from the body coordinate system to the LENU coordinate system (4). Combining the two equations above, there is

$$\Delta {\mathit{X}}_e={\mathit{X}}_{eb}-{\mathit{X}}_{ea}={\mathit{C}}_{d,a}^e{\mathit{C}}_{b,a}^d{\mathit{X}}_{ba}-{\mathit{C}}_{d,b}^e{\mathit{C}}_{b,b}^d{\mathit{X}}_{bb}$$

(22)

Since the rotation matrix is orthogonal [28], (for one orthogonal matrix $\mathit{M}$, there is ${\mathit{M}}^{-1}={\mathit{M}}^T$), (22) is transformed to

$${\mathit{X}}_{ba}={\left({\mathit{C}}_{b,a}^d\right)}^T{\left({\mathit{C}}_{d,a}^e\right)}^T\left({\mathit{C}}_{d,b}^e{\mathit{C}}_{b,b}^d{\mathit{X}}_{bb}+\Delta {\mathit{X}}_e\right)$$

(23)

When attitude errors are small values, the rotation matrix from body coordinate system to LENU system can be written as [29]

$$\begin{array}{c}{\tilde{\mathit{C}}}_b^d=\left(\mathit{I}-\mathit{\varphi }\right){\mathit{C}}_b^d\hfill \\ \mathit{\varphi }=(\mathit{\delta }\times )=\left[\begin{array}{ccc}0& -\Delta \psi & \Delta \theta \\ \Delta \psi & 0& -\Delta \gamma \\ -\Delta \theta & \Delta \gamma & 0\end{array}\right]\hfill \\ \mathit{\delta }={\left[\begin{array}{ccc}\Delta \psi & \Delta \theta & \Delta \gamma \end{array}\right]}^T\hfill \end{array}$$

(24)

Thus

$$\begin{array}{c}{\mathit{C}}_{b,a}^d={(\mathit{I}-{\mathit{\varphi }}_a)}^T{\tilde{\mathit{C}}}_{b,a}^d\hfill \\ {\mathit{C}}_{b,b}^d={(\mathit{I}-{\mathit{\varphi }}_b)}^T{\tilde{\mathit{C}}}_{b,b}^d\hfill \end{array}$$

(25)

Substitute (25) in (23) and ignore the product of ${\mathit{\varphi }}_a$ and ${\mathit{\varphi }}_b$

$${\mathit{X}}_{ba}\approx {\tilde{\mathit{C}}}_d^{b,a}\left(\left(\mathit{I}-{\mathit{\varphi }}_a\right){\mathit{C}}_e^{d,a}{\mathit{C}}_{d,b}^e-{\mathit{C}}_e^{d,a}{\mathit{C}}_{d,b}^e{{\mathit{\varphi }}_b}^T\right){\tilde{\mathit{C}}}_{b,b}^d{\mathit{X}}_{bb}+{\tilde{\mathit{C}}}_d^{b,a}\left(\mathit{I}-{\mathit{\varphi }}_a\right){\mathit{C}}_e^{d,a}\Delta {\mathit{X}}_e$$

(26)

Both sides of (26) are multiplied by ${\mathit{C}}_{d,a}^e{\tilde{\mathit{C}}}_{b,a}^d$

$${\mathit{C}}_{d,a}^e{\tilde{\mathit{C}}}_{b,a}^d{\mathit{X}}_{ba}=({\mathit{C}}_{d,b}^e-{\mathit{C}}_{d,a}^e{\mathit{\varphi }}_a{\mathit{C}}_e^{d,a}{\mathit{C}}_{d,b}^e-{\mathit{C}}_{d,b}^e{{\mathit{\varphi }}_b}^T){\tilde{\mathit{C}}}_{b,b}^d{\mathit{X}}_{bb}+{\mathit{C}}_{d,a}^e(\mathit{I}-{\mathit{\varphi }}_a){\mathit{C}}_e^{d,a}\Delta {\mathit{X}}_e$$

(27)

Expand ${\mathit{X}}_{ba}$ and ${\mathit{X}}_{bb}$ into the form of first-order linearity as (11). Since the item $\mathit{\varphi }$ and $\mathit{\xi }$ are both small values, their product is ignored. Therefore, (27) becomes

$$\begin{array}{c}{\mathit{C}}_{d,a}^e{\tilde{\mathit{C}}}_{b,a}^d{\tilde{\mathit{X}}}_{ba}-{\mathit{C}}_{d,b}^e{\tilde{\mathit{C}}}_{b,b}^d{\tilde{\mathit{X}}}_{bb}+\Delta {\mathit{X}}_e\approx {\mathit{C}}_{d,b}^e{\tilde{\mathit{C}}}_{b,b}^d{\mathit{J}}_b{\mathit{\xi }}_b\hfill \\ -{\mathit{C}}_{d,a}^e{\tilde{\mathit{C}}}_{b,a}^d{\mathit{J}}_a{\mathit{\xi }}_a-({\mathit{C}}_{d,a}^e{\mathit{\varphi }}_a{\mathit{C}}_e^{d,a}+{\mathit{C}}_{d,b}^e{{\mathit{\varphi }}_b}^T{\mathit{C}}_e^{d,b}){\mathit{C}}_{d,b}^e{\tilde{\mathit{C}}}_{b,b}^d{\tilde{\mathit{X}}}_{bb}\hfill \end{array}$$

(28)

In the above formula, the left part of the equation is labeled as ${\mathit{Z}}^{*}$. Let ${\mathit{H}}_1^{*}=-{\mathit{C}}_{d,a}^e{\tilde{\mathit{C}}}_{b,a}^d{\mathit{J}}_a$, ${\mathit{H}}_2^{*}=-{\mathit{C}}_{d,b}^e{\tilde{\mathit{C}}}_{b,b}^d{\mathit{J}}_b$ and the last term in the right part of the equation is represented as $\mathit{P}$, we arrive at

$${\mathit{Z}}^{*}={\mathit{H}}_1^{*}{\mathit{\xi }}_a+{\mathit{H}}_2^{*}{\mathit{\xi }}_b+\mathit{P}$$

(29)

The following work is to transform $\mathit{P}$ to the linear form of attitude errors. The matrix $\mathit{P}$ is written as the following

$$\mathit{P}=-(\mathit{A}{\mathit{\varphi }}_a{\mathit{A}}^T+\mathit{B}{\mathit{\varphi }}_b^T{\mathit{B}}^T)\mathit{M}$$

(30)

where

$$\begin{array}{c}\mathit{A}={\mathit{C}}_{d,a}^e=\left[\begin{array}{ccc}{\mathit{a}}_1& {\mathit{a}}_2& {\mathit{a}}_3\end{array}\right]\hfill \\ \mathit{B}={\mathit{C}}_{d,b}^e=\left[\begin{array}{ccc}{\mathit{b}}_1& {\mathit{b}}_2& {\mathit{b}}_3\end{array}\right]\hfill \\ \mathit{M}=\mathit{B}{\tilde{\mathit{C}}}_{b,b}^d{\tilde{\mathit{X}}}_{bb}\hfill \end{array}$$

${\mathit{a}}_i$ and ${\mathit{b}}_i$ are the column vectors of A and B respectively.

Remark 1.

Here giving an equality:

$$\mathit{A}{\mathit{\varphi }}_a{\mathit{A}}^T\mathit{M}=\mathit{A}{\left[\begin{array}{ccc}-{\mathit{a}}_2& {\mathit{a}}_1& \mathit{0}\\ {\mathit{a}}_3& \mathit{0}& -{\mathit{a}}_1\\ \mathit{0}& -{\mathit{a}}_3& {\mathit{a}}_2\end{array}\right]}^{T}diag\left(\mathit{M},\mathit{M},\mathit{M}\right)\left[\begin{array}{c}\Delta {\psi }_a\\ \Delta {\theta }_a\\ \Delta {\gamma }_a\end{array}\right]$$

(31)

Proof of Remark 1.

$$\begin{array}{cc}\hfill Left& =\left[\begin{array}{ccc}{\mathit{a}}_1& {\mathit{a}}_2& {\mathit{a}}_3\end{array}\right]\left[\begin{array}{ccc}\mathit{0}& -\Delta {\psi }_a& \Delta {\theta }_a\\ \Delta {\psi }_a& \mathit{0}& -\Delta {\gamma }_a\\ -\Delta {\theta }_a& \Delta {\gamma }_a& \mathit{0}\end{array}\right]\left[\begin{array}{c}{\mathit{a}}_1^T\\ {\mathit{a}}_2^T\\ {\mathit{a}}_3^T\end{array}\right]\mathit{M}\hfill \\ & =\left[\begin{array}{ccc}{\mathit{a}}_1& {\mathit{a}}_2& {\mathit{a}}_3\end{array}\right]\left[\begin{array}{c}-\Delta {\psi }_a{\mathit{a}}_2^T+\Delta {\theta }_a{\mathit{a}}_3^T\\ \Delta {\psi }_a{\mathit{a}}_1^T-\Delta {\gamma }_a{\mathit{a}}_3^T\\ -\Delta {\theta }_a{\mathit{a}}_1^T+\Delta {\gamma }_a{\mathit{a}}_2^T\end{array}\right]\mathit{M}\hfill \\ & =\left(\left({\mathit{a}}_2{\mathit{a}}_1^T-{\mathit{a}}_1{\mathit{a}}_2^T\right)\Delta {\psi }_a+\left({\mathit{a}}_1{\mathit{a}}_3^T-{\mathit{a}}_3{\mathit{a}}_1^T\right)\Delta {\theta }_a+\left({\mathit{a}}_3{\mathit{a}}_2^T-{\mathit{a}}_2{\mathit{a}}_3^T\right)\Delta {\gamma }_a\right)\mathit{M}\hfill \\ \hfill Right& =\left[\begin{array}{ccc}{\mathit{a}}_1& {\mathit{a}}_2& {\mathit{a}}_3\end{array}\right]\left[\begin{array}{ccc}-{\mathit{a}}_2^T& {\mathit{a}}_3^T& \mathit{0}\\ {\mathit{a}}_1^T& \mathit{0}& -{\mathit{a}}_3^T\\ \mathit{0}& -{\mathit{a}}_1^T& {\mathit{a}}_2^T\end{array}\right]\left[\begin{array}{c}\Delta {\psi }_a\mathit{M}\\ \Delta {\theta }_a\mathit{M}\\ \Delta {\gamma }_a\mathit{M}\end{array}\right]\hfill \\ & =\left[\begin{array}{ccc}-{\mathit{a}}_1{\mathit{a}}_2^T+{\mathit{a}}_2{\mathit{a}}_1^T& {\mathit{a}}_1{\mathit{a}}_3^T-{\mathit{a}}_3{\mathit{a}}_1^T& -{\mathit{a}}_2{\mathit{a}}_3^T+{\mathit{a}}_3{\mathit{a}}_2^T\end{array}\right]\times \left[\begin{array}{c}\Delta {\psi }_a\mathit{M}\\ \Delta {\theta }_a\mathit{M}\\ \Delta {\gamma }_a\mathit{M}\end{array}\right]\hfill \\ & =Left\hfill \end{array}$$

So, the Theorem 1 is true. ☐

Likewise

$$\begin{array}{c}\mathit{B}{{\mathit{\varphi }}_b}^T{\mathit{B}}^T\mathit{M}=-\mathit{B}{\mathit{\varphi }}_b{\mathit{B}}^T\mathit{M}\hfill \\ =-\mathit{B}{\left[\begin{array}{ccc}-{\mathit{b}}_2& {\mathit{b}}_1& \mathit{0}\\ {\mathit{b}}_3& \mathit{0}& -{\mathit{b}}_1\\ \mathit{0}& -{\mathit{b}}_3& {\mathit{b}}_2\end{array}\right]}^{T}diag\left(\mathit{M},\mathit{M},\mathit{M}\right)\left[\begin{array}{c}\Delta {\psi }_b\\ \Delta {\theta }_b\\ \Delta {\gamma }_b\end{array}\right]\hfill \end{array}$$

(32)

Let

$$\begin{array}{cc}\hfill {\mathit{H}}_3^{*}& =-\mathit{A}{\left[\begin{array}{ccc}-{\mathit{a}}_2& {\mathit{a}}_1& \mathit{0}\\ {\mathit{a}}_3& \mathit{0}& -{\mathit{a}}_1\\ \mathit{0}& -{\mathit{a}}_3& {\mathit{a}}_2\end{array}\right]}^{T}diag\left(\mathit{M},\mathit{M},\mathit{M}\right)\hfill \\ \hfill {\mathit{H}}_4^{*}& =\mathit{B}{\left[\begin{array}{ccc}-{\mathit{b}}_2& {\mathit{b}}_1& \mathit{0}\\ {\mathit{b}}_3& \mathit{0}& -{\mathit{b}}_1\\ \mathit{0}& -{\mathit{b}}_3& {\mathit{b}}_2\end{array}\right]}^{T}diag\left(\mathit{M},\mathit{M},\mathit{M}\right)\hfill \\ \hfill {\mathit{\delta }}_a& ={\left[\begin{array}{ccc}\Delta {\psi }_a& \Delta {\theta }_a& \Delta {\gamma }_a\end{array}\right]}^T{\mathit{\delta }}_b={\left[\begin{array}{ccc}\Delta {\psi }_b& \Delta {\theta }_b& \Delta {\gamma }_b\end{array}\right]}^T\hfill \end{array}$$

(33)

Substituting (31)∼(33) in (30), $\mathit{P}$ is finally expanded as

$$\mathit{P}={\mathit{H}}_3^{*}{\mathit{\delta }}_a+{\mathit{H}}_4^{*}{\mathit{\delta }}_b$$

(34)

Let (34) in (29)

$$\begin{array}{cc}\hfill {\mathit{Z}}^{*}& ={\mathit{H}}_1^{*}{\mathit{\xi }}_a+{\mathit{H}}_2^{*}{\mathit{\xi }}_b+{\mathit{H}}_3^{*}{\mathit{\delta }}_a+{\mathit{H}}_4^{*}{\mathit{\delta }}_b\hfill \\ & ={\mathit{H}}_1^{*}{\left[\begin{array}{ccc}\Delta r_{am}+\Delta r_{an}& \Delta {\alpha }_{am}+\Delta {\alpha }_{an}& \Delta {\beta }_{am}+\Delta {\beta }_{an}\end{array}\right]}^T\hfill \\ & +{\mathit{H}}_2^{*}{\left[\begin{array}{ccc}\Delta r_{bm}+\Delta r_{bn}& \Delta {\alpha }_{bm}+\Delta {\alpha }_{bn}& \Delta {\beta }_{bm}+\Delta {\beta }_{bn}\end{array}\right]}^T\hfill \\ & +{\mathit{H}}_3^{*}{\left[\begin{array}{ccc}\Delta {\psi }_a& \Delta {\theta }_a& \Delta {\gamma }_a\end{array}\right]}^T+{\mathit{H}}_4^{*}{\left[\begin{array}{ccc}\Delta {\psi }_b& \Delta {\theta }_b& \Delta {\gamma }_b\end{array}\right]}^T\hfill \end{array}$$

(35)

where $\left(\Delta r_{am},\Delta {\alpha }_{am},\Delta {\beta }_{am}\right),\left(\Delta r_{bm},\Delta {\alpha }_{bm},\Delta {\beta }_{bm}\right)$ are constant measurment biases of missile A and B respectively, $\left(\Delta r_{an},\Delta {\alpha }_{an},\Delta {\beta }_{an}\right),\left(\Delta r_{bn},\Delta {\alpha }_{bn},\Delta {\beta }_{bn}\right)$ are random noises added to these measurements.

Considering the terms of attitude errors, we redefine

$${\mathit{\chi }}^{*}\left(k\right)={\left[\Delta r_{am}\Delta {\alpha }_{am}\Delta {\beta }_{am}\Delta r_{bm}\Delta {\alpha }_{bm}\Delta {\beta }_{bm}\Delta {\psi }_a\Delta {\theta }_a\Delta {\gamma }_a\Delta {\psi }_b\Delta {\theta }_b\Delta {\gamma }_b\right]}^T$$

(36)

to be the 12-dimensional state vector. The corrected discrete equation of the state is

$${\mathit{\chi }}^{*}\left(k\right)={\mathit{F}}^{*}\left(k\right){\mathit{\chi }}^{*}\left(k-1\right)+{\mathit{W}}^{*}\left(k\right)$$

(37)

Since the attitude and measurement biases are all considered as constant values.

$${\mathit{F}}^{*}\left(k\right)={\left[\begin{array}{ccc}1& 0& 0\\ 0& \ddots & 0\\ 0& 0& 1\end{array}\right]}_{12\times 12}$$

Denoting ${\mathit{H}}^{*}=\left[\begin{array}{cccc}{{\mathit{H}}_1}^{*}& {{\mathit{H}}_2}^{*}& {{\mathit{H}}_3}^{*}& {{\mathit{H}}_4}^{*}\end{array}\right]$ In (35), the new measurement equation is

$${\mathit{Z}}^{*}\left(k\right)={\mathit{H}}^{*}\left(k\right){\mathit{\chi }}^{*}\left(k\right)+{\mathit{V}}^{*}\left(k\right)$$

(38)

where ${\mathit{V}}^{*}\left(k\right)$ is still zero mean white Gaussian process noise with covariance ${\mathit{R}}^{*}\left(k\right)$. Kalman filtering techniques using (37) and (38) can be used here as well, which is unnecessary to go into the details.

4. Target Tracking with Error Compensation

4.1. Compensation PLKF Algorithm

In Section 3, the coordinate of target in the body coordinate system has been given in (10):

$$\left\{\begin{array}{c}{x}_b=\left(\tilde{r}-\Delta r\right)cos\left(\tilde{\alpha }-\Delta \alpha \right)cos\left(\tilde{\beta }-\Delta \beta \right)\hfill \\ y_b=\left(\tilde{r}-\Delta r\right)sin\left(\tilde{\alpha }-\Delta \alpha \right)\hfill \\ z_b=-\left(\tilde{r}-\Delta r\right)cos\left(\tilde{\alpha }-\Delta \alpha \right)sin\left(\tilde{\beta }-\Delta \beta \right)\hfill \end{array}\right.$$

(39)

where $\Delta r=\Delta r_m+\Delta r_n,\Delta \alpha =\Delta {\alpha }_m+\Delta {\alpha }_n,\Delta \beta =\Delta {\beta }_m+\Delta {\beta }_n$.

In order to improve the accuracy of target tracking, the error compensation method is introduced.The estimation of registration errors including range $\Delta r_m^{*}$, elevation angle $\Delta {\alpha }_m^{*}$, azimuth angle $\Delta {\beta }_m^{*}$ and attitude errors $\Delta {\psi }^{*}$, $\Delta {\theta }^{*}$, $\Delta {\gamma }^{*}$ are obtained through ECEF-KF. These estimated values are compensated to the error terms of the equation above and the rotation matrix ${\tilde{\mathit{C}}}_b^d$ in (24), that is

$$\Delta r_m\approx \Delta r_m^{*}\Delta {\alpha }_m\approx \Delta {\alpha }_m^{*}\Delta {\beta }_m\approx \Delta {\beta }_m^{*}$$

(40)

$$\mathit{C}{{}_b^d}^{*}={(\mathit{I}-{\mathit{\varphi }}^{*})}^T{\tilde{\mathit{C}}}_b^d{\mathit{\varphi }}^{*}=\left[\begin{array}{ccc}0& -\Delta {\psi }^{*}& \Delta {\theta }^{*}\\ \Delta {\psi }^{*}& 0& -\Delta {\gamma }^{*}\\ -\Delta {\theta }^{*}& \Delta {\gamma }^{*}& 0\end{array}\right]$$

(41)

Let $r^{\prime }=\tilde{r}-\Delta r_m^{*}$, ${\alpha }^{\prime }=\tilde{\alpha }-\Delta {\alpha }_m^{*}$, ${\beta }^{\prime }=\tilde{\beta }-\Delta {\beta }_m^{*}$, (39) becomes

$$\left\{\begin{array}{c}{x}_b=\left(r^{\prime }-\Delta r_n\right)cos\left({\alpha }^{\prime }-\Delta {\alpha }_n\right)cos\left({\beta }^{\prime }-\Delta {\beta }_n\right)\hfill \\ y_b=\left(r^{\prime }-\Delta r_n\right)sin\left({\alpha }^{\prime }-\Delta {\alpha }_n\right)\hfill \\ z_b=-\left(r^{\prime }-\Delta r_n\right)cos\left({\alpha }^{\prime }-\Delta {\alpha }_n\right)sin\left({\beta }^{\prime }-\Delta {\beta }_n\right)\hfill \end{array}\right.$$

(42)

Considering that $\Delta r_n,\Delta {\alpha }_n,\Delta {\beta }_n$ are small values, there are $cos\Delta {\alpha }_n\approx 1$, $cos\Delta {\beta }_n\approx 1$, $sin\Delta {\alpha }_n\approx \Delta {\alpha }_n$, $sin\Delta {\beta }_n\approx \Delta {\beta }_n$. While, ignore the high-level small quantities, which means $\Delta {\alpha }_n\Delta {\beta }_n\approx 0$, $\Delta {\alpha }_n\Delta r_n\approx 0$, $\Delta r_n\Delta {\beta }_n\approx 0$. Then, the equation above turns to the following pseudo measurement:

$$\left\{\begin{array}{c}{l}_x\triangleq r^{\prime }cos{\alpha }^{\prime }cos{\beta }^{\prime }=x_b-n_x\hfill \\ l_y\triangleq r^{\prime }sin{\alpha }^{\prime }=y_b-n_y\hfill \\ l_z\triangleq -r^{\prime }cos{\alpha }^{\prime }sin{\beta }^{\prime }=z_b-n_z\hfill \end{array}\right.$$

(43)

where $n_x,n_y,n_z$ are pseudo measurement noises, here are

$$\left\{\begin{array}{c}{n}_x=\Delta {\beta }_n{r}^{\prime }cos{\alpha }^{\prime }sin{\beta }^{\prime }+\Delta {\alpha }_n{r}^{\prime }sin{\alpha }^{\prime }cos{\beta }^{\prime }-\Delta r_{n}cos{\alpha }^{\prime }cos{\beta }^{\prime }\hfill \\ n_y=-\Delta {\alpha }_n{r}^{\prime }cos{\alpha }^{\prime }-\Delta r_{n}sin{\alpha }^{\prime }\hfill \\ n_z=\Delta {\beta }_n{r}^{\prime }cos{\alpha }^{\prime }cos{\beta }^{\prime }-\Delta {\alpha }_n{r}^{\prime }sin{\alpha }^{\prime }sin{\beta }^{\prime }+\Delta r_{n}cos{\alpha }^{\prime }sin{\beta }^{\prime }\hfill \end{array}\right.$$

The covariance of these measurement noises is

$${\mathit{R}}_n=\left[\begin{array}{ccc}Var\left(n_x\right)& Cov\left(n_x,n_y\right)& Cov\left(n_x,n_z\right)\\ Cov\left(n_y,n_x\right)& Var\left(n_y\right)& Cov\left(n_y,n_z\right)\\ Cov\left(n_z,n_x\right)& Cov\left(n_y,n_z\right)& Var\left(n_z\right)\end{array}\right]$$

(44)

where

$$\begin{array}{c}{\sigma }_r^2=Var\left(\Delta r_n\right),{\sigma }_{\alpha }^2=Var\left(\Delta {\alpha }_n\right),{\sigma }_{\beta }^2=Var\left(\Delta {\beta }_n\right)\hfill \\ \left\{\begin{array}{c}Var\left(n_x\right)={\left({\sigma }_{\beta }{r}^{\prime }cos{\alpha }^{\prime }sin{\beta }^{\prime }\right)}^2+{\left({\sigma }_{\alpha }{r}^{\prime }sin{\alpha }^{\prime }cos{\beta }^{\prime }\right)}^2+{\left({\sigma }_{r}cos{\alpha }^{\prime }cos{\beta }^{\prime }\right)}^2\hfill \\ Var\left(n_y\right)={\left({\sigma }_{\alpha }{r}^{\prime }cos{\alpha }^{\prime }\right)}^2+{\left({\sigma }_{r}sin{\alpha }^{\prime }\right)}^2\hfill \\ Var\left(n_z\right)={\left({\sigma }_{\beta }{r}^{\prime }cos{\alpha }^{\prime }cos{\beta }^{\prime }\right)}^2+{\left({\sigma }_{\alpha }{r}^{\prime }sin{\alpha }^{\prime }sin{\beta }^{\prime }\right)}^2+{\left({\sigma }_{r}cos{\alpha }^{\prime }sin{\beta }^{\prime }\right)}^2\hfill \\ Cov\left(n_x,n_y\right)=Cov\left(n_y,n_x\right)=cos{\alpha }^{\prime }sin{\alpha }^{\prime }cos{\beta }^{\prime }\times \left({{\sigma }_r}^2-{\left(r^{\prime }{\sigma }_{\alpha }\right)}^2\right)\hfill \\ Cov\left(n_x,n_z\right)=Cov\left(n_z,n_x\right)=cos{\beta }^{\prime }sin{\beta }^{\prime }\times \left({\left(r^{\prime }cos{\alpha }^{\prime }{\sigma }_{\beta }\right)}^2-{\left(r^{\prime }sin{\alpha }^{\prime }{\sigma }_{\alpha }\right)}^2-{\left(cos{\alpha }^{\prime }{\sigma }_r\right)}^2\right)\hfill \\ Cov\left(n_y,n_z\right)=Cov\left(n_z,n_y\right)=cos{\alpha }^{\prime }sin{\alpha }^{\prime }sin{\beta }^{\prime }\times \left({\left(r^{\prime }{\sigma }_{\alpha }\right)}^2-{{\sigma }_r}^2\right)\hfill \end{array}\right.\hfill \end{array}$$

Representing $\mathit{U}={\left[\begin{array}{cccccc}{x}_b& y_b& z_b& {\dot{x}}_b& {\dot{y}}_b& {\dot{z}}_b\end{array}\right]}^T$ as the state vector, the continuous equation is

$$\dot{\mathit{U}}=\mathit{SU}+\mathit{G}\mathit{\omega }$$

(45)

where $\mathit{\omega }={\left[\begin{array}{ccc}{\mathit{\omega }}_x& {\mathit{\omega }}_y& {\mathit{\omega }}_z\end{array}\right]}^T$ is zero-mean white noise with covariance $\Sigma$, and $\mathit{S}$ is the transition matrix. The CV (Constant Velocity) model [30] is chosen as the target motion model, that is

$$\mathit{S}=\left[\begin{array}{cc}{\mathit{0}}_3& {\mathit{I}}_3\\ {\mathit{0}}_3& {\mathit{0}}_3\end{array}\right]\mathit{G}=\left[\begin{array}{c}{\mathit{0}}_3\\ {\mathit{I}}_3\end{array}\right]$$

The discrete-time state function is

$$\mathit{U}\left(k\right)=\mathit{T}\left(k\right)\mathit{U}\left(k-1\right)+\Lambda \left(k\right)$$

(46)

where $\Lambda \left(k\right)$ is discrete-time process noise, and the state transition matrix $\mathit{T}\left(k\right)=\mathit{T}\left(t_k,t_{k+1}\right)$ at time $t_k$ over time interval $t_{\Delta }=t_{k+1}-t_k$ is given by

$$\mathit{T}\left(k\right)=e^{\mathit{S}{t}_{\Delta }}\approx \mathit{I}+t_{\Delta }\mathit{S}$$

The covariance of the discrete-time process noise is

$${\mathit{Q}}_k=E\left[{\Lambda }_k{{\Lambda }_k}^T\right]={\int }_{t_k}^{t_{k+1}}\mathit{T}\left(t_k,\tau \right)\mathit{G}\Sigma {\mathit{G}}^T\mathit{T}{\left(t_k,\tau \right)}^{T}d\tau$$

Let $\mathit{L}={\left[\begin{array}{ccc}{l}_x& l_y& l_z\end{array}\right]}^T$ denotes the measurement vector. Therefore, the measurement equation is transformed to the linear function of the target state.

$$\begin{array}{cc}\hfill \mathit{L}\left(k\right)& =\mathit{Y}\left(k\right)\mathit{U}\left(k\right)+\mathit{\mu }\left(k\right)\hfill \\ \hfill \mathit{Y}\left(k\right)& =\left[\begin{array}{cc}{\mathit{I}}_3& {\mathit{0}}_3\end{array}\right]\hfill \end{array}$$

(47)

where $\mathit{\mu }\left(k\right)$ is zero-mean Gaussian noise.

4.2. Conpensation UKF Algorithm

If we directly take the compensation inputs as the measurements of filter ${\mathit{L}}^{*}={\left[r^{{}^{\prime }}{\alpha }^{{}^{\prime }}{\beta }^{{}^{\prime }}\right]}^T$, then the measurement equation will be changed as the non-linear form (8)

$${\mathit{L}}^{*}\left(k\right)=h\left(\mathit{U}\left(k\right)\right)+{\mathit{\mu }}^{*}\left(k\right)$$

(48)

where ${\mathit{\mu }}^{*}\left(k\right)$ is measurement noise with $\Omega \left(k\right)$ as variance. Combined with (46) and (48), the UKF algorithm for nonlinear system state estimation is exactly required. The formulas of UKF are directly given as follows.

Initializing:

$$\begin{array}{c}\widehat{\mathit{U}}\left(0\right)=E\left[\mathit{U}\left(0\right)\right]\hfill \\ \mathit{P}\left(0\right)=E\left[\left(\mathit{U}\left(0\right)-\widehat{\mathit{U}}\left(0\right)\right){\left(\mathit{U}\left(0\right)-\widehat{\mathit{U}}\left(0\right)\right)}^T\right]\hfill \end{array}$$

(49)

Computing sigma point set:

$$\ell \left(k\right)=\left[\begin{array}{ccc}\widehat{\mathit{U}}\left(k\right)& \widehat{\mathit{U}}\left(k\right)+\sqrt{\left(n+\lambda \right)\mathit{P}\left(k\right)}& \widehat{\mathit{U}}\left(k\right)-\sqrt{\left(n+\lambda \right)\mathit{P}\left(k\right)}\end{array}\right]$$

(50)

Prediction updating:

$$\begin{array}{c}\ell \left(k+1|k\right)=\mathit{T}\left(k\right)\ell \left(k\right)\\ \widehat{\mathit{U}}\left(k+1|k\right)={\displaystyle \sum _{i=0}^{2n}}{\mathit{W}}_i^m{\ell }_i\left(k+1|k\right)\\ \mathit{P}\left(k+1|k\right)={\displaystyle \sum _{i=0}^{2n}}{\mathit{W}}_i^c\left[{\ell }_i\left(k+1|k\right)-\widehat{\mathit{U}}\left(k+1|k\right)\right]\times {\left[{\ell }_i\left(k+1|k\right)-\widehat{\mathit{U}}\left(k+1|k\right)\right]}^T+\mathit{Q}\left(k+1\right)\\ {\widehat{\mathit{L}}}_i^{*}\left(k+1|k\right)=h\left({\ell }_i\left(k+1|k\right)\right)\\ {\widehat{\mathit{L}}}^{*}\left(k+1|k\right)={\displaystyle \sum _{i=0}^{2n}}{\mathit{W}}_i^m{\widehat{\mathit{L}}}_i^{*}\left(k+1|k\right)\end{array}$$

(51)

Measurement updating:

$$\begin{array}{c}{\mathit{P}}_{LL}\left(k+1\right)={\displaystyle \sum _{i=0}^{2n}}{\mathit{W}}_i^c\left[{\widehat{\mathit{L}}}_i^{*}\left(k+1|k\right)-{\widehat{\mathit{L}}}^{*}\left(k+1|k\right)\right]{\left[{{\widehat{\mathit{L}}}_{\mathit{i}}}^{*}\left(k+1|k\right)-{\widehat{\mathit{L}}}^{*}\left(k+1|k\right)\right]}^T+\Omega \left(k+1\right)\\ {\mathit{P}}_{UL}\left(k+1\right)={\displaystyle \sum _{i=0}^{2n}}{\mathit{W}}_i^c\left[\ell \left(k+1|k\right)-\widehat{\mathit{U}}\left(k+1|k\right)\right]{\left[{\widehat{\mathit{L}}}_i^{*}\left(k+1|k\right)-{\widehat{\mathit{L}}}^{*}\left(k+1|k\right)\right]}^T\\ \mathit{K}\left(k+1\right)={\mathit{P}}_{UL}\left(k+1\right){\left({\mathit{P}}_{LL}\left(k+1\right)\right)}^{-1}\\ \widehat{\mathit{U}}\left(k+1\right)=\widehat{\mathit{U}}\left(k+1|k\right)+\mathit{K}\left(k+1\right)\left({\mathit{L}}^{*}{\left(k+1|k\right)}_k-{\widehat{\mathit{L}}}^{*}\left(k+1|k\right)\right)\\ \mathit{P}\left(k+1\right)=\mathit{P}\left(k+1|k\right)-\mathit{K}\left(k+1\right){\left({\mathit{P}}_{LL}\left(k+1\right)\right)}^{-1}{\left(\mathit{K}\left(k+1\right)\right)}^T\end{array}$$

(52)

4.3. Compensation Condition and Strategy

Remark 2.

Considering the situation where two missiles and target are on the same latitude or longitude plane during their fight, the 3-D tracking problem will become 2-D.

As shown in the Figure 3, $OXYZ$ coordinate system is the longitude, latitude and height coordinate system. The axes $OX$, $OY$, $OZ$ denote longitude, height and latitude respectively. In this moment, the two missiles and the target are on the same latitude plane $Z=Z_0$ and the track plane $ABT$ is parallel to the plane $OXY$, which means the measurement of space registration will lost the measurements on $OZ$ axis and the tracking problem in 3-D space turns to be on the plane $OXY$. If two missiles keep flying on this plane for some time, the estimation of errors may be divergent, which will worsen the compensation effect.

A judging condition is presented here: Assume $\Gamma$ is the normal vector of the tracking plane $ABT$, ${\mathit{r}}_i$ is the unit vector parallel to the axes, ${\vartheta }_i$ is the vectorial angle between $\Gamma$ and ${\mathit{r}}_i$.

$$\left|cos{\vartheta }_i-1\right|=\left|\frac{{\mathit{r}}_i·\Gamma }{∥{\mathit{r}}_i∥∥\Gamma ∥}-1\right|\le \epsilon$$

(53)

where $\epsilon$ is a sufficiently small value, $∥\cdot ∥$ is the Euclidean metric of vector [31]. If the inequality satisfies, the situation in Remark 2 will happen.

To avoid the decreasing precision of compensation tracking, the estimation of registration errors will be introduced to a LFOF when (53) is true, that is

$$y=\frac{1}{\kappa s+1}u\Rightarrow \dot{y}=-\frac{1}{\kappa }y+\frac{1}{\kappa }u$$

(54)

where u is the error items, y is the output of the filter, $\kappa$ is the propotionality coefficient. The fourth-order Runge-Kutta algorithm [32,33] can be used to solve the above differential equation.

Figure 4 shows the flow of whole algorithms. Before target tracking, the ECEF-KF is introduced to obtain registration errors of measurements of missiles. The errors acquired are furtherly introduced to the tracking system through the conditional compensation. When the tracking plane is parallel to one of three coordinate planes, the LFOF is taken to inhibit the divergence of error estimations.

The compensation PLKF method with LFOF is called the CCPLKF (Conditional Compensation Pseudo Linear Kalman Filter), otherwise, called UCPLKF (Unconditional Compensation Pseudo Linear Kalman Filter). Accordingly, the compensation UKF method is divided to CCUKF and UCUKF.

5. Simulation

In this section, the performance of the proposed algorithm in spatial registration compared with traditional ECEF-KF is demonstrated. And the target tracking scheme is proved effective through the comparison of linear filters and nonlinear filters.

In the simulation, the ground target on curvilinear motion with variable velocity is considered. The velocity of target is (N 10 sin$\frac{2\pi t}{T}$, E 5m/s), wherein t is the current time and T is the total simulation time.

The initial position of target is (longitude $108.5^{\circ }$, latitude $34.{01}^{\circ }$, height 0 m). The same terms of 1th and 2th missiles are (longitude ${108}^{\circ }$, latitude ${34}^{\circ }$, height 300 m) and (longitude ${108}^{\circ }$, latitude $34.{02}^{\circ }$, height 200 m) respectively. There are two motion situations of missiles being considered:

Case 1: Both two missiles make uniform linear flight to the east, the velocity are (N 0 m/s, E 120 m/s) and (N 0 m/s, E 100 m/s) respectively.

Case 2: The missiles make uniform linear flight as well, the velocity are (N 5 m/s, E 120 m/s) and (N −7 m/s, E 100 m/s) respectively. Figure 5 shows the absolute trajectories of missiles and target in the longitude, latitude and height coordinate system under the cases above.

The 1th sensor offset error is (range 200 m, elevation angle $0.5^{\circ }$, azimuth angle $0.2^{\circ }$) and the 2th sensor offset error is (range 300 m, elevation angle $0.3^{\circ }$, azimuth angle $0.2^{\circ }$). Both two sensors’ standard deviation of measurement noises is (5 m, $0.{01}^{\circ }$, $0.1^{\circ }$) All the measurement errors and noises are added to ideal measurements to create reported values. Both two platforms’ initial attitudes are (yaw $1^{\circ }$, pitch $0^{\circ }$, roll $3^{\circ }$) and offset errors of these angles are $0.{01}^{\circ }$, which are added to initial attitudes to create deviated values. The sampling interval is 0.1 s. All the simulation results are obtained over 100 Monte Carlo trials. The performance of improved ECEF-KF algorithm proposed in the paper is compared with the method based on ECEF-LS in [24] and the standard Kalman filter, which is called traditional ECEF-KF.

The estimations of sensor errors are proposed in Figure 6, wherein the left column is about missile 1 and the right column presents the outcome of missile 2. The results of two cases are compared in one figure. It is obvious that the curves of case 2 jitter severely during 100∼200 s, which does not appear in the case 1. As mentioned in Section 4.2, the jitter is caused by the approach of latitudes of two missiles and target (see in Figure 5b). At 150 s, the two missiles move to nearly the same latitude (about $34.{01}^{\circ }$), the latitude of target is about $34.{0186}^{\circ }$, they are almost on the same latitude plane. That means the measurements of ECEF-KF become two dimensional, which causes the divergence during this time and converge gradually with the continual motion of missiles.

The Figure 7 shows the estimations of attitude biases. The divergency during 100∼200 s appears similarly under case 2. Besides, it is found in Figure 7c,f that the roll error increases after 200 s. This is because the longitudes of missiles come close to the targets’ at the end of track, which leads to the same phenomenon caused by the approach of latitude. Since the measurement equation of ECEF-KF is a first-order linear function of state, the estimations converge quickly from beginning (about 2 s in Figure 7a) and jitter near the truth.

To demonstrate the priority of proposed registration method for sensors on motivated platforms, Figure 8 illustrates the bias estimates of traditional ECEF-KF and improved one under case 1. The overlook of attitudes of orientated frames makes the former deviate from the truth as well as the disability to estimate the align errors. On the contrary, the latter which follows the truth closely gives better performance on the results. The reason causing the disappointment of traditional one is that the attitudes of missiles lead to the noncoincidence of LENU system and missile body system and influence the measurement obtained from sensors. For instance, the positive yaw value would increase the measurement of azimuth angle which is actually invariant in LENU system. However, the item ${\mathit{C}}_b^d$ of improved method transform the measurement from body system to LENU system, isolating the disturbance of frame angles.

The RMSE (Root Mean Square Error) of the estimation of two methods is listed in

Table 1. RMSE of registration errors.

Method	Platform	Range (m)	Elevation (${}^{\circ }$)	Azimuth (${}^{\circ }$)
Improved	Missile 1	0.1878	0.0126	0.0327
ECEF-KF	Missile 2	0.1888	0.0057	0.0255
Traditional	Missile 1	0.2667	0.0522	0.5557
ECEF-KF	Missile 2	0.2688	0.0475	1.0674

. Obviously, the improved ECEF-KF outperforms the original strategy in all terms, especially for the azimuth because of the relatively large value of yaw angle, which demonstrates the proposed algorithm has more advantage in spatial registration for mobile sensors.

Take case 2 for example, the target tracking is conducted on missile 1. Figure 9 compares the estimations of different schemes, wherein the left column presents the results of linear filters and the nonlinear filters are illustrated in the right list. The trajectory of target is estimated in the LENU coordinate system of missile 1 (see in Figure 5b). The parameters are given $\epsilon =0.001,\kappa =10$.

Remark 3.

It is notable that ε is the limit of (53) and κ is the scale factor of integrator in (54). The former wherein will influence the time of error compensation (the lager ε is, the longer the LFOF will be taken) and the latter wherein will enhance the stability of error estimations (the lager κ is, the smoother the estimation through LFOF will be).

Seen from the left column of Figure 9, the accuracy of estimation dramatically improved after compensation (the estimation of UCPLKF is much closer to the truth value than PLKF). This is because the constant biases are subtracted from the input of UCPLKF. But during 100∼200 s, the estimations of these biases deviate from the truth, resulting to the unsatisfactory performance of UCPLKF in this period. The CCPLKF which can effectively inhibit the jitter of curves through a condition-based LFOF is prior to UCPLKF. Since the LFOF only works when (53) satisfies, so it does not cause the time delay to the whole tracking process. In the right column, likewise, the CCUKF is the optimal method among the nonlinear tracking schemes.

The RMSE of the estimation of target state is listed in

Table 2. RMSE of tracking errors.

Method	RMSEx (m)	RMSEy (m)	RMSEz (m)	RMSEr (m)
PLKF	191.6083	269.8833	88.9778	342.6377
UCPLKF	6.3140	20.5581	24.1859	32.4938
CCPLKF	6.2862	18.8728	23.5337	30.9746
UKF	191.3337	269.8901	89.1703	342.7357
UCUKF	6.2568	20.5293	24.3977	32.3645
CCUKF	6.2366	18.8422	23.7803	30.8145

. Seen from the results, the accuracy of tracking system can be greatly improved through compensation and the conditional compensation methods (CCPLKF and CCUKF) have nearly the same and the smallest errors. The CCUKF although has slight edge over CCPLKF in the items RMSEx and RMSEy, but suffer from more error in RMSEz, which turns the advantage to disadvantage in joint distance. This result can be explained through the comparison of Figure 9c,f. As mentioned, the registration errors jitter severely during 100∼200 s, which are compensated directly to the measurement inputs of UKF yet indirectly introduced to PLKF. Thus, the latter has the better input-jitter tolerance, which can also be proved in Figure 10.

The estimations of CCPLKF and CCUKF are compared in Figure 10. Although the local magnifications in Figure 10a,b show the smoother curves of CCUKF than those of CCPLKF, but the Figure 10c exposes its flaw when the inputs jitter more severely. Moreover, the another fatal weakness of CCUKF is its relatively large computation. Given from simulation, the operation time caused by CCPLKF is 2.32 s, whereas CCUKF takes 5.62 s. In summary, the CCPLKF method keeps the best performance in target tracking.

6. Conclusions

In this paper, a novel spatial registration algorithm is proposed to estimate biases of measurements and orientation angles simultaneously. The linear and nonlinear filtering algorithms are introduced to solve the three-dimensional maneuvering target tracking problem. The estimations of registration method are used to modify the inputs of tracking scheme. In addition, a low pass first order filter is selectively taken to inhibit the jitter of estimation without the time delay. Simulation shows the good performance and robustness of the registration approach and the advantage of linear tracking method with modified inputs. Research results of this paper can also be extended to solve the registration and tracking of a dynamic network consisted of large quantities of sensors and multi-maneuvering targets [34,35].