A pure angle target trajectory estimation method

Abstract

The application discloses a kind of pure angle target trajectory estimation methods, this method will typical target motion model be converted into the autoregressive moving average model of position component to realize state decoupling, by adjusting the relationship between pseudo-linear least square cost function and process noise energy to obtain regularized pseudo-linear least square model.To reduce the estimation deviation generated by the correlation between pseudo-linear measurement matrix and noise, the regularized instrumental variable least square position estimation is obtained by using instrumental variable estimation method.The method of the application obtains the analytical expression of batch estimation of azimuth position component in the framework of regularized least square estimation.The method has linear time calculation complexity, and its estimation performance is irrelevant to initial state.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of target tracking, and relates to a pure angle target trajectory estimation method. BACKGROUND

[0002] Pure bearing target state smoothing is a batch estimation method for target state by using a series of target bearing angle measurements collected by observation stations, and is widely applied to the fields of air traffic control, robot navigation, formation and path planning. Unlike a filter which uses only current and past measurements to estimate target position, a smoother considers current, past and future measurements when estimating target position, so has higher estimation accuracy than the filter. Common nonlinear smoothers include an extended Kalman smoother (EKS) and an unscented Kalman smoother (UKS). The EKS linearizes a nonlinear function by using a first-order Taylor expansion, and combines a Kalman smoother to realize its recursive form, so inevitably introduces linearization error. In addition, it is not easy to calculate a Jacobian matrix in general cases, which increases the algorithm calculation complexity. From the optimization point of view, the EKS is very sensitive to initialization, and improper initial value makes it difficult for the algorithm to converge to a local / global optimal solution. The UKS abandons the traditional method of linearizing a nonlinear function, and uses unscented transformation to transfer the first and second moments of a probability distribution, and can achieve higher-order approximation in nonlinear fitting. However, the parameter selection problem of the UKS has not been completely solved, and the accuracy of the initial value also has a non-negligible influence on the performance of the UKS.

[0003] The accuracy of the existing nonlinear recursive Bayesian smoothing algorithm is affected by the initial value, and an inaccurate initial value greatly affects the algorithm performance. For high-dimensional state estimation problems, the nonlinear recursive Bayesian filtering algorithm does not consider state decoupling, and it is difficult to achieve an effective trade-off between estimation accuracy and calculation complexity. SUMMARY

[0004] In view of the deficiencies of the prior art, the application provides a pure angle target trajectory estimation method. First, the high-dimensional state decoupling problem is considered, the position component is separated from the target state, the calculation complexity is reduced, the initial value is not affected, and the accuracy of target tracking trajectory is improved.

[0005] A pure angle target trajectory estimation method comprises the following steps:

[0006] Step 1, a motion model for estimating a target is established, and state decoupling is performed to obtain an autoregressive moving average model about a position component.

[0007] As a preference, the motion model of the target is a discrete white noise acceleration model, a discrete Wiener process acceleration model or a discrete Singer acceleration model.

[0008] Step 2, using a sensor to measure the azimuth angle of the target at the current time

[0009]

[0010] is a real azimuth angle, r k = [r x,k , r y,k ] Τ represents a sensor position vector at the kth time, ε k is an angle error obeying a mean of 0 and a covariance of The azimuth angle nonlinear least square cost function is converted into a pseudo-linear least square cost function where K represents the total observation time, b k = A k r k .

[0011] Step 3, using the pseudo-linear least square cost function and the relationship between the process noise energy obtained by the autoregressive moving average model, a regularized pseudo-linear least square cost function is established, and after optimization solution, the regularized pseudo-linear least square estimation of the target position component is obtained

[0012] Step 4, for the regularized pseudo-linear least square estimation obtained in step 3 a tool variable matrix B = blkdiag (B1, B2, …, B K ) is designed, where represents the pseudo-linear least square estimation of the target position at the kth time. Then the tool variable batch optimization equation is derived, and the regularized tool variable least square estimation of the target position component is obtained as the target trajectory estimation result, where:

[0013]

[0014] I2 represents a 2*2 unit matrix.

[0015] The present application has the following beneficial effects:

