Measurement-driven Gauss-Hermite particle filter with soft spatiotemporal constraints for multi-optical theodolites target tracking

Hongwei ZHANG, Pengfei LI

a School of Aeronautics and Astronautics, Sun Yat-sen University, Shenzhen 518071, China

b Army Academy of Artillery and Air Defense Zhengzhou Campus, Zhengzhou 450007, China

KEYWORDS Causality-invariant;Measurement-driven Gauss-Hermite Particle Filter(MGHPF);Multi-optical theodolite tracking system;Soft spatiotemporal constraints;Target maneuvering behavior

Abstract Multi-Optical Theodolite Tracking systems (MOTTs) can stealthily extract the target’s status information from bearings only through non-contact measurement.The constrained MOTTs are partially compatible, yet many existing research works and results are based on the known model, ignoring its discrimination with the target maneuvering behavior pattern.To compensate for these mismatches, this paper develops a Measurement-driven Gauss-Hermite Particle Filter(MGHPF), which elegantly fuses the spatiotemporal constraints and its soft form to perform MOTT missions.Specifically, the target dynamic model and tracking algorithm are based on the target behavior pattern with the adaptive turn rate, fully exploiting the spatial epipolar geometry characteristics for each intersection measurement by a minimax strategy.Then,the center of the feasible area is approximated via the analytic coordinate transformation, and the latent samples are updated via the deterministic Gauss-Hermite integral method with the target’s predictive turn rate.Simultaneously, the effects of truncation correction and compensation feedback from the current measurement and historical estimation data are adaptively incorporated into the PF’s importance distribution to cover the mixture likelihood.Besides,an effective causality-invariant updating rule is provided to estimate the parameters of these soft spatiotemporal constrained MOTTs with convergence guarantees.Simulated and measured results show good agreement; compared with the stateof-the-art Multi-Model Rao-Blackwell Particle Filter (MMRBPF), the proposed MGHPF improves the filtering accuracy by 7.4%-34.7% and significantly reduces the computational load.

1.Introduction

Multi-Optical Theodolite Tracking systems (MOTTs) refer to extracting useful information about the target’s status, such as positions and velocities from only the noisy bearing measurements collected from the multi-station optical theodolites.1–3An airspace point target can be located via multi-optical theodolites using the interaction measurement method,4as shown in Fig.1, where s1, s2, h1, and h2are the theodolites and their heights.xw, yw, and zware the three axes of the world coordinate system.zw1and zw2are the horizontal axes of the dual-station optical theodolites.T, T1, and T2are the point target, projection points on the theodolite’s contour and horizontal planes.A1,A2,E1,and E2are the azimuth and elevation angles,respectively.I12is the intersection angle.This non-contact tracker contains the unique property of concealment and safety.MOTTs have proven to be very important and have found wide applications in many different fields,such as missile tracking,5navigation and guidance,6aircraft monitoring,7and so forth.Due to the congenital lack of distance information, the observation information is incomplete, especially in the case where the intersection angle in Fig.1 is beyond the scope of 30°- 150°.8,9On the other hand, most unknown target maneuvers lack sufficient prior information,10,11and it is hard to establish an accurate target dynamic model to describe both the temporal evolution and spatial characteristics.This kind of spatiotemporal uncertainty brings many thorny problems to the Bayesian estimation for MOTTs,such as mixture likelihood supported by different areas,roundoff errors caused by the uncertain noise from the coordinate transformation and outside environment, and so on.12–14The observability, target dynamic model, and tracking algorithm are critical elements for a successful MOTT mission.

