Bearings-Only Target Tracking with an Unbiased Pseudo-Linear Kalman Filter

: In bearings-only target tracking, the pseudo-linear Kalman ﬁlter (PLKF) attracts much attention because of its stability and its low computational burden. However, the PLKF’s measurement vector and the pseudo-linear noise are correlated, which makes it suffer from bias problems. Although the bias-compensated PLKF (BC–PLKF) and the instrumental variable-based PLKF (IV– PLKF) can eliminate the bias, they only work well when the target behaves with non-manoeuvring movement. To extend the PLKF to the manoeuvring target tracking scenario, an unbiased PLKF (UB–PLKF) algorithm, which splits the noise away from the measurement vector directly, is proposed. Based on the results of the UB–PLKF, we also propose its velocity-constrained version (VC–PLKF) to further improve the performance. Simulations show that the UB–PLKF and VC–PLKF outperform the BC–PLKF and IV–PLKF both in non-manoeuvring and manoeuvring scenarios.

There are two common ways to deal with the nonlinearity in the BOT problem. The first way is to choose the nonlinear Bayesian filtering algorithms. Early approaches are based on the extended Kalman filter and its improved versions [1,13,14]. However, the extended Kalman filter uses the first-order Taylor expansion to replace the corresponding nonlinear function, which will certainly lead to truncation errors. Other more sophisticated Kalman filters, such as the unscented Kalman filter [15] and the cubature Kalman filter [16], applied to the BOT problem, can be found in [17,18]. Another important approach is based upon particle filtering (see, for instance, [19,20]). However, the high computational requirements of the particle filter would involve high-performance expensive hardware that discourages its implementation in BOT applications. It is important to also mention here the non-Bayesian-based shadowing filter that, recently, has been applied to the BOT problem [21]. However, such a filter is a kind of gradient descent algorithm, hence, it is highly sensitive to the step size as well as the computation of the Jacobian matrix [22].
Another way to cope with the nonlinearlity in BOT is to convert it into linear form, and the most common method used is the pseudo-linear Kalman filter (PLKF) [23]. It has lower computational complexity and is more robust compared with the nonlinear Bayesian

Notations
Throughout the paper, vectors and matrices are denoted by boldface lower-case and upper-case letters, respectively. E{·} and tr{·} denote the expectation operator and trace operator, respectively; I n×n denotes the n × n identity matrix; 0 n×m denotes the n × m zero matrix; the superscript "BC", "IV", "UB" and "VC" denote, respectively, the vector or matrix in BC-PLKF, IV-PLKF, UB-PLKF and VC-PLKF, and any vector or matrix with this kind of superscript has the same dimension as its original form in PLKF; the superscript "T" denotes the transpose operation of a vector or matrix; the superscript "−1" denotes the inverse operation of a matrix; the superscript "+" means that the vector or matrix belongs to the posterior estimate, and "−" means that the vector or matrix belongs to the predictive estimate; y = x(i : j) means that y consists of the ith through the jth element of x, and · denotes the l-2 norm of a vector.

Materials and Methods
In this section, we first formulate the 2D BOT problem, then we provide brief reviews of the PLKF and its two improved versions-the BC-PLKF and the IV-PLKF. After these, we propose the UB-PLKF to cope with the biased estimate problem in PLKF. Then, we incorporate the velocity constraint information into the UB-PLKF to construct the VC-PLKF.