[0016] This invention considers the high-dimensional state decoupling problem, deriving an autoregressive moving average model for the position component to separate it from the target state. This allows for the direct acquisition of an analytical solution for the target position with linear computational complexity. By combining the azimuth pseudo-linear least squares cost function, a regularized pseudo-linear least squares cost function for the position component is obtained. Optimizing this function yields a pseudo-linear batch optimization equation, leading to the regularized pseudo-linear least squares optimization solution for the position component. The estimation performance depends only on the adjustment parameter, unaffected by the initial value. Furthermore, the optimal value of this adjustment parameter can be obtained using the L-curve method, making it more objective and reliable. Finally, an instrumental variable matrix independent of the azimuth pseudo-linear error is designed, and the instrumental variable batch optimization equation is derived to obtain a regularized instrumental variable least squares estimate of the target position. Simulation results demonstrate that this method outperforms the extended Kalman smoothing algorithm and the unscented Kalman smoothing algorithm in estimation accuracy, while maintaining comparable computational complexity. Attached Figure Description

[0017] Figure 1 Flowchart of a pure angle target trajectory estimation method;

[0018] Figure 2 This is a schematic diagram of the target tracking trajectory in the embodiment;

[0019] Figure 3 A comparison of the root mean square error of various tracking methods in the embodiments;

[0020] Figure 4 This example compares the deviations of various tracking methods. Detailed Implementation

[0021] The present invention will be further explained below with reference to the accompanying drawings;

[0022] This embodiment assumes a moving target and a motion sensor in a motion scenario, with the sensor performing maneuvers. The trajectory of the moving target is predicted using the pure angle target trajectory estimation method described in this invention, as follows: Figure 1 As shown, the specific steps include:

[0023] Step 1: Establish a motion model for the moving target. In this embodiment, a discrete white noise acceleration model is used as an example to establish the following target motion model:

[0024] zx k =F c x k +G c w k

[0025] Where z represents the forward difference operator, used to describe the relationship between the target state at time k and time k+i, z ix k = x k+i . denotes the target state at the kth time instant, denotes the position component at the kth time instant, denotes the velocity component at the kth time instant, w k = [w x,k , w y,k ] T denotes a Gaussian white noise with zero mean and Q w = diag(q x , q y ) as variance, F c and G c denote the state transition matrix and the process noise gain matrix, respectively:

[0026]

[0027] T s denotes the sampling time of the sensor. The state decoupling of the above discrete white noise acceleration model gives the autoregressive moving average model of the position component at the kth time instant:

[0028]

[0029] where, Ψ c (z) = z 2 - 2z + 1.

[0030] The state decoupling separates the position component from the four-dimensional target state, which reduces the dimension of the estimated state and facilitates the implementation of the smoothing algorithm.

[0031] Step 2, use the sensor to measure the azimuth angle of the target at the current time

[0032]

[0033] is the true azimuth angle, r k = [r x,k , r y,k ] Τ denotes the sensor position vector at the kth time instant, ε k is the angle error subject to zero mean and covariance.

[0034] Since the observation value and the target position have a nonlinear relationship, the angle nonlinear measurement equation is usually converted into a pseudo-linear measurement equation. Assuming that ε k is small enough, there exists ε​k ≈sinε k , then ε k is the pseudo-linear least squares cost function:

[0035]

[0036] where K denotes the total number of observation time,△x k and△y k denote the target-to-sensor distance vector x-axis component and y-axis component, respectively, d k denotes the target-to-sensor Euclidean distance, and d k is ignored.

[0037]

[0038] where, b k = A k r k .

[0039] Step 3, according to the relationship between the pseudo-linear least squares cost function and the process noise energy , a regularized pseudo-linear least squares cost function is established:

[0040]

[0041] Transformed into matrix form:

[0042]

[0043] where, A = blkdiag(A1, A2, …, A K ), b = [b1, b2, …, b K ] T , Ψ is a matrix related to the autoregressive moving average model:

[0044]

[0045] I2 denotes a 2 × 2 unit matrix.

[0046] Let the following pseudo-linear batch optimization equation is obtained:

[0047]

[0048] The optimal value of the adjustment parameter λ is obtained by the following optimization criterion:

[0049]

[0050] The regularization pseudo-linear least squares optimization result of the target position component is obtained by solving:

[0051]

[0052] As an optional method, when λ∈(0,+∞), an L-curve is obtained according to the above optimization criterion, and the λ value corresponding to the vertex of the L-curve is taken as the optimal value of the adjustment parameter.

[0053] Step 4, a tool variable matrix B of the same size as matrix A is designed, B=blkdiag(B1,B2,…,B K ), wherein represents the pseudo-linear least squares estimation of the target position at the kth moment. The matrix A in is replaced by the tool variable matrix, and the following equation is obtained:

[0054]

[0055] The equation is the regularization tool variable batch optimization equation, and the regularization tool variable least squares estimation of the target position component is obtained by solving the equation, which is taken as the target trajectory estimation result.