The observability criteria for MOTTs mainly depends on the intrinsic and extrinsic parameters,such as calibration,9optimal deployment,15and ad hoc geometric models.16Usually,spatial measurements are processed in spherical or polar coordinates,and the target dynamics are often modeled in Cartesian coordinates, so the output matrix for the spherical measurements needs to be linearized using the Extended Kalman Filter(EKF)method.17To improve the estimation accuracy in some complex nonlinear situations,18various Sigma point filters19such as the Unscented Kalman Filter(UKF),Cubature Kalman Filter(CKF),20and Gauss-Hermite Kalman filter(GKF)have arisen in MOTTs application.These Sigma KF methods directly approximate the Probability Density Function (PDF)for the target’s posterior.19Unfortunately, due to the random nonlinearity, the distributions for the statistical noise of azimuth and elevation angles are always non-Gaussian during the target’s temporal evolution.1,17Usually, researchers use student-t distribution,flicker/glint noise,or jump value to simulate the non-Gaussian noise.21To overcome the Gaussian premise for these KF-type filters,the Particle Filter(PF)methods are used to cope with the nonlinear non-Gaussian estimation problems via a set of weighted importance samples.22To reduce the variance of the weighted particles,23,24many studies have attempted to use deterministic numerical approaches, such as the EKF,UKF,GKF to generate the importance function for PF.These novel algorithms are hence named the EPF,25UPF,26and GPF.27To further refine the state space,many ad hoc Multiple Model(MM)estimators are designed by merging a set of nonlinear filters based on different theories,such as the interacting strategy28and Rao-Blackwell theory.29Nevertheless, for a highly maneuvering airspace point target tracked via MOTTs, the high model noise variance and model variations must be considered in the target dynamic models for the single-model and MM filters,respectively14.

Fig.1 Target tracking via dual-station optical theodolites.

On another research frontier, data-driven approaches have received much attention in designing target dynamic models and tracking algorithms for MOTTs.Based on this,the recursive and derivative-based Gaussian Process(GP)methods have been developed for target tracking and smoothing, representing the possible target trajectories by using a distribution over an infinite number of functions rather than a finite set of models.14,30To further improve the accuracy of the GP hyperparameters for the high maneuvering target, many works have attempted to perform the state estimating and learning steps recursively by adding additional terms such as historical data,31inducing point set representation32and optimal strategy.33During these GP recursive processes, choosing an appropriate information measure to quantify the impact of uncertainties is critical to obtain good performance for target trajectory estimation.More recently,constrained optimization methods have attracted much attention in MOTTs applications.Target dynamic systems always involve various state constraints, for example, physical laws such as energy conservation laws,12mathematical properties such as quaternion norm properties,34and measurement conditions such as symmetric coupling.35Essentially, the compatibility and modeling of the constrained dynamic systems still suffer from a serious flaw in incongruous situations but can be rescued by an intrinsic correction of the method36.

Fundamentally,the constrained MOTTs are partially compatible, whereas the dynamics and constraints are not completely congruous.Soft constraints make sense for cases where the constraints are heuristic rather than rigorous.37However, many existing research results are based on the known approximation model set, which is the mathematical representation or description of the target behavior pattern at a certain accuracy level.24Little attention is paid to the discrimination with the true behavior mode whenever the mismatch between the approximation model and the true behavior mode is of concern.To address these issues, this paper develops a Measurement-driven Gauss-Hermite Particle Filter (MGHPF) for MOTTs, aiming to incorporate the soft spatiotemporal constraints into the dynamic system and tracking algorithm with convergence,particularly in cases where the constraint function has some uncertainty or fuzziness.The main contributions lie in the following three aspects.

(1) The target dynamic model and tracking algorithm are based not on the approximation model but on the actual target behavior pattern with the target’s turn rate,which is predicted via analytic coordinate transformations with spatial epipolar geometry constraints.

(2) Soft spatiotemporal constraints that contain physical meaning are implemented into the target’s temporal evolution via a minimax strategy to compensate for the mismatches between the prior, likelihood, and posterior of a target.

(3) An effective causality-invariant updating rule is developed to extract the parameters of the soft-constrained MOTTs, the convergence and computational complexity are interpreted by using the same-order infinitesimal numerical error from the viewpoint of pure mathematics.

The remainder of the paper is organized as follows.The dynamic model and problem formulation are briefly described in Section 2.The proposed MGHPF algorithm is derived and discussed in Section 3.Illustrated examples are exhibited in Section 4.The study is concluded in Section 5.

2.Discrete-time dynamic model and problem formulation