Problem Formulation
We focus on the BOT problem in 2D-plane with the bearing measurements received by a moving observer, which is shown in Figure 1. We denote by p k = [p x,k , p y,k ] T ∈ R 2×1 and v k = [v x,k , v y,k ] T ∈ R 2×1 the position and velocity of the target at the discrete time index k = 1, · · · , N, respectively. We then have target state vector The observer position is represented by the vector s k = [s x,k , s y,k ] T ∈ R 2×1 , which is assumed to be known. In the following, we assume that the target is moving under the constant velocity model as discussed in [32], then the target dynamic model is: where F ∈ R 4×4 is the transition matrix, and w k−1 ∈ R 4×1 is the process noise, which is assumed to be a Gaussian random vector with mean 0 4×1 and covariance matrix Q ∈ R 4×4 . The matrices F and Q are given by [32] where T is the sample interval, which is assumed to be equal to 1 s throughout the paper. q x and q y are respectively the power spectral densities of w k in two coordinates.
We adoptβ k to denote the bearing collected by the observer at k. It can be written as where ∆x k = p x,k − s x,k , ∆y k = p y,k − s y,k , tan −1 is the four-quadrant inverse tangent function, and n k is the bearing noise, which is assumed to be Gaussian with zero mean and variance σ 2 k . σ k is assumed to be known, and n k is independent of the process noise w k throughout the paper.

Overview of the PLKF, BC-PLKF, and IV-PLKF
This subsection first discusses the PLKF algorithm and then analyses its bias. Afterwards, the improved PLKF algorithms-BC-PLKF and IV-PLKF in [25] are introduced.

PLKF
Obviously, we have the following equation: multiplying d k = ∆x 2 k + ∆y 2 k to both sides of (6), and notice that d k × cos β k = ∆x k , d k × sin β k = ∆y k , we have Then we have the following pseudo-linear measurement equation: where z k = [sinβ k , − cosβ k ]s k , H k = [sinβ k , − cosβ k , 0, 0], and η k = −d k sin n k is the pseudo-linear noise. The variance of η k is denoted by R k = E{η 2 k } = d 2 k · E{sin 2 n k } ≈ d 2 k σ 2 k for small bearing noise n k . Combining (1) and (8), the PLKF is given by step 1 Predicting the state:x step 2 Predicting the covariance matrix: step 3 Calculating the gain matrix: step 4 Updating the state:x step 5 Updating the covariance matrix: wherex k|k−1 ∈ R 4×1 and P k|k−1 ∈ R 4×4 denote, respectively, the one-step prediction and the corresponding prediction error covariance matrix, and K k ∈ R 4×1 denotes the Kalman gain, andx k|k ∈ R 4×1 and P k|k ∈ R 4×4 are, respectively, the posterior state estimate and corresponding estimation error covariance matrix. Since the true values of d k and R k are not available, the approximated values are used,

BC-PLKF
According to [25], the posterior bias e k =x k|k − x k of the PLKF can be written as: where The first term A k propagates the error from the last time k − 1, and it is not the cause that leads to the biased estimate in PLKF. The second term B k propagates the bias from the correlation between H k and w k−1 . If we assume the process noise w k is relatively small, then B k can be ignored. When it comes to the third term C k , it propagates the bias from the correlation between H k and η k , which cannot be ignored since they all contain the bearing noise n k . Thus, we can have the conclusion that it is the correlation between the H k and η k that leads to the biased estimate, and if we can compensate the bias from C k , the bias in x k|k can be reduced.
So the bias compensation can be achieved bŷ However, the true value of C k cannot be obtained; we are supposed to replace it with its conditional expectation based onx k|k , which can be formulated as: [25] where M = [I 2×2 , 0 2×2 ], and we use the formula P k|k = P −1 Finally, the BC-PLKF is summarized below [25]: Predicting the state:x step 2 Predicting the covariance matrix: step 3 Calculating the gain matrix: step 4 Updating the state: step 5 Updating the covariance matrix: step 6 Bias compensation:

IV-PLKF
Unlike the BC-PLKF, the IV-PLKF tries to construct a vector G k , which is chosen to be independent of η k , and at the same time is strongly correlated with H k . The optimal form of G k is the noise-free version of H k [25]: Since the true bearing measurement β k is unavailable, a suboptimal vector based upon the estimateβ BC k from BC-PLKF is chosen: The complete IV-PLKF algorithm is [25]: step 2 Predicting the covariance matrix: step 3 Calculating the gain matrix: step 4 Updating the state: step 5 Updating the covariance matrix: step 6 Bias compensation: step 7 IV estimation: We shall notice that, although the asymptotic convergence of the IV based method is verified in [26], the performance of the IV-PLKF is still limited by the BC-PLKF due to the usage ofx BC k|k in calculatingβ BC k . When the BC-PLKF performance deteriorates in the manoeuvring scenario, the IV-PLKF will also suffer.