[0056] The prediction results of the present method, the extended Kalman smoothing algorithm and the unscented Kalman smoothing algorithm are compared, the number of Monte Carlo is set to 1000 times, and the initial values of the extended Kalman smoothing algorithm and the unscented Kalman smoothing algorithm are the same. The tracking trajectory diagram is shown in Figure 2 , the root mean square error (RMSE) result of the position component is shown in Figure 3 , and the bias (BIAS) result of the position component is shown in Figure 4 . The calculation formulas of the RMSE and BIAS of the kth parameter are as follows:

[0057]

[0058] wherein, L represents the number of Monte Carlo, represents the true target position, represents the estimated target position. It can be seen from Figure 2 that the tracking trajectory obtained by the present method is closer to the true trajectory of the target motion, while the tracking trajectories of the extended Kalman smoothing algorithm and the unscented Kalman smoothing algorithm are far from the true trajectory at the initial moment. From Figure 3 and Figure 4It can be seen that in the first 15 moments, the RMSE and BIAS of the method are lower than those of the extended Kalman smoothing algorithm and the unscented Kalman smoothing algorithm, which also shows that the method can be not affected by the initial value, and the performance is relatively stable.

[0059] When the test is performed in this embodiment, the computer processor performance is Intel(R) Core(TM) i7-1065G7 CPU @1.30GHz (8 CPUs), ~1.5GHz, and the running time comparison results of the three methods are as follows:

[0060] Algorithm Regularized instrumental variable estimation Extended Kalman smoothing Unscented Kalman smoothing Run time (s) 0.005 0.0022 0.0073 .

Claims

1. A method of pure angle target trajectory estimation, the method comprising: Specifically comprising the following steps: Step 1, establishing a motion model of an estimated target and performing state decoupling to obtain an autoregressive moving average model about a position component; the motion model of the target is a discrete white noise acceleration model: zx k = F c x k + G c w k Where z represents the forward difference operator, used to describe the relationship between the target state at time k and time k+i, z i x k =x k+i ; This represents the target state at time k. This represents the position component at time k. Let w represent the velocity component at time k. k =[w x,k ,w y,k ] T This indicates that the mean is zero and the variance is Q. w =diag(q) x ,q y Gaussian white noise, F c and G c These represent the state transition matrix and the process noise gain matrix, respectively: T s denotes the sampling time of the sensor; state decoupling of the above discrete white noise acceleration model, the autoregressive moving average model of the position component at the kth moment is obtained: ​ wherein Ψ c (z) = z 2 -2z + 1; Step 2, measure the azimuth angle of the target at the current time using the sensor is the true azimuth angle, r k = [r x,k , r y,k ] Τ represents the sensor position vector at the kth time instant, ε k is the angle error following a zero-mean, covariance ; the azimuth nonlinear least-squares cost function is converted to a pseudo-linear least-squares cost function where K represents the total number of observation time instants, b k = A k r k ; Step 3, using pseudo-linear least squares cost function The relationship between the process noise energy obtained from the autoregressive moving average model and the pseudo-linear least squares cost function is established, and the pseudo-linear least squares estimation of the target position component is obtained after optimization solution Step 4, the regularized pseudo-linear least squares estimation obtained in step 3 A tool variable matrix B = blkdiag(B1, B2, …, B K ), where denotes the regularized pseudo-linear least squares estimation of the target position component at the kth moment; then the tool variable batch optimization equation is derived Solving obtains the regularized tool variable least squares estimation of the target position component as the target trajectory estimation result, where: b = [b1, b2,..., b K ] T I2 represents a unit matrix with a size of 2*2.

2. The method of claim 1, wherein: the azimuth angle of the target at the current time instant measured by the sensor Assume that there exists ε k ≈sinε k then: where Δx k and Δy k denote the target-to-sensor distance vector x and y components, respectively, d k denotes the target-to-sensor Euclidean distance, ignoring d k The pseudo-linear least-squares cost function for the azimuth angle is then given by:

3. The method of claim 1, wherein: In step 3, a regularized pseudo-linear least square cost function is established as follows: It is converted into a matrix form as follows: where A = blkdiag(A1, A2,..., An) ; and K ) ; Let The pseudo-linear batch optimization equation is obtained as follows: An optimal value of the adjustment parameter λ is obtained through the following optimization criterion: A regularized pseudo-linear least square estimation result of the target position component is obtained by solving:

4. The method of claim 3, wherein: When λ∈(0, +∞), an L-curve is obtained according to the optimization criterion, and a value of λ corresponding to a vertex of the L-curve is taken as an optimal value of the adjustment parameter.

Images