An unmanned aerial vehicle target positioning method based on two-stage unbiased pseudo-linear Kalman filtering

Abstract

The present application integrates the idea of separating noise and true value into the PLKF framework, and proposes an unmanned aerial vehicle target positioning method based on two-stage unbiased pseudo-linear Kalman filtering (2S-UPLKF). The angle estimation based on EKF in the first stage can effectively decouple noise interference when the unmanned aerial vehicle is tracking at a long distance, providing a robust initial state for the system. The second stage constructs a noise-true value separation mechanism to eliminate the correlation between the observation matrix and the noise in the pseudo-linear equation from the principle level. This method not only inherits the advantage of efficient calculation of pseudo-linear filtering, but also realizes the active suppression of bias in the dynamic tracking process through two-stage collaborative optimization. Theoretical analysis and simulation experiments show that, compared with other nonlinear filtering algorithms, it can still effectively suppress error fluctuations under extreme conditions such as strong nonlinear observation, large angle noise and long distance observation, and has better tracking performance.

Description

Technical Field

[0001] This invention belongs to the field of positioning methods, specifically relating to a UAV target positioning method based on a two-stage unbiased pseudo-linear Kalman filter. Background Technology

[0002] While UAVs' optoelectronic systems can acquire real-time image sequences of targets and their surrounding areas, they struggle to directly and dynamically measure the target's three-dimensional coordinate trajectory in a geographic coordinate system. Therefore, achieving continuous and precise target tracking and positioning using airborne optoelectronic systems—i.e., UAV target geographic tracking and positioning—has become a key research problem for UAVs. Specifically, target geographic tracking and positioning refers to the process of continuously acquiring UAV navigation data and angle, distance, and speed information from optoelectronic measurement equipment during UAV flight, combining this with target motion characteristics and spatiotemporal correlations to calculate and predict the target's dynamic trajectory and precise position information in a geographic coordinate system in real time. This process requires not only instantaneous target positioning but also the establishment of a target motion model to achieve continuous target tracking and trajectory prediction, ensuring stable and reliable full-process monitoring and positioning of moving targets.

[0003] Due to the numerous error factors and long propagation chains in UAV target tracking and positioning, and the often adverse environmental conditions such as strong winds and vibrations during actual missions, the airborne optoelectronic platform is severely affected by nonlinear factors during mission observation. Furthermore, when UAVs perform reconnaissance and surveillance missions in dangerous target areas or non-airspace regions, the optoelectronic platform primarily images large areas at long distances using a high-altitude, oblique-view approach. Its observation conditions often exhibit extreme characteristics such as large tilt angles, long distances, long focal lengths, and strong nonlinearity, leading to severe ill-conditioned nature in the platform's observation equations. This makes the target tracking and positioning accuracy highly sensitive to observation errors caused by disturbances. Therefore, research on target tracking and positioning methods with strong computational robustness and high tracking accuracy is of great significance.

[0004] Domestic and international scholars have conducted a series of studies on the problem of significantly reduced target tracking and positioning accuracy caused by strong nonlinear factors, mainly from two paths: active tracking and positioning and passive tracking and positioning. Active tracking and positioning utilizes active detection methods such as laser target designators to acquire the distance and angle information between the UAV and the target in real time, and combines it with the target motion model to achieve continuous tracking and high-precision positioning of the target. Liu et al. used distance information to solve the nonlinear estimation problem of passive positioning and tracking by a single observer. LEE et al. developed a target tracking system based on a laser rangefinder and conducted experimental tests using a low-speed fishing boat and a UAV to verify the performance and effectiveness of the proposed scheme. Liu et al. significantly improved the positioning accuracy of the electro-optical pod in continuous target tracking by correcting the relative angular displacement between the UAV and its onboard strapdown electro-optical platform, combined with the distance between the UAV and the target; Wang et al. used the recursive least squares (RLS) filtering method to locate ground targets. At the same time, by using a real-time zoom lens distortion correction method, they reduced the circular error probability (CEP) of multi-target tracking and positioning by 7%.

[0005] However, active tracking and localization methods require actively emitting radiation signals during operation, which may expose the UAV's position and increase the risk of mission execution. Furthermore, since laser beams typically coincide with the platform's optical axis, active localization can only acquire distance information to the target at the center of the field of view, making it difficult to track multiple dynamic targets simultaneously, thus limiting its application in multi-target tracking scenarios. Therefore, although active tracking and localization exhibits high accuracy and reliability in continuous single-target tracking, its applicable scenarios remain limited. With the development of sensing technology, the cost, size, and power consumption of visual sensors have gradually decreased. In recent years, using visual sensors (such as visible light cameras, thermal imaging sensors, depth image sensors, and optical flow sensors) for UAV navigation and localization has become a research hotspot. This localization method passively receives signals radiated from the target during mission execution, without actively engaging with the outside world optically or electrically, maximizing the safety of the UAV itself; this is called a passive localization method. Kang et al. measured the angle of arrival (AOA) of two UAVs to achieve passive localization of an unknown target. They also designed a weighted least squares (WLS) estimator, effectively reducing the estimation error of the target position. Zhao et al. used a delta-generalized labeled multi-Bernoulli filter to overcome the influence of the nonlinear motion of the radiation source, accurately tracking the target and capturing its trajectory. XU et al. established a nonlinear system model of the target's spatial position based on the geometric relationship between the line-of-sight (LOS) pointing vector and the target point. Then, they used the capacitive Kalman filter (CKF) algorithm to estimate the target position. Luo et al., based on geometric constraints, proposed an improved three-stage extended Kalman filter (3S-EKF) to solve the coupling problem between the azimuth and elevation measurement equations. Lin et al. used an interactive multi-model unscented Kalman filter (IMM-UKF) to perform end-to-end estimation of the target's position, effectively solving the problem of accurate positioning of non-cooperative targets. However, the above nonlinear algorithms are all based on Gaussian noise, and their ability to solve tracking and positioning problems with strong nonlinear characteristics is relatively limited, and their computational complexity is high.