The Proposed UB-PLKF and VC-PLKF
We first propose the UB-PLKF to cope with the biased estimate problem in PLKF. Then we incorporate the velocity constraint information into the UB-PLKF to construct the VC-PLKF.

UB-PLKF
Revisiting the matrix H k in (8), we can rewrite it as follows: where we assume cos n k ≈ 1 and sin n k ≈ n k for small bearing noise. Substituting (41) into (8), we get: here we also assume that and multiplying 1/m k to both sides of (42), we have: Notice that in (43), the measurement vector is independent of the noise, and the noise here is the bearing noise n k , not the pseudo-linear noise η k . Based on (43), the expectation of C k is It is clear that (44) equals 0 becauseH k is no longer correlated with n k . Then, an unbiased PLKF algorithm is obtained as follows: Predicting the state:x step 2 Predicting the covariance matrix: step 3 Calculating the gain matrix: step 4 Updating the state: step 5 Updating the covariance matrix: Obviously, the true values of the bearing β k and m k in (43) cannot be obtained, we replace them with corresponding approximated versionsβ UB k andm k , which are defined as follows: We emphasise here that the main idea behind the IV-PLKF and the UB-PLKF is similar. They both try to construct a noise-free measurement vector. However, the IV-PLKF performance is limited by the BC-PLKF, while the UB-PLKF does not suffer from this. The advantages of the UB-PLKF will be verified in the simulation part.

VC-PLKF
Based on the UB-PLKF, we develop its velocity constrained version in this part. First, we shall handle an optimisation problem with an equality constraint. Let v k|k =x UB k|k (3:4) and e v,k = v k − v k|k , we have: where Let v k|k−1 =x k|k−1 (3:4) in (45), and according to the Kalman formula, we have: where k =z k −H kx UB k|k−1 . Substituting (55) into the velocity constraint in (54), we have: According to the Joseph formula [33], the update formula of the partitioned posterior covariance matrix can be written as: where and Utilising (56) and (57) and the Lagrange multiplier method, we can construct the performance index  k as follows: Taking the first derivative of  k with respect to K v,k and setting it to zero yields: Then we have the first-order condition: Substituting (62) into (56), and according to [27], the scalar equation with λ k can be obtained as: where˜ k = 2 k /W k . Then, λ k can be written as where Notice that we use −b − , because the minimum performance index  k occurs when the minus sign is chosen according to [27].
Substituting (64)-(67) into (62) we get: where the asterisk was added to distinguish from the unconstrained form K v,k . (see Appendix A).
step 3 Calculating the gain matrix for UB-PLKF: x VC k|k =x k|k−1 + K VC k k (78) In most cases, however, what we know is the target velocity range, and this means we should handle a problem with inequality constraints. To translate the inequality constraint into the equality constraint, we adopt the rule in [29], that is, Here, v min and v max denote the lower and upper bounds of the velocity, respectively. Equation (80) means that if v k|k ∈ [v min , v max ], the steps after (75) are avoided, and we just outputx VC k|k =x UB k|k and P VC k|k = P UB k|k . Otherwise, if v k|k is less than v min or greater than v max , we do the VC-PLKF with the equality constraint with v = v min or v = v max , respectively.
We shall emphasise that, although the VC-PLKF and the algorithms in [29] all make use of the velocity constraints, the VC-PLKF constructs a norm-constrained Kalman filter, which is an iterative algorithm; however, algorithms in [29] are all based on the batch algorithms, which are computationally expensive and can hardly handle the manoeuvring target tracking problem.