2.1.Discrete-time dynamic model

The main task of MOTTs lies in extracting the current target status Xk?RdXfrom the indirect measurement Zk?RdZ,where k ?N is the discrete time index, dXand dZare dimensionalities for the state and measurement vectors.1–3In the Three-Dimensional (3D) Cartesian coordinate, the target’s base state can be expressed as where the turn rate ωk>0 and ωk<0 represent the clockwise and anticlockwise motions.Tkis the measurement interval.qx,qy,qzare the intensity of acceleration noise on x, y, and z axes.02×2denotes a two-by-two matrix of zeros.

2.2.Statement of problem formulation

Theoretically, there is often a causal, but inaccurate or noisy relationship between the true parameters and the measurement of a target.36,39Practically, even the dual-station optical theodolites are optimally deployed, and the initial error is set to be Gaussian,the noise for azimuth and elevation angles will become non-Gaussian during the target’s temporal evolution process due to the inherent measurement nonlinearity,as illustrated in Fig.2.In fact, the numerical approximation of the state estimation for the MOTTs is accurate only in the feasible area.

Physically, the energy of the measurement noise cannot be supported infinitely.12Mathematically,the subset IX,k（Zk)that satisfies the causal mapping relationship should be in the feasible area of interest AX,k（Zk), which can be represented by

Fig.2 Bearing measurement collected from dual-passive theodolites.

Clearly, p1（?) in Eq.(8) is non-Gaussian, so Eq.(7) cannot be available with a closed-form solution.Carcia-Fernandez et al.12update the ith Gaussian component using UKF and select λτ,kby computing the proportion of the traces for the different covariance matrices.Alternatively,we approximate p1（?)using an analytical method associated with the soft spatiotemporal constraints.To measure the truncation effects flexibly,we compute the proposal parameter λτ,kand introduce the mixture priors to the PF framework by establishing the suboptimal importance distribution.Section 3.2 gives the details.

3.Implementation of proposed methodology

3.1.Approximate center of feasible area

where φkis the directional angle, ωmaxis the maximum turn rate.Zk｜k-1and Skare the prediction and innovation covariance for the spatial measurements.

The minimax filters based on the game theory are specifically designed for robustness.11,28Accordingly, we model the MOTT problem as a game where one player is the estimator that attempts to attain the accurate target turn rate, whereas the other player is the outside environment with uncertain noise.We rewrite the minimax objective function as

Geometrically,the image points for the spatial point should be at the polar lines.Mathematically, this constrained geometry relationship can be modeled by using the fundamental matrix F35as

where fj,Gj,Tj,φj,rjand Cjare the focal length,rotation and turn matrices, azimuth and elevation angles for the optical axis, and the 3D position for the optical center of the jth theodolite.

Solving Eq.(14) by the least square method42yields the maximum likelihood position Xmax,kand variance Pmax,kas

Fig.3 Spatial intersection measurement with epipolar geometry constraints of dual-station optical theodolites.

Fig.4 Feasible area for maneuvering target in Cartesian coordinates.

Actually,in Cartesian coordinate system,the target maneuvers can occur in any direction,i.e.,φL,k?［0,2π].The feasible area defined in Eq.(6) should therefore be a spherical surface as shown in Fig.4 (c).Without loss of generality, by rotating the result in Eq.(16)around xLaxis with a direction angle φL,k,we can extend Eq.(16) as

3.2.Suboptimal importance distribution

3.3.Summary and analysis

The proposed MGHPF method is summarized in the following Algorithm 1.

Initialization: Size of importance samples: Ns; threshold:η ?［0,1]; original prior: X0,k,P0,k( )；index: ←k-1.Involve the spatial characteristics into the temporal evolution by using Eqs.(13)-(15).Approximate the center of the feasible area:pc Xk Xk-1,a1:k( )（｜)←N Xc1,k,Pc1,k.For k=1 Generate suboptimal importance distribution Eq.(30).For n=1,2,???,Ns Importance samples: Xnk ←π Xnk Xn 1:k-1,Z1:k().( ).Normalize the importance weights w～n k up to a constant by using Sequential set: Xn 1:k ← Xn ( )and Pn 1:k ← Pn 1:k-1,Xn k 1:k-1,Pn k Eqs.(37)-(38).End Update the output:^Xk｜k =ΣNs ( )T Endif k:=k+1 n=1w～n kXnk,^Pk｜k =ΣNsn=1w～n k Xnk- ^Xk｜k( )Xnk- ^Xk｜k