[0006] Pseudolinear filtering methods, by replacing traditional nonlinear azimuth measurement equations with pseudolinear estimation equations, not only reduce the computational complexity of the algorithm but also exhibit higher adaptability during system initialization, reducing the stringent requirements for initial parameter settings. Cheng et al. merged measurement matrix estimation with pseudo-measurement errors and used a fully linear sequential filtering framework to estimate the combined error. Yang et al. designed a pseudolinear Kalman filter algorithm for the presence of random observer position errors, effectively solving the problem of reduced state estimation accuracy caused by random noise interference to the observer position. However, in the target tracking process of UAV electro-optical systems, the highly nonlinear characteristics of the observation model can lead to serious bias problems in pseudolinear Kalman filter algorithms, thus affecting tracking performance. To address this issue, existing technologies have conducted in-depth analysis of the bias characteristics in pure azimuth target motion and introduced a bias compensation mechanism into the pseudolinear estimation system, thus proposing a bias-compensated pseudolinear Kalman filter algorithm (BC-PLKF). This algorithm significantly alleviates the bias problem in traditional pseudolinear filtering algorithms by effectively reducing the correlation between the measurement matrix and pseudolinear noise, achieving significant results in improving target tracking accuracy. However, the effectiveness of the BC-PLKF algorithm largely depends on the assumption of low azimuth noise. In actual UAV operating environments, due to numerous complex interference factors, the algorithm's bias compensation performance significantly decreases when azimuth noise is high, making it difficult to maintain high-precision target tracking capabilities. Instrumental variable Kalman filter (IVKF) utilizes the instrumental variable matrix to reduce the correlation between the pseudo-measurement matrix and pseudo-linear noise, further improving state estimation performance while ensuring the stability and low complexity of PLKF. However, the instrumental variables are constructed using the BC-PLKF method. As observation noise increases, the correlation between the instrumental variables and the observation matrix weakens, significantly reducing the performance of IV-PLKF. Yang et al. proposed a distributed instrumental variable Kalman filter (DIVKF) using finite-time average consistency, which iteratively approximates the true values ​​of relative distance and angle to achieve higher filtering accuracy.

[0007] Other existing technologies employ selective angle measurement strategies to impose constraints on the construction of instrumental variables, thereby achieving asymptotic unbiasedness in state estimation. Although these methods are innovative in instrumental variable reconstruction, they do not fundamentally solve the correlation problem between pseudo-measurement matrices and pseudo-linear noise. Their performance is limited by noise sensitivity and the limitations of instrumental variable construction. In particular, under conditions of large tilt angles and long-distance observation by UAVs, the ill-conditioned nature of the observation equations leads to a sharp decline in tracking accuracy. Summary of the Invention

[0008] To address the aforementioned issues, this invention incorporates the concept of noise and truth separation into the PLKF framework, proposing a UAV target localization method based on a two-stage unbiased pseudolinear Kalman filter (2S-UPLKF). The first stage, angle estimation based on EKF, effectively decouples noise interference during long-range UAV tracking, providing a robust initial state for the system. The second stage constructs a noise-truth separation mechanism, eliminating the correlation between the observation matrix and noise in the pseudolinear equations from a fundamental perspective. This algorithm not only inherits the computational efficiency of pseudolinear filtering but also achieves proactive error suppression during dynamic tracking through two-stage collaborative optimization. Theoretical analysis and simulation experiments demonstrate that, compared to other nonlinear filtering algorithms, 2S-UPLKF effectively suppresses error fluctuations under extreme conditions such as strongly nonlinear observations, large-angle noise, and long-range observations, exhibiting superior tracking performance.

[0009] The present invention adopts the following technical solution:

[0010] A UAV target localization method based on two-stage unbiased pseudo-linear Kalman filtering includes the following steps:

[0011] Step 1: Collect the target state at time k-1 Covariance Matrix State noise matrix and measurement noise matrix The data input is based on the EKF-based angle estimation algorithm, which sequentially performs a prediction phase, an update phase, and an angle estimation phase. The final output is the target state prediction value at time k based on the EKF-based angle estimation algorithm. :

[0012] Prediction phase: Calculate the prior estimates of the state variables at time k. Prior estimation of error covariance ;

[0013] , Let k be the state transition matrix at time k;

[0014]

[0015] Update phase: Calculate the posterior state estimate at time k based on the EKF angle estimation algorithm. Posterior estimate of error covariance :

[0016]

[0017]

[0018]

[0019] In the formula, Let k be the linear observation matrix at time k. The actual measured value of the target position at time k;

[0020] Angle estimation stage: Calculate the predicted target state value at time k based on the EKF angle estimation algorithm:

[0021] , Nonlinear observation function

[0022] Output: ;

[0023] Step 2: Collect the target state at time k-1. Covariance Matrix State noise matrix and measurement noise matrix The target state prediction value based on the EKF angle estimation algorithm at time k The input is a UPLKF-based state estimation algorithm. This algorithm sequentially performs a prediction phase, an update phase, and an angle estimation phase, ultimately outputting the predicted target state at time k based on UPLKF. and target state error covariance ;

[0024] Prediction phase: Calculate prior estimates of state variables and prior estimates of error covariance.

[0025] , Let k be the state transition matrix at time k;

[0026]

[0027] Update phase: Calculate the UPLKF-based posterior state estimate at time k. Posterior estimate of error covariance :

[0028] , The actual pseudo-measured value of the target position at time k. The equivalent pseudo-measurement value of the target position at time k;

[0029] ;

[0030] ;

[0031]

[0032]

[0033] and Let k be the target azimuth and elevation angles. and The target's azimuth and elevation angles at time k are observed due to noise. , Let be the state vector of the target at time k;