Results
In this section, the proposed UB-PLKF and VC-PLKF are compared with the BC-PLKF and the IV-PLKF via Monte Carlo (MC) simulations. We adopt the root mean square error (RMSE) and the time-averaged RMSE as the performance metrics.
Assuming thatx i k|k is the state vector that we estimate at k via i th MC simulation. We can define the RMSEs obtained from M = 300 MC simulations as follows: (3:4). The time-averaged RMSEs are also adopted and are computed as [25]: where U = N − P + 1 is the effective time that takes into account the computation of time-averaged RMSEs, N = 350 is the total number of time index k, and P = 60 is chosen to eliminate the influence of the initial error according to [25].

Scenario 1: Non-Manoeuvring Target Tracking
We first test the filters' performance against poor initialisation and poor target sensor geometry with a non-manoeuvring scenario, which is given in Figure 2. The initial state vector of the target is x 0 = [400, 400, 3, 0] T , and to test the performance against poor initialisation, we set the initial state of the estimatex 0|−1 Gaussian, and its mean is [500, 300, 2, −1] T and the covariance matrix is P 0|−1 = diag (625, 625, 4, 4). The observer starts at [−500, −500] T m and follows a linear path with a constant speed of [0, 3] T m/s for the first 180 scans. Then, it executes a coordinated turn for 50 scans with a turn rate of 1.8 • /s followed by 120 scans, where it moves in a straight line with velocity [3, 0] T m/s. Notice that at the last 120 scans, the bearing rate between the target and the observer is 0, which constructs the case of poor target sensor geometry. q x and q y in (3) are q x = q y = 0.2 m 2 /s 3 . The standard deviation σ k = 5 • is assumed to be time invariant. The upper bound of the velocity is v max = 6 m/s, while the lower bound is v min = 1 m/s. Figure 3 shows the RMSEs for different algorithms. The IV-PLKF and BC-PLKF show similar performances in RMSEs of both the position and velocity, and so do the UB-PLKF and VC-PLKF. However, the biases of the UB-PLKF and VC-PLKF are much smaller than the BC-PLKF and IV-PLKFs, and this suggests that splitting the noise n k away from the H k is more effective than subtracting the bias from PLKF. Divergence problems have not been present in RMSEs of UB-PLKF and VC-PLKF, and this suggests that the proposed UB-PLKF and VC-PLKF can work well in conditions of poor initialisation and poor target sensor geometry.   9.45] T m/s, and executes a coordinated turn for the whole 350 scans with a turn rate of −1.8 • /s, where "−" means the anticlockwise direction. We also adopt the Cramer-Rao lower bound (CRLB) [34] and the constrained Cramer-Rao lower bound (CCRLB) [35] in this scenario to evaluate the performance of the four algorithms (the derivation of CRLB and CCRLB can be found in Appendix B).   Figure 5 plots the RMSEs and CRLB for the four algorithms. The RMSEs of the VC-PLKF are closest to the CRLB and CCRLB, followed by UB-PLKF. We also find that the velocity RMSEs of UB-PLKF and VC-PLKF decrease after the manoeuvring, which means that the proposed algorithms are more adaptable to abrupt changes in the velocity. Figure 6 shows the time-averaged RMSEs for σ k ∈ {1 • , · · · , 10 • }; it also verifies that the VC-PLKF and UB-PLKF outperform the BC-PLKF and IV-PLKF. The time-averaged RMSEs of the four algorithms are also flat, which proves that the four algorithms all have good robustness to the different levels of the bearing noise.  To test the performance of the proposed algorithms in the case in which there exists a noise statistics mismatch, we again assume that the deviation σ k is time-varying. When k < 51 or k > 201, σ k = 3 • , whereas σ k = 7 • at the other times. The other settings remain unchanged. Figure 7 shows the RMSEs of different algorithms. It is clear that the position RMSEs of the BC-PLKF and IV-PLKF get worse when the mismatch begins, and retain high levels the rest of the time. However, the mismatch has little influence on the proposed UB-PLKF and VC-PLKF. The shapes of their positions and velocity RMSE curves are basically the same as those in Figure 5. This verifies that the UB-PLKF and VC-PLKF are more robust to the noise statistics mismatch. When it comes to the velocity RMSEs, some differences may occur. We can find that, although the velocity RMSEs of the IV-PLKF gets worse after the mismatch begins, it becomes smaller after the 100th scan. The possible reason is that at the 100th scan, the target starts to manoeuvre, and at this time, large measurement errors occur. So the large variance may more adaptable to this case. However, after the 201st scan, the velocity RMSEs of the BC-PLKF and IV-PLKF get worse again and finally exceed the RMSEs of UB-PLKF and VC-PLKF.  Table 1 lists four different cases of velocity constraints to compare the influence of different constraints in VC-PLKF. The time-averaged RMSEs are given by Figure 8. The closer the constraint is to the true speed, the smaller the RMSE of the VC-PLKF is. However, this improvement is at the cost of reducing the application scope of the algorithm. Thus, the choices of the upper and lower bounds depend on our prior knowledge of the target, otherwise it is suggested to choose a large range of constraints.