(1) Convergence analysis

(2) Computational complexity analysis

4.Examples

To validate the effectiveness of the proposed MGHPF algorithm,100 runs of MC simulation were carried out for simulation scenarios and real measurement data.We choose four model-based algorithms to compare and analyze: IMMEKF,IMMUKF(the interacting MM-based estimators with parallel EKF and UKF), Multiple Model Rao-Blackwell Particle Filter (MMRBPF),29and Constrained Auxiliary truncation PF(CAPF) based on the constant velocity model.42The Markov switching model for the three MM-based filters is chosen from Ref.17, the constant turn rate for the clockwise and counterclockwise motions are set to ω=±3 （o)/s, respectively.The initial model transition probability matrix is given as

The following two quantitative parameters are adopted to compare the filtering performance, (A) Root Mean Square Error (RMSE) of the target position, which can measure the total average filtering performance at each time k.(B)Root Time Averaged Mean Square Error (RTAMSE) of the target position,43which can measure the total average filtering performance after the target maneuvers.The definitions are

where Mc is the total runs of the MC simulation experiments.tmaxand teare the total measurement sojourn and the termination time step of the target maneuvers, respectively.

4.1.Simulation scenario

4.1.1.Simulation scenario and parameters

4.1.2.Effect of process noise

Figs.6, 7, and 8 show the RMSE curves on position, pure errors on x axis, y axis, and z axis for the IMMEKF,IMMUKF, MMRBPF, CAPF, and MGHPF algorithms under different process noise under Case 1, Case 2 and Case 3, respectively.Tables 1, 2, and 3 report the statical average maximum values, range, mean and variance for RMSE on position,and errors on the x axis,y axis,and z axis for the five algorithms under different process noise.The MM-based filters show more sensitivity to the change of process noise.

Fig.5 3D simulation trajectory in Cartesian coordinate.

Fig.6 Filtering performance comparison under Case 1.

Fig.7 Filtering performance comparison under Case 2.

Fig.8 Filtering performance comparison under Case 3.

In Fig.6, the IMMUKF and MMRBPF show relatively larger errors, especially when the aircraft climbs at time t=25 s and t=60 s, the peaks of the estimated fluctuation are as prominent as 43 m and 22 m.This is mainly because the selection range of Sigma points or importance samples for UKF and PF highly rely on the initial prior distribution.With the increase of the process noise, the degree of fluctuations for the position RMSE and errors on the x axis, and z axis become more severe during the steady motion phase,and more moderate during the climb maneuver phase.In Fig.7 and Fig.8, the RMSE curves for IMMUKF show the most considerable maximum fluctuation.Both quantitative and qualitative comparisons show that the proposed MGHPF method exhibits the most minor error and the beststability under different process noise; this is mainly because of the following two reasons: (A) The accuracy of importance samples is enhanced by exploiting the high precision of Gauss-Hermite rule into the importance distribution.(B) The mismatch between the prior and posterior of a target can be compensated via the optimal mixture importance distribution.

Table 1 Average position RMSE and fluctuations on three axes: mean, variance, max, range under Case 1.

Table 2 Average position RMSE and fluctuations on three axes: mean, variance, max, range under Case 2.

Table 3 Average position RMSE and fluctuations on three axes: mean, variance, max, range under Case 3.

4.1.3.Effect of measurement noise

Figs.9, 7 and 10 show the qualitative comparison of filtered results for the IMMEKF, IMMUKF, MMRBPF, CAPF,and MGHPF algorithms under different measurement noise variances, including the RMSE curves on position, and errors on the x axis,y axis,and z axis under Case 4,Case 2 and Case 5,respectively.Tables 4,2,and 5 report the quantitative comparison of the filtered results for the five algorithms under different measurement noise, including the statistical average maximum, range, mean and variance for the position RMSE and the errors on the x axis, y axis, and z axis.