[0034] , Let be the equivalent observation matrix of the target position at time k. for The estimated value;

[0035]

[0036]

[0037]

[0038] Output: ;

[0039] Step 3, The target state at time k , The covariance matrix at time k Substitute the values ​​from steps 1 and 2 into the iterative calculation to obtain the target state at time k+1.

[0040] Furthermore, the initial position of the target is a preset fixed value.

[0041] Compared with the prior art, the present invention, by adopting the above technical solution, has the following advantages:

[0042] By utilizing the state equation of the UAV's optoelectronic platform imaging system and combining swarm intelligence optimization and particle filtering techniques, the geographical location of the target is optimally estimated, thereby achieving accurate positioning of ground targets by the UAV under severe nonlinear influences. The main innovations of this invention can be summarized as follows:

[0043] (1) The optimization mechanism of the firefly algorithm is incorporated into the framework of the particle filter algorithm to guide the particles to move towards the high likelihood region;

[0044] (2) Introduce multiple mutation strategies and elastic mechanisms to change the mode of particle interaction, and solve the problem of particle degradation caused by severe nonlinear factors and over-optimization;

[0045] (3) Use the mutant firefly optimized particle filter to make the optimal estimate of the target position, reduce the number of particles required for the standard particle filter algorithm to run, and improve the robustness and positioning accuracy of the algorithm.

[0046] The present invention will now be described in detail with reference to the accompanying drawings and embodiments. Attached Figure Description

[0047] Figure 1 This section defines coordinate systems and their relationships, where (a) is a diagram showing the relationship between land rectangular coordinates and geographic coordinates; (b) is a diagram showing the relationship between geographic coordinates and UAV coordinates; and (c) is a diagram showing the relationship between UAV coordinates and ca.

[0048] Figure 2 This is a flowchart illustrating the present invention;

[0049] Figure 3 This is a schematic diagram of an AOA target tracking model for unmanned aerial vehicles (UAVs).

[0050] Figure 4 This is a schematic diagram of the movement of the drone and the target.

[0051] Figure 5 This is a diagram showing the azimuth and elevation angles;

[0052] Figure 6 This is a schematic diagram showing the error variation of different algorithms in position and velocity estimation under different measurement noise conditions;

[0053] Figure 7 This is a schematic diagram showing the changes in position and velocity RMSE with initial distance for different algorithms. Detailed Implementation

[0054] The principles and features of the present invention are described below with reference to the accompanying drawings. The examples given are only for explaining the present invention and are not intended to limit the scope of the present invention.

[0055] In the description of this invention, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0056] 1. Research and Development Background

[0057] This section introduces the relevant background of passive localization and tracking, including the dynamic physical model of the target radiation source, the measurement model, the multi-target stochastic finite set system model for time-varying multi-radiation source tracking, the basic theory of Bayesian multi-target recursion, and the δ-GLMB filter. Furthermore, some basic principles of stochastic finite sets discussed in this section are supplemented in Appendix B.

[0058] 1.1 Definition of Coordinate System

[0059] The model described in this invention has seven sets of spatial coordinates, as follows:

[0060] (1) Geodetic Coordinate System GCF Based on the International Land Reference System (WGS-84). The GCF uses the center of the Earth's ellipsoid as its origin. Longitude is used. ,latitude and height A three-dimensional curvilinear coordinate system used to describe the position of points on the Earth's surface.

[0061] (2) Geocentric rectangular coordinate system ECEF Inertial coordinate system, such as Figure 1 As shown in (a). ECEF also uses the center of the Earth's ellipsoid as its origin. The positive direction of the axis points to the Earth's North Pole. The positive direction of the axis points to the intersection of the Prime Meridian and the equator. The positive direction of the axis is perpendicular to the other two axes and points towards the equator.

[0062] (3) Navigation coordinate system NED : Figure 1 In figure (a), the origin is located at point K in the red coordinate system and... Figure 1 (b) The red coordinate system, whose origin is located at the UAV's center of mass, is the same coordinate system as the NED coordinate system. The NED uses the UAV's center of mass as its reference origin. The positive direction of the axis points to the Earth's north. The positive direction points eastward on Earth. The positive axis is perpendicular to the Earth's surface and points downwards.

[0063] (4) Body coordinate system HRD :like Figure 1 As shown in Figure (b), the origin is located in the green coordinate system of the UAV, which is the HRD coordinate system, and its origin coincides with the NED. The positive direction of the axis points directly in front of the aircraft. The positive direction of the axis points towards the right wing of the aircraft. The positive axis direction is downward along the vertical axis of the carrier. The relationship between HRD and NED is as follows: Figure 1 As shown in Figure (b), the three-axis attitude angles λ, θ, and κ are measured by the inertial navigation system. The coordinate system is shown in dark green. This involves translating the origin of the HRD to the coordinate system after the optical center of the camera in the photoelectric platform, with the translation vector being... .

[0064] (5) Camera coordinate system C : Figure 1 In Figure (c), the orange coordinate system with its origin located on the pod is the C coordinate system, and its origin is the optical center of the camera. positive direction of axis and The positive axes are parallel to the image plane and point to the lower right side of the camera. The positive axis points along the optical axis towards the camera direction. The attitude angle of the pod is measured by a photoelectric encoder.

[0065] (6) Image physical coordinate system W : Figure 1 In the middle (c) figure, the dark blue coordinate system with the origin located at the center of the image plane is the W coordinate system. shaft and The shafts are respectively with shaft and The axes are parallel.

[0066] (7) Image coordinate system I : Figure 1 In figure (c), the origin is located in the upper left corner of the image plane, and the light blue coordinate system is the I coordinate system. shaft and The shaft is also the same as shaft and The axes are parallel.

[0067] 1.2 Target Position Estimation Model Based on Improved Cuckoo Algorithm and Optimized Particle Filter

[0068] The azimuth and elevation data of the target relative to the pod, acquired by the airborne optoelectronic system, are transmitted to the aircraft. Combined with the geographic coordinates and flight attitude information provided by the aircraft via GPS / INS, a series of coordinate transformations are performed to locate the target. Since the pod's measurement data is obtained based on its own coordinate system, while the target's final position needs to be represented in a geodetic coordinate system, corresponding coordinate transformations are necessary, such as... Figure 2As shown in the dashed box in the image.

[0069] The process by which a drone determines the location of a target in a geodetic coordinate system using an optoelectronic platform is as follows: First, the camera scans the target area. Once the target is detected, the image tracker obtains its pixel coordinates in the image coordinate system. Combined with known camera focal lengths The LOS vector in the camera coordinate system C can be calculated as follows: During target search, the camera's rotation in the reference coordinate system is determined by its azimuth and pitch angles. This indicates that the drone's flight attitude also affects the LOS vector, with its attitude angle being... Translation between different coordinate systems does not change the line-of-sight (LOS) direction. In the camera coordinate system, the LOS vector can be rotated multiple times to transform it into the UAV's target-pointing vector in the navigation coordinate system (NED). Therefore, through coordinate transformation A, the electro-optical platform can use the above parameters to measure the relative angle of the optical axis pointing in the NED under inertial conditions. The accuracy of these parameter measurements determines the accuracy of the target's angle of arrival (AOA) estimation relative to the UAV. Finally, GPS provides the UAV's position in the geodetic coordinate system. The geodetic coordinates of the final target can be obtained by coordinate transformation B.

[0070] The C coordinate system is the initial coordinate system for locating the target in coordinate transformation A. Then, the LOS vector, expressed in homogeneous coordinates, is...

[0071]

[0072] In the formula, , The center pixel of the image, i.e., the pixel coordinates of the intersection of the optical axis and the image plane; , These are the horizontal and vertical dimensions of a pixel.

[0073] pass Figure 1 The four rotations in Figures (b) and (c) yield the following vector in the ECEF coordinate system pointing from the camera's optical center to the target (hereinafter referred to as the pointing vector):

[0074]

[0075] In the formula, , These are rotation matrices, the expressions for which are given in Appendix A. Then, the azimuth and pitch angles of the target observed by the UAV pod in the navigation coordinate system are respectively...

[0076]

[0077] By using equations (2) and (3) to estimate the AOA through the pointing vector, the coordinates of the target relative to the UAV in the navigation coordinate system NED can be obtained. .like Figure 1 As shown in (a), combined with the UAV's GPS location And the installation deviation of the pod relative to the aircraft's center of gravity. The navigation coordinate system NED can be used along Axis offset, Around rotation, Around rotation and along The axis offset is converted to the geocentric rectangular coordinate system ECEF:

[0078]

[0079] In the formula, This is the rotation matrix, the expression of which is given in Appendix B. The target point... The formula for converting to geodetic coordinates is also available in Appendix B.

[0080] Appendix A: Rotation matrices involved in coordinate transformation A

[0081] Rotation matrix from camera coordinate system to body coordinate system:

[0082]

[0083] In the formula, , , .

[0084] Rotation matrix from body coordinate system to navigation coordinate system:

[0085]

[0086] In the formula, , , .

[0087] This completes the AOA estimation of the target relative to the UAV and the UAV's position estimation in the geodetic coordinate system. Clearly, the accuracy of the AOA estimation directly affects the UAV's positioning accuracy towards the target. Therefore, the main contribution of the filter proposed in this invention lies in quickly and accurately estimating the relative position using the azimuth and pitch angles of the pointing vector.

[0088] 1.3 UAV Measurement Equations Based on AOA

[0089] Geometric description of the UAV AOA target tracking problem is as follows: Figure 3 As shown. and For drones Position and velocity at any given moment ; Time of the first Location of ground target and speed This is the unknown parameter vector that needs to be estimated using AOA. Since the position and velocity of the UAV are known, the relative motion between the UAV and the target is taken as the research object. Relative position and velocity are selected as... Time of the first The motion state vector of each target, i.e.

[0090]

[0091] but One goal is The state transition equation at time t is

[0092]

[0093] In the formula, It is a state vector; This is the state transition matrix, which is related to the target's kinematic model. Represents a block diagonal matrix; system process noise It follows an independent, zero-mean, additive Gaussian distribution, and its covariance matrix is... The order of elements and The same applies, so I won't elaborate further.

[0094] Assuming the target moves at a constant speed and the effect of altitude is ignored, then the transition matrix... process noise covariance matrix The expression is

[0095]

[0096] In the formula, The sampling interval; , , , The power spectral density of the process noise is respectively in , and The components of the coordinate axes.

[0097] Time of the first The AOA of a ground target relative to the drone includes the azimuth angle. and pitch angle ,and as well as Its observation equation is

[0098]

[0099] In the formula: and These are observations affected by noise; the observation noise vector. It follows a mutually independent zero-mean Gaussian random distribution and is independent of the system process noise. Its covariance matrix is... Nonlinear observation function The calculation formula is

[0100]

[0101] Therefore, drones Always The observation equation for each target is:

[0102]

[0103] In the formula, For observation vectors; and These are the corresponding set of nonlinear functions and the observation noise vector, respectively. The covariance matrix is .

[0104] According to equations (5), (9), and (10), the pseudo-linear equations for the target's azimuth and elevation angles can be obtained:

[0105]

[0106]

[0107] Based on the characteristics of the aerial environment, ,but , , Similarly, then...

[0108]

[0109]

[0110] After linearization, equations (13) and (14) are further simplified to obtain

[0111]

[0112] In the formula, , , , for The equivalent measurement error at time t, and this error follows a mean of 0 and a variance of 0. The normal distribution is given by the formula:

[0113]

[0114] The final AOA-based UAV state-space model consists of equations (6) and (15). After obtaining the initial state estimate... Under these conditions, the target tracking problem of UAVs can be equivalent to using the state transition equation and pseudo-observations at historical moments. Estimate target state .

[0115] 1.4 Observability Analysis

[0116] AOA-based measurement models can estimate the target's state using the LOS axis azimuth information of the electro-optical pod. However, due to the use of azimuth angles... and pitch angle The inherent nonlinearity of ground target localization means that even with a correct filtering structure and accurate parameter measurements, target state estimation may still fail to converge in certain situations. This necessitates addressing the problem of complete observability of the target state from pure azimuth measurements, i.e., whether a unique tracking solution exists for the system.

[0117] According to the observability criterion: for the initial set In 3D vector There is a Grammian matrix

[0118]

[0119] In the formula If there exists a positive integer Make rank satisfies

[0120]

[0121] Then the system is It is completely observable.

[0122] Ignoring measurement noise, when ,have:

[0123]

[0124] In the formula, Let be the state variables corresponding to the azimuth measurement equation. Then, define the pseudo-observation matrix as follows:

[0125]

[0126] In the formula,

[0127] make Then there is

[0128]

[0129] Through derivation, it is obtained The determinant value is:

[0130]

[0131] According to Cauchy's inequality If and only if When it is always a constant Therefore, as long as the UAV does not always fly in the azimuth direction of the target, i.e., radial motion, the state variable... It is observable. Similarly, using pitch angle information, it can be proven that if and only if... When the state variable is not constant, It is observable. However, UAVs often observe targets at large tilt angles and long distances, and their limited power results in slow climb rates, with very small pitch and pitch angle change rates. Although the system is observable, its observability is weak, and the target localization convergence speed is slow and prone to divergence.

[0132] In summary, when a UAV locates a ground target based on AOA estimation, the observable condition is that the UAV does not always move radially toward the target.

[0133] 2. The method of the present invention

[0134] 2.1 Pseudo-linear Kalman Filter

[0135] This section will introduce the basic principles of the pseudo-Kalman filter algorithm and analyze its deviations.

[0136] The pseudolinear equation (15) transforms the nonlinear observation equation (8) into a linear form. Then, the Kalman filter is applied to the pseudolinear system composed of equations (6) and (15), resulting in the following pseudolinear Kalman filter algorithm (PLKF).

[0137] Step 1: State Update Phase

[0138]

[0139]

[0140] Step 2 Measurement Update Phase:

[0141]

[0142]

[0143]

[0144] In the formula, and These are the state prediction value vector and the prediction error covariance matrix, respectively; The Kalman gain matrix; and These are the state estimate vector and the estimation error covariance matrix, respectively. To measure the noise matrix, and , , Related.

[0145] By performing a deviation analysis on PLKF and substituting equation (25) into equation (27), we can obtain... Equivalent expression

[0146]

[0147] Applying the matrix inversion formula to equation (25), we can obtain...

[0148]

[0149] Substituting equation (29) into equation (26), we get

[0150]

[0151] Equation (30) minus the actual state The instantaneous estimation bias can be obtained as follows:

[0152]

[0153] Therefore, by taking the expectation of equation (31), we can obtain the bias of the PLKF estimate as follows:

[0154]

[0155] In the formula:

[0156]

[0157]

[0158]

[0159] Therefore, the estimation bias consists of three parts: ① Bias introduced by the time prediction ② Process noise and Bias caused by correlation ③ Due to the pseudo-linearity of the system, the measurement matrix... and pseudolinear noise vector The existence of correlation can cause bias. .in, This is due to inherent system bias. Secondly, because the target process with constant velocity has low noise, it makes... and The correlation is weak. .However, and Both are affected by observation noise, and the correlation between them cannot be ignored. Therefore, through the above deviation analysis of the pseudolinear system, it can be concluded that... and The correlation between the measurement noise and the observed quantity is the fundamental reason for the bias in PLKF. The magnitude of the bias depends only on the measurement noise level and is independent of the observed quantity; increasing the number of observations does not reduce the estimation bias. Furthermore, the bias is not significant when the measurement noise is low, but as the measurement noise increases, the bias caused by pseudo-linearization increases rapidly, resulting in a significant degradation in the algorithm's estimation performance. To ensure target tracking accuracy, the bias must be addressed.

[0160] 2.2 Two-stage unbiased pseudolinear Kalman filter algorithm

[0161] Current solutions to the bias problem in PLKF (Plan-Based Tracking) mainly fall into two categories: bias-compensated PLKF (BC-PLKF) and instrumental variable-based PLKF (IV-PLKF). BC-PLKF improves target tracking accuracy by calculating and recompensating the estimated bias of the PLKF. However, this method is only suitable for situations with low measurement noise; its debiasing effect deteriorates significantly when measurement noise is high. IV-PLKF, on the other hand, utilizes instrumental variables to reduce... and The correlation between the instrumental variables and the BC-PLKF method effectively reduces the bias caused by pseudo-linearization in principle. However, the instrumental variables are constructed using this method, and as observation noise increases, the correlation between the instrumental variables and the BC-PLKF method becomes more pronounced. The weakening of the correlation significantly reduces the performance of IV-PLKF. Therefore, to address the above problems, this invention incorporates the idea of ​​separating noise and truth into the PLKF framework, proposing a two-stage unbiased pseudo-linear Kalman filter algorithm (2S-UPLKF) for UAV ground target tracking. First, an EKF is designed as the first-stage angle filter to perform posterior estimation of the azimuth and pitch angles of the UAV's LOS, solving the problem of algorithm divergence caused by noise coupling in long-range scenarios. Then, by separating the noise term from the observation matrix, the second-stage filter—an unbiased pseudo-linear Kalman filter (UPLKF)—is constructed, achieving the unbiasedness of the algorithm in principle. The workflow diagram of 2S-UPLKF is shown below. Figure 2 As shown in the green box.

[0162] 2.2.1 First Stage: Angle Estimation Based on EKF

[0163] EKF is a nonlinear filtering algorithm without fundamental bias, with low computational cost and relatively stable angle estimation. Based on the original state model of equations (6) and (10), the nonlinear model is approximated as a linear model using the Taylor formula, resulting in a linear observation matrix.

[0164]

[0165] In the formula, This is the distance prediction value.

[0166] Then, the first stage of angle estimation based on EKF is as follows.

[0167] The first stage is an angle estimation algorithm based on EKF:

[0168] enter: , for The state vector of the first-stage algorithm is input at each step. for The covariance matrix at time t, The state noise matrix is... To measure the noise matrix.

[0169] For

[0170] Prediction phase: Calculate prior estimates of state variables and prior estimates of error covariance.

[0171] Step 1: , Let be the state transition matrix.

[0172] Step 2:

[0173] Update phase: Calculate Kalman gain, state posterior estimate, and error covariance posterior estimate.

[0174] Step 1:

[0175] Step 2: , Actual measured value

[0176] Step 3:

[0177] Angle estimation stage: Calculate the posterior estimates of azimuth and elevation angles.

[0178]

[0179] Output:

[0180] 2.2.2 Second Stage: UPLKF-based State Estimation

[0181] UPLKF separates angular measurement noise from the observation matrix, thus avoiding bias compensation. The unbiasedness achieved at the theoretical level ensures the algorithm effectively guarantees the convergence of target state estimation. For the azimuth measurement equation, we have...

[0182]

[0183]

[0184] Substituting equations (34) and (35) into equation (15) yields

[0185]

[0186] In the formula,

[0187]

[0188]

[0189]

[0190] Multiply both sides of equation (37) by a diagonal matrix. , can be obtained

[0191]

[0192] It is worth noting that the state vector in equation (40) It is independent of observation noise, and the noise in the formula is angle measurement noise. non-pseudolinear noise .

[0193] According to the result of equation (32), the fundamental deviation is as follows.

[0194]

[0195] because and Compared with angle measurement noise respectively , Irrelevant, therefore This indicates that the algorithm is unbiased. However, in practical applications, due to the true value of the observed angle... , and distance vector Since it is unknown, only an estimated value can be used. Instead. This idea is similar to IV-PLKF, both attempting to construct a noise-free observation vector. And ensuring the estimation matrix... With truth value Strong correlation is the main convergence condition for IV-PLKF. If the angle estimate from this method is directly used as the true value input, it may lead to inaccurate initial estimations. With truth value The lack of correlation between these values ​​leads to divergence in the target motion state estimation. Therefore, the posterior estimates of the azimuth and pitch angles obtained from the first-stage EKF algorithm are used as the true angle inputs to this algorithm, thus obtaining...

[0196]

[0197]

[0198]

[0199]

[0200] Therefore, the state estimation steps based on UPLKF in the second stage are summarized as follows.

[0201] The second stage is a state estimation algorithm based on UPLKF.

[0202] enter: , for The state vector of the second-stage algorithm is re-input at each step.

[0203] For

[0204] Prediction phase: Calculate prior estimates of state variables and prior estimates of error covariance.

[0205] Step 1:

[0206] Step 2:

[0207] Update phase: Calculate Kalman gain, state posterior estimate, and error covariance posterior estimate.

[0208] Step 1: , This is the actual pseudo-measured value. Equivalent pseudo-measurement value

[0209] , This is the equivalent observation matrix.

[0210] Step 1:

[0211] Step 2:

[0212] Step 3:

[0213] Output:

[0214] 2.3 Algorithm Time Complexity Analysis

[0215] 3 Performance Indicators

[0216] 3.1 Accuracy Indicators

[0217] Assumption and They represent the first In this experiment Given the true state vector and estimated state vector at time t, the expressions for the bias and RMSE are as follows:

[0218]

[0219]

[0220] In the formula, This represents the number of experiments. Further, the average time deviation and RMSE for target tracking can be obtained:

[0221]

[0222]

[0223] In the formula, , The total number of time points in the tracking process. This is the number of moments when error recording begins.

[0224] 3.2 CRLB

[0225] The theoretical minimum variance of state estimation can be represented by the Cramer-Rao Lower Bound (CRLB). Since process noise affects the state transition process of a moving target, the posterior CRLB (PCRLB) is used as a reference value for the state estimation performance, resulting in the following inequality:

[0226]

[0227] In the formula, The Fisher information matrix is ​​calculated using the following formula:

[0228]

[0229] In the formula, and These are the covariance matrices of the process noise and the observation noise, respectively. For the system state equation (6) in The Jacobi matrix at that point is equal to the state transition matrix; For the nonlinear observation equation (10) in The Jacobi matrix at that location is calculated using the following formula:

[0230]

[0231] In the formula,

[0232]

[0233]

[0234]

[0235] Therefore, the time-averaged PCRLB for target tracking is

[0236]

[0237] Appendix A: Rotation matrices involved in coordinate transformation A

[0238] Rotation matrix from camera coordinate system to body coordinate system:

[0239]

[0240] In the formula, , , .

[0241] Rotation matrix from body coordinate system to navigation coordinate system:

[0242]

[0243] In the formula, , , .

[0244] Appendix B: Rotation matrices involved in coordinate transformation B

[0245] Rotation matrix from navigation coordinate system to geocentric rectangular coordinate system

[0246]

[0247] In the formula, ,

[0248] ,

[0249] ,

[0250] , , , , and These are the longitude, latitude, altitude, radius of curvature of the Earth's circumference and the first eccentricity of the meridian ellipse of the UAV.

[0251] 4.2 Simulation Analysis

[0252] 4.2.1 Simulation comparison under different angle measurement noise

[0253] Simulation scenarios such as Figure 4 As shown, the starting point of the drone is The speed is ,by The system rotates around a center point with a radius of 100m. Sampling interval. The photoelectric system at each fixed time Observations were conducted using Monte Carlo simulations 5000 times. The observation noise of the UAV's inertial navigation system and optoelectronic system at each moment follows an independent zero-mean Gaussian distribution, and the standard deviation of the observation angular noise is [missing value]. The variation of the observed angle under different standard deviations is as follows: Figure 5 As shown.

[0254] The target remains centered in the field of view, starting from the origin, with a velocity of... The power spectral density of the process noise is set to , , Due to the initial state of the target Due to the influence of multivariate Gaussian noise, the initial condition setting of the filtering algorithm needs to focus on solving the parameter initialization problem under the constraint of no prior information. Based on the error characteristics of the photoelectric platform sensor, the initial estimated value of the state vector is set to 1.5 times the true value, and the initial covariance matrix is ​​set to... The observation noise covariance matrix R and the process noise covariance matrix Q are both set to the actual system noise parameters.

[0255] To evaluate the tracking performance of the 2S-UPLKF algorithm, EKF, CKF, PLKF, and IVKF were selected as comparison algorithms. Monte Carlo simulation was used to compare and analyze the AOA target tracking errors of different algorithms. Figure 6 The error variations of different algorithms in position and velocity estimation under different measurement noise conditions were compared.

[0256] Depend on Figure 6As shown in Figure (a), the EKF algorithm exhibits the largest position estimation error, which shows a rapid increasing trend. The magnitude of the error is related to the noise intensity. The correlation is positive; the PLKF algorithm in The CKF maintains sub-meter positioning accuracy in low-noise ranges, but the deviation increases rapidly under strong noise conditions, exhibiting typical strong noise failure characteristics. The CKF reduces the error amplitude by 42.3% through a third-order spherical-radial volume rule, but its position deviation still gradually increases due to consistency failure caused by noise interference. The IVKF and 2S-UPLKF have similar position estimation accuracy under low-noise conditions, both controlled below 6m, but the 2S-UPLKF... The deviation value was reduced by 46% compared to IVKF, demonstrating a significant advantage in robustness.

[0257] Figure 6 The position RMSE evolution curves in (b) reveal deeper characteristics of each algorithm. The EKF algorithm still has the highest position RMSE, due to initial value sensitivity and divergence effects caused by first-order nonlinearity. The PLKF algorithm is close to the posterior Cramer-Rao bound in the low-noise region, but due to the coupling effect of position bias and random error, the estimation error shows a nonlinear growth trend when the noise is high. CKF significantly reduces the position RMSE compared to EKF, but is still slightly inferior to IVKF and 2S-UPLKF under strong noise conditions. IVKF solves the noise correlation problem caused by pseudolinearity through a bias compensation mechanism, and its position RMSE is reduced by an average of 31.5% compared to PLKF. It is particularly noteworthy that 2S-UPLKF... The RMSE performance under high noise conditions consistently outperforms the comparison scheme, especially in... The time efficiency is reduced by 17.4% compared to the suboptimal algorithm, indicating its superior noise resistance in the state estimation process.

[0258] Figure 6 Figures (c) and (d) further validate the state estimation characteristics of each algorithm. The velocity estimation index shows a similar trend to that of position estimation, but the performance differences between algorithms are slightly reduced. 2S-UPLKF significantly outperforms EKF and PLKF in velocity estimation, with RMSE scores reduced by 30% and 20% compared to CKF and IVKF, respectively, demonstrating the technical advantages of two-stage collaborative estimation. Figure 6 Comparative analysis shows that even when the differences in speed estimation accuracy among various algorithms decrease, 2S-UPLKF can still maintain a lower position tracking error, verifying the effectiveness of its nonlinear noise debiasing capability in strong noise scenarios. Table 1 shows the 95% confidence interval widths for different algorithm performance metrics. EKF has the largest interval width, significantly higher than other algorithms. 2S-UPLKF has the smallest position and velocity confidence intervals, statistically validating the algorithm's improved unbiasedness and noise resistance. Furthermore, the confidence interval width and... Figure 3-4 The ratio of the error magnitudes shown remains stable within a small range, indicating that... Figure 6 The validity of the error analysis results.

[0259] Table 1

[0260]

[0261] 5.2.2 Simulation Comparison under Different Initial Distance Conditions

[0262] Under certain relative velocity conditions, the initial distance between the UAV and the target will affect the angle observation. Therefore, this section mainly studies the change in the accuracy of target state estimation under different initial distance conditions. We take the initial position vectors of the UAV and the target from Section 5.2.1. Using the reference position vector and keeping the UAV altitude constant, different starting points were set within a distance range of 1 to 4 times the initial distance, with an interval step size of 0.25 times the initial distance. During the experiment, all other experimental parameters were kept constant, and a uniform standard deviation of angle observation error was used. To ensure the validity of the observation data. The changes in position and velocity RMSE with initial distance for different algorithms are as follows: Figure 6 As shown.

[0263] Depend on Figure 7It can be seen that the estimation accuracy of each algorithm gradually decreases as the initial distance increases. At 1.75 times the initial distance, the position RMSE ranking is EKF > PLKF > CKF > IVKF > 2S-UPLKF, and the velocity RMSE ranking is EKF > CKF > PLKF > IVKF > 2S-UPLKF. Except for the EKF algorithm, the position and velocity RMSE performance of other tracking algorithms shows relatively small decreases. When the initial distance is further extended to 2.5 times the baseline value, the errors of each algorithm increase significantly and show obvious differentiation. The errors of the EKF and CKF algorithms fluctuate significantly; in the 1.75-2.5 times initial distance range, their position RMSE exceeds that of other algorithms by 50m, indicating that they have lost the ability to estimate target motion parameters under this condition. In contrast, PLKF, IVKF, and 2S-UPLKF exhibit significant robustness advantages, with their error growth showing a gradual characteristic. Among them, 2S-UPLKF has the smallest error increase and can consistently approach PCRLB. Furthermore, even with significant velocity estimation errors, the PLKF algorithm still achieves more accurate position estimation. This result indicates that, with increasing initial distance, the pseudo-linearization strategy is more suitable for UAV AOA target tracking tasks.

[0264] Table 2 details the computation time comparison data for each algorithm. To facilitate quantitative comparison and analysis of cross-algorithm performance, this paper uses the runtime of the EKF algorithm as a reference benchmark and normalizes the runtimes of other algorithms. The results show that the pseudo-linear computation of PLKF slightly increases the computational cost compared to EKF; CKF significantly increases the computational cost due to the nonlinear transformation of the sampling point set with equal weights; IVKF further increases the runtime compared to PLEKF by reducing pseudo-linear noise and the correlation of the observation coefficient matrix through instrumental variables; 2S-UPLKF requires the construction of a two-stage filter, and its computational cost is slightly higher than that of IVKF, but significantly lower than that of CKF.

[0265] Table 2 Comparison of relative running times for different algorithms

[0266]

[0267] Comparative analysis of simulation experiments on the localization algorithm under different measurement error levels and initial distance conditions shows that the proposed 2S-UPLKF algorithm not only exhibits higher tracking accuracy but also demonstrates significant advantages in estimation stability, with its performance indicators showing no significant fluctuations in multiple repeated experiments. It is worth noting that when the measurement noise variance... When the initial distance is relatively short, the RMSE of the 2S-UPLKF algorithm can converge to the theoretical optimum of the Cramer-Rao lower bound. Furthermore, in high-noise environments… In long-distance scenarios, the position tracking accuracy is improved by an average of about 31.6% compared with other algorithms. The 2S-UPLKF algorithm shows strong environmental adaptability, further verifying its robustness advantage in complex environments.

[0268] 5. Conclusion

[0269] To address the challenges of strong nonlinear observation, large-angle noise interference, and ill-conditioned equations at long distances in UAV geographic target tracking and localization, a robust 2S-UPLKF tracking algorithm is proposed to effectively reduce estimation bias. A two-stage collaborative optimization mechanism effectively solves the inherent bias problem of traditional pseudolinear filtering. The first stage, based on EKF angle estimation, achieves noise decoupling and robust initialization. The second stage, a truth-noise separation mechanism, eliminates the correlation between the observation matrix and noise at the fundamental level. This innovative design significantly improves the unbiasedness of the algorithm in dynamic tracking scenarios while maintaining computational efficiency. Simulation results show that under 0.5° strong angular noise conditions, the 2S-UPLKF reduces the position RMSE by 17.4% and the velocity estimation error by 20% compared to IVKF, with the 95% confidence interval width being the best among the compared algorithms. Furthermore, in long-distance observation scenarios with a distance of 2.5 times the initial distance, its tracking accuracy still approaches the lower bound of the PCRLB theory, verifying the algorithm's robustness under extreme observation conditions and providing a theoretical basis and algorithmic foundation for accurate geographic tracking of moving targets in highly dynamic environments.

[0270] The above description provides examples of the preferred embodiments of the present invention. Parts not detailed herein are common knowledge to those skilled in the art. The scope of protection of the present invention is determined by the claims. Any equivalent modifications based on the technical teachings of the present invention are also within the scope of protection of the present invention.

Claims

1. A UAV target localization method based on two-stage unbiased pseudo-linear Kalman filtering, characterized in that, Includes the following steps: Step 1: Collect the target state x at time k-1 k-1|k-1 The covariance matrix P k-1|k-1 The state noise matrix Q and the measurement noise matrix R are input into the EKF-based angle estimation algorithm. The EKF-based angle estimation algorithm sequentially performs a prediction stage, an update stage, and an angle estimation stage, and finally outputs the target state prediction value at time k based on the EKF angle estimation algorithm. Prediction phase: Calculate the prior estimates of the state variables at time k. Prior estimate of error covariance P k|k-1 ; F k Let k be the state transition matrix at time k; Update phase: Calculate the posterior state estimate at time k based on the EKF angle estimation algorithm. Posterior estimate of sum of error covariance In the formula, J k Let z be the linear observation matrix at time k. k The actual measured value of the target position at time k; Angle estimation stage: Calculate the target state prediction value based on the EKF angle estimation algorithm at time k: h(·) is a nonlinear observation function Output: Step 2: Collect the target state x at time k-1. k-1|k-1 The covariance matrix P k-1|k-1 The state noise matrix Q and the measurement noise matrix R are compared with the target state prediction value at time k based on the EKF angle estimation algorithm. The input is a UPLKF-based state estimation algorithm. This algorithm sequentially performs a prediction phase, an update phase, and an angle estimation phase, ultimately outputting the predicted target state at time k based on UPLKF. and target state error covariance Prediction phase: Calculate prior estimates of state variables and prior estimates of error covariance. F k Let k be the state transition matrix at time k; Update phase: Calculate the UPLKF-based posterior state estimate at time k. Posterior estimate of sum of error covariance The actual pseudo-measured value of the target position at time k. The equivalent pseudo-measurement value of the target position at time k; m k1 =[-sinβ k ,-cosβ k ,0]Mx k +‖d k ||cosε k ; β k and ε k Let k be the target azimuth and elevation angles. and For the target azimuth and elevation angles observed at time k, which are affected by noise, M = [I 3×3 ,0 3×3 ], x k Let be the state vector of the target at time k; Let be the equivalent observation matrix of the target position at time k. For G k The estimated value; Output: Step 3, The target state x at time k k|k , P, the covariance matrix at time k k|k Substitute the values ​​from steps 1 and 2 into the iterative calculation to obtain the target state at time k+1.

2. The UAV target localization method based on two-stage unbiased pseudo-linear Kalman filtering according to claim 1, characterized in that, The target's initial position is a preset fixed value.

Images