Discussion
Section 3 compares the algorithms' performances in terms of RMSE and time-averaged RMSE. Figures 3 and 5 show that the proposed algorithms outperform the BC-PLKF and IV-PLKF in [25] both in non-manoeuvring and manoeuvring scenarios. This is not surprising since the BC-PLKF just subtracts the approximated bias and does not split the noise away from the H k . When the target manoeuvres or the target sensor geometry becomes poor, the approximated bias may not be accurate, which leads to huge biases in BC-PLKF. The IV-PLKF also suffers from this, since it utilizes the results of the BC-PLKF to calculate the approximated bearings. However, the proposed algorithms are unbiased and can be rapidly adapted to the changes in the scenarios. Table 2 records the runtimes of the four algorithms, which are normalised by the UB-PLKF runtime. It shows that the UB-PLKF costs the least time among the four improved algorithms because it avoids the bias compensation step in BC-PLKF. The VC-PLKF and BC-PLKF have similar complexity, while the IV-PLKF takes the longest time to run. Considering the RMSEs, the time-averaged RMSEs and the runtimes, we can draw the conclusion that the proposed UB-PLKF and VC-PLKF outperform the BC-PLKF and IV-PLKF, both in terms of tracking accuracy and real-time application. In this paper, we assume that the statistical characteristics of noise are known a priori; however, this is not true in practical application. So, future research may contain the construction of the UB-PLKF in terms of the unknown statistical characteristics of noise by variational inference [36].

Conclusions
To handle the bias problem in the traditional PLKF algorithm, we propose the UB-PLKF algorithm. Unlike the existing BC-PLKF and IV-PLKF, the UB-PLKF splits the noise away from the measurement vector directly, which can lead to an unbiased estimate. We then develop the VC-PLKF by solving a partial norm-constrained optimisation problem with an inequality relation to improve the performance of UB-PLKF. Simulation results show that the UB-PLKF and VC-PLKF outperform the BC-PLKF and IV-PLKF according to the RMSE, the time-averaged RMSE, and the computational complexity. The results also verify that the proposed algorithms can adapt to the abrupt changes of the target velocity, and thus are more suitable for the manoeuvring target tracking scenario. x k+1 lg p x k+1 |x k x k+1 lg p z k+1 |x k+1 . (A9) Here p x k+1 |x k and p z k+1 |x k+1 are the transition and measurement probability density functions, and ∆ Ψ Φ is the second-order partial derivative operators. Based on (1) and (43), we have D 11 k = F T Q −1 F, D 12 k = −F T Q −1 , D 21 k = −Q −1 F, and D 22 k = Q −1 +H T k+1Hk+1 /σ 2 k+1 . The initial state of J 0 is chosen to be P 0|−1 . When it comes to the constrained Cramer-Rao lower bound, its FIM J C,k can be calculated as [35] where U k is the gradient matrix which is defined by Finally, according to the relationship between the FIM and Cramer-Rao lower bound J −1 k CRLB, we can calculate the CRLB and CCRLB recursively.