With the increase of the measurement noise, the filtering performance for all the five filters degrades obviously.In Case 4, when the aircraft climbs at t=25 s and t=60 s, the peaks of the estimated fluctuation for IMMUKF are as prominent as 14.95 m and 32.39 m.In Case 5, these two peaks for IMMUKF are as prominent as 22.75 m and 39.39 m.This phenomenon is consistent with the fact that the performance of the KF-type filters is limited by the Gaussian distribution,and UKF is sensitive to the outliers.3,12Both quantitative and qualitative comparisons show that the proposed MGHPF method shows a minor sensitivity to the measurement noise.This is mainly because of the following two reasons: (A) The diversity of importance samples is improved by fusing the mixture likelihood into the importance distribution.(B) The mismatch between the likelihood and posterior of a target can be compensated via modifying the truncated prior and incorporating it into the mixed importance distribution of a target.

Fig.9 Filtering performance comparison under Case 4.

Fig.10 Filtering performance comparison under Case 5.

Table 4 Average position RMSE and fluctuations on three axes: mean, variance, max, range under Case 4.

Table 5 Average position RMSE and fluctuations on three axes: mean, variance, max, range under Case 5.

4.1.4.RATMSE and average time

It is worth mentioning that, as can be seen from the five qualitative RMSE curve figures,for the sigle-model-based CAPF in the fifth segment from t=85 s to t=100 s,the curves for the position RMSE and errors on the x axis, y axis, and z axis exhibit the most significant fluctuations.Tables 6 and 7 report the maximum values, average mean and variances of the RATMSE for the five algorithms under different processes and measurement noise, respectively.

Both quantitative and qualitative comparisons indicate that CAPF shows more serious instability after the target maneuvers.Fortunately, the proposed MGHPF offers no less stability than the MMRBPF algorithm regarding RATMSE.There are two main reasons as follows, (A) The spatial epipolar geometry constraints are fully exploited in each intersection measurement.(B) The importance samples are propagated using the target’s predicted turn rate, which is driven by the effective measurement sequence.

Additionally, Table 8 lists the execution time required for one MC run in Case 2 for the five algorithms in comparison.As expected, the MGHPF algorithm shows a slightly higher calculation requirement than the conventional IMMEKF and IMMUKF methods.Compared with the state-of-the-art MM RBPF, the MGHPF improves the filtering accuracy by 7.4%-34.7%and reduces the computational load significantly.

4.1.5.Non-Gaussian noise

Assume the target glint and the measurement disturbance is non-Gaussian.21As our simulation scenario shows,the outliers have a very high variance but relatively low occurrence probability.A Laplacian distribution can describe these outliers better because it has a heavier tail than a Gaussian distribution υGwith the same variance.For robustness, we use the score function to mingle a low-probability high-amplitude Laplacian noise WLaand a high-probability low-amplitude Gaussian noise WGto model the glint noise Wg.

Tactfully,we select the moderate variance in Case 2 and the relatively larger variance in Case 5 as the parameters for the Gaussian noise WGand the thicker-tailed Laplacian noise WLa, respectively.Denote ε as the glint probability, the PDF for Wgcan be represented as

We set ε=0.05.Figs.11(a),(b),(c)and(d)show the curves for the position RMSE,errors on the x axis,y axis,and z axis for the IMMKF,IMMUKF,MMRBPF,CAPF and MGHPF algorithms under glint noise, respectively.Table 9 reports the maximum values, statistical average mean and variance of RMSE and RATMSE for the five algorithms under glint noise.

It can be seen from the qualitative comparison in Figs.11(b),(c)and(d),the RMSE curves of IMMUKF show the most significant fluctuation.The maximum value for RMSE is close to 40 m.Compared with the IMMEKF and IMMUKF, thesingle-model based CAPF shows larger fluctuation on x axis,and smaller fluctuation on y and z axes.The MMRBPF has the smallest maximum values,the MGHPF provides the smallest mean and variance for both the RMSE and RATMSE.

Table 6 max, mean, and variance for RAEMSE under different process noise.

Table 7 max, mean, and variance for RAEMSE under different measurement noise.

Table 8 Execution time required for one MC simulation experiment.

Both quantitative and qualitative comparisons show that the proposed MGHPF method is considerably self-adjustable to the uncertain noise.The two main reasons are(A)The state is normed via the minimax strategy with spatial epoliar geometry constraints.(B) An effective causality-invariant updating rule is provided to learn the parameters of these spatiotemporal constrained MOTTs with the convergence guarantees.

4.2.Real unmanned aerial vehicle tracking

In this subsection, a prototype non-contact dual-station theodolites measurement system was built to track a moving unmanned aerial vehicle (UAV) using the proposed MGHPF algorithm.Fig.12(a)shows the schematic for the spatial interaction measurement method.The outdoor test site was 140 m long from east to west, and 50 m wide from north to south.According to the theodolite parameters on the training ground, the trajectory range for UAV is［0 m, 140 m]× ［0 m, 50 m]× ［0 m, 500 m].The coordinates for the optical centers of the dual-station prototype theodolites are located at ［15 m, 25 m, 1.65 m] and［125 m, 25 m, 1.65 m], respectively.

The total flight time was 60 s,we recorded 20 measurement points,and attained 16 valid measurement points by using the spatial interaction measurement method, remoting 4 gross errors.Figs.12(b),(c),and(d)show the measurement and filtered results for the 3D position of the UAV on x axis, y axis,and z axis,respectively.The columnar points labeled with red,cyan, and blue colors are the data returned from the UAV itself,the measurement calculated using the spatial intersection method, and the filtered results attained via the proposed MGHPF algorithm, respectively.

Table 9 Quantitative statistics of average filtering performance under glint noise.

Fig.12 Measurements and filtered results for a moving UAV.

Quantitative comparison results indicate that the simulated and measured results show good agreement.This is mainly because we introduce the effective spatial measurement into the establishment of the suboptimal proposal distribution.In our approach, the effects of correction and compensation from the spatial measurement information and historical estimation are utilized simultaneously, ensuring the diversity and accuracy of the importance samples and modulating their weights.So, the posterior distribution for the target can be well characterized.Theoretically, we have verified that if we can design the suboptimal importance distribution to cover the mixture priors and modulate their influence effectively, the filter can be adaptive with the actual measurement environment.

5.Conclusions

The non-contact MOTTs contain the unique property of concealment and safety with high measurement accuracy.Most published works and results only take into account the compatible constrained constructure.Therefore, incorporating spatiotemporal constraints and causality-invariance into the dynamic model and tracking algorithm for MOTTs is promising but challenging.To this end, in this paper, we develop a measurement-driven algorithm based on a minimax strategy with soft spatiotemporal constraints, which elegantly integrates the target’s spatial characteristics and soft-constrained to accomplish the MOTT mission.Specifically, our proposed dynamic model fully exploits the target true behavior pattern and spatial measurement information by combining PF with the deterministic Gauss-Hermite rule and adaptive measure.In addition, we derive the causality-invariant updating rule to extract the status parameter and prove the convergence and computational complexity of the algorithm from the viewpoint of pure mathematics.Empirically,we conduct the experiments under eight different noise conditions.Simulation results demonstrate that MGHPF remarkably outperforms the state-of-the-art approaches.

Since the soft-constrained has an intimate relationship with the physical system, future work can include further applications in data association for multiple target tracking in complex environments.

Declaration of Competing Interest

The authors declare that they have no known competing financial interests or personal relationships that could have appeared to influence the work reported in this paper.

Acknowledgements

This study was co-supported supported in part by the Guangdong Basic and Applied Fundamental Research Fund Project, China (No.2019A1515111099).Open Research Fund of CAS Key Laboratory of Space Precision Measurement Technology, China (Nos.SPMT2021002, SPMT2022001).