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

Abstract

According to the invention, the idea of noise and truth value separation is fused into a PLKF framework, and an unmanned aerial vehicle target positioning method (2S-UPLKF) based on two-stage unbiased pseudo-linear Kalman filtering is provided. In the first stage, noise interference can be effectively decoupled during long-distance tracking of the unmanned aerial vehicle based on EKF angle estimation, and a robust initial state is provided for the system. In the second stage, a noise-truth value separation mechanism is constructed, and the correlation between an observation matrix and noise in a pseudo-linear equation is eliminated from the principle level; the method not only inherits the advantage of high efficiency of pseudo-linear filtering calculation, but also realizes active suppression of deviation in the dynamic tracking process through two-stage collaborative optimization. Theoretical analysis and simulation experiments show that compared with other nonlinear filtering algorithms, the method can still effectively suppress error fluctuation under extreme conditions of strong nonlinear observation, large-angle noise, long-distance observation and the like, and has better tracking performance.

Description

Technical Field

[0001] The present invention belongs to the field of positioning methods, and particularly relates to a method for UAV target positioning based on two-stage unbiased pseudo-linear Kalman filtering. Background Art

[0002] The optoelectronic system of an unmanned aerial vehicle (UAV) can collect image sequence data of a target and its surrounding area in real time, but it is difficult to directly and dynamically measure the three-dimensional coordinate trajectory of the target in the geographic coordinate system. Therefore, using the airborne optoelectronic system to achieve continuous and accurate tracking and positioning of the target, that is, UAV target geographic tracking and positioning, has become one of the key research issues of UAVs. Specifically, target geographic tracking and positioning refers to the process of continuously obtaining the navigation data of the UAV and the angle, distance, and speed information of the optoelectronic measurement device during the flight of the UAV, and combining the target motion characteristics and spatio-temporal correlation to real-time calculate and predict the dynamic motion trajectory and accurate position information of the target in the geographic coordinate system. This process not only requires instantaneous positioning of the target, but also requires establishing a target motion model to achieve continuous tracking and trajectory prediction of the target, ensuring stable and reliable full-process monitoring and positioning of the moving target.

[0003] Due to the many error factors and long transfer chain in UAV target tracking and positioning, and being often affected by harsh environments such as strong winds and vibrations during the actual mission execution, the airborne optoelectronic platform is severely affected by non-linear factors during mission observation. In addition, when the UAV performs reconnaissance and surveillance tasks in dangerous target areas or non-airspace areas, the optoelectronic platform mainly images in the way of high-altitude oblique view, long distance, large area, and its observation conditions often have extreme characteristics of large inclination angle, long distance, long focal length, and strong non-linearity, resulting in a serious ill-condition of the observation equation of the optoelectronic platform, and the target tracking and positioning accuracy is very sensitive to the observation error caused by perturbations. Therefore, it is of great significance to carry out research on target tracking and positioning methods with strong computational robustness and high tracking accuracy.

[0004] Scholars at home and abroad have mainly carried out a series of studies on the problem that the target tracking and positioning accuracy is significantly reduced due to strong nonlinear factors from two paths: active tracking and positioning, and passive tracking and positioning. Active tracking and positioning uses active detection means such as laser target indicators to obtain the distance and angle information between the UAV and the target in real time, and combines the target motion model to achieve continuous tracking and high-precision positioning of the target. Liu et al. used the 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 optoelectronic pod in continuous target tracking by correcting the relative angular displacement between the unmanned aerial vehicle and its onboard strapdown optoelectronic platform and combining 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 means of the real-time zoom lens distortion correction method, the circular error probability (CEP) of multi-target tracking and positioning was reduced by 7%.

[0005] However, active tracking and positioning methods need to actively emit radiation signals during operation, which may lead to the exposure of the UAV's own position and increase the risk of mission execution. In addition, since the laser ray usually coincides with the platform's optical axis, active positioning can only obtain the distance information of the target at the center point of the field of view and it is difficult to track multiple dynamic targets simultaneously at the same moment, which limits its application in multi-target tracking scenarios. Therefore, although active tracking and positioning shows high accuracy and reliability in single-target continuous tracking, its applicable scenarios still have certain limitations. With the development of sensing technology, the cost, volume, and power consumption of vision sensors have gradually decreased. In recent years, using vision sensors (such as visible light cameras, thermal imaging sensors, depth image sensors, optical flow sensors, etc.) for UAV navigation and positioning has become a research hotspot. This positioning method passive receives the signal source radiated by the target during mission execution without actively making optical or electrical contact with the outside world, which maximally ensures the safety of the UAV itself and is called a passive positioning method. Kang et al. measured the angle of arrival (AOA) of two UAVs to achieve passive positioning of unknown targets. At the same time, a weighted least squares (WLS) estimator was designed to effectively reduce the estimation error of the target position. Zhao et al. used the δ-generalized labeled multi-Bernoulli filter to overcome the influence of the non-linear motion of the radiation source and accurately track the target and capture its motion trajectory. XU et al. established a non-linear 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, the cubature Kalman filter (CKF) algorithm was used to estimate the target position. Luo et al. proposed an improved three-stage extended Kalman filter (3S-EKF) based on geometric constraints, which can solve the coupling problem between the azimuth angle and elevation angle measurement equations. Lin et al. used the 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 non-linear algorithms are all based on Gaussian noise and have relatively limited ability to solve tracking and positioning problems with strong non-linear characteristics, and the computational complexity is relatively high.

[0006] The pseudo-linear filtering method replaces the traditional non-linear azimuth measurement equation with a pseudo-linear estimation equation, which not only reduces the computational complexity of the algorithm but also shows higher adaptability during the system initialization stage, reducing the stringent requirements for initial parameter settings. Cheng et al. combined the measurement matrix estimation and the pseudo-measurement error, and used the full linear sequential filtering framework to estimate the combined comprehensive error; Yang et al. designed a pseudo-linear Kalman filtering algorithm in the presence of random position errors of the observer, effectively solving the problem of reduced state estimation accuracy caused by random noise interference of the observer's position. However, during the target tracking process of the UAV optoelectronic system, the highly non-linear characteristics of the observation model will cause serious deviation problems in the pseudo-linear Kalman filtering algorithm, thereby affecting the tracking performance. Regarding the above problems, references xx-xx deeply analyzed the deviation characteristics in the pure azimuth target motion, introduced the deviation compensation mechanism into the pseudo-linear estimation system, and thus proposed the bias compensated pseudo-linear Kalman filtering algorithm (BC-PLKF). This algorithm significantly alleviates the deviation problem existing in the traditional pseudo-linear filtering algorithm by effectively reducing the correlation between the measurement matrix and the pseudo-linear noise, and has achieved remarkable results in improving the target tracking accuracy. However, the application effect of the BC-PLKF algorithm largely depends on the assumption of small azimuth angle noise. In the actual operating environment of UAVs, due to the existence of many complex interference factors, when the azimuth angle noise is large, the deviation compensation efficiency of this algorithm will significantly decay, making it difficult to continuously maintain a high-precision target tracking ability. The instrumental variable Kalman filter (IVKF) uses the instrumental variable matrix to reduce the correlation between the pseudo-measurement matrix and the pseudo-linear noise, and further improves the state estimation performance on the premise of ensuring the stability and low complexity of the PLKF. However, the instrumental variable is constructed by the BC-PLKF method. As the observation noise increases, the correlation between the instrumental variable and the observation matrix weakens, significantly reducing the performance of the IV-PLKF. Yang et al. proposed the distributed instrumental variable Kalman filter (DIVKF) using finite-time average consensus, which gradually approaches the true values of the relative distance and angle through iteration to achieve higher filtering accuracy.

[0007] There are also existing technologies that use a selective angle measurement strategy to impose constraints on the construction of instrumental variables, thereby achieving the asymptotic unbiasedness of state estimation. Although these methods have certain innovations in the reconstruction of instrumental variables, they still do not completely solve the correlation problem between the pseudo-measurement matrix and the pseudo-linear noise in essence. Their performance is limited by noise sensitivity and the limitations of instrumental variable construction. Especially under the conditions of large inclination angles and long-distance observations of unmanned aerial vehicles (UAVs), the ill-conditioning of the observation equation leads to a sharp decline in tracking accuracy. Summary of the Invention

[0008] To address the above problems, the present invention incorporates the idea of separating noise and true values into the PLKF framework and proposes a UAV target localization method based on two-stage unbiased pseudo-linear Kalman filtering (2S-UPLKF). The angle estimation based on the EKF in the first stage can effectively decouple noise interference during long-distance tracking of UAVs and provide 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 noise in the pseudo-linear equation at the principle level. This algorithm not only inherits the advantage of high computational efficiency of pseudo-linear filtering but also actively suppresses the deviation during the dynamic tracking process through two-stage collaborative optimization. Theoretical analysis and simulation experiments show that compared with other non-linear filtering algorithms, 2S-UPLKF can still effectively suppress error fluctuations under extreme conditions such as strong non-linear observations, large-angle noise, and long-distance observations, and has better tracking performance.

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

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

[0011] Step 1: Collect the target state x k-1|k-1 and covariance matrix P k-1|k-1 , the state noise matrix Q, and the measurement noise matrix R data are input into the angle estimation algorithm based on the EKF. The angle estimation algorithm based on the EKF sequentially performs a prediction stage, an update stage, and an angle estimation stage, and finally outputs the predicted value of the target state of the angle estimation algorithm based on the EKF at time k

[0012] Prediction stage: Calculate the prior estimate of the state quantity at time k and the prior estimate of the error covariance P k|k-1 ;

[0013] F k is the state transition matrix at time k;

[0014] P k|k-1 = F k P k-1|k-1 Fk T +Q k

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

[0016]

[0017] where J k is the linear observation matrix at time k, and z k is the actual measured value of the target position at time k;

[0018] Angle estimation phase: Calculate the predicted target state of the EKF-based angle estimation algorithm at time k:

[0019] h(·) is the non-linear observation function

[0020] Output:

[0021] Step 2. Input the target state x k-1|k-1 and covariance matrix P k-1|k-1 at time k-1, the state noise matrix Q, and the measurement noise matrix R, together with the predicted target state of the EKF-based angle estimation algorithm at time k, into the UPLKF-based state estimation algorithm. The UPLKF-based state estimation algorithm performs the prediction phase, update phase, and angle estimation phase in sequence, and finally outputs the predicted target state and the target state error covariance

[0022] Prediction phase: Calculate the prior state estimate and prior error covariance estimate

[0023] F k is the state transition matrix at time k;

[0024] P k|k-1 = F k P k-1|k-1 F k T +Q k

[0025] Update phase: Calculate the posterior state estimate of the UPLKF-based algorithm at time k and the posterior error covariance estimate

[0026] The actual pseudo-measurement value of the target position at time k, is the equivalent pseudo-measurement value of the target position at time k;

[0027] m k1 = [-sinβ k , -cosβ k , 0]Mx k + ||d k ||cosε k ;

[0028]

[0029] β k and ε k are the azimuth angle and elevation angle of the target at time k, and are the observed values of the azimuth angle and elevation angle of the target affected by noise at time k, M = [I 3×3 , 0 3×3 , x k is the state vector of the target at time k;

[0030] is the equivalent observation matrix of the target position at time k, is the estimated value of G k ;

[0031]

[0032] Output:

[0033] Step 3, Take as the target state x k|k at time k, as the covariance matrix Pk|k at time k, and substitute them into Steps 1 and 2 for iterative calculation to obtain the target state at time k + 1.

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

[0035] After the present invention adopts the above technical solutions, compared with the prior art, it has the following advantages:

[0036] By using the state equation of the imaging system of the UAV optoelectronic platform and combining swarm intelligence optimization and particle filtering techniques, the geographical location of the target is optimally estimated, so as to realize the accurate positioning of the ground target by the UAV under the influence of severe nonlinearity. The main innovations of the present invention can be summarized as follows:

[0037] (1) Incorporate the optimization mechanism of the firefly algorithm into the framework of the particle filter algorithm to guide the particles to move towards the high-likelihood region;

[0038] (2) Introduce a multi-mutation strategy and a spring force mechanism to change the mode of particle interaction, and solve the particle degradation problem caused by severe nonlinear factors and over-optimization;

[0039] (3) Use the mutated firefly optimized particle filter to perform optimal estimation of the target position, reduce the number of particles required for the operation of the standard particle filter algorithm, and improve the robustness and positioning accuracy of the algorithm.

[0040] The present invention will be described in detail below with reference to the accompanying drawings and embodiments. Description of the Drawings

[0041] Figure 1 is the definition of the coordinate system and its relationships, where (a) is the correlation diagram between the terrestrial rectangular coordinates and the geographical coordinates; (b) is the correlation diagram between the geographical coordinates and the UAV coordinates; (c) is the correlation diagram between the UAV coordinates and ca;

[0042] Figure 2 is the flow schematic diagram of the present invention;

[0043] Figure 3 is the schematic diagram of the UAV AOA target tracking model;

[0044] Figure 4 is the schematic diagram of the UAV-target motion state;

[0045] Figure 5 is the schematic diagram of the azimuth angle and the pitch angle;

[0046] Figure 6 is the schematic diagram of the error variation of the position and speed estimation under different measurement noise conditions for different algorithms;

[0047] Figure 7 is the schematic diagram of the variation of the position and speed RMSE of different algorithms with the initial distance. Detailed Embodiment

[0048] The principles and features of the present invention will be 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.

[0049] In the description of the present invention, it should be noted that the orientation or positional relationship indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings. It is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation. Therefore, it should not be construed as a limitation to the present invention.

[0050] 1 R & D Background

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

[0052] 1.1 Definition of Coordinate System

[0053] The model described in the present invention has seven sets of spatial coordinates, as described below:

[0054] (1) Geodetic Coordinate System GCF(O gcf -LBH): Based on the International Terrestrial Reference System WGS-84. GCF takes the center of the earth ellipsoid as the origin. It is a three-dimensional curvilinear coordinate system that uses longitude L, latitude B, and height H to describe the position of points on the earth's surface.

[0055] (2) Earth-Centered Earth-Fixed Coordinate System ECEF(O ecef -X ecef Y ecef Z ecef ): An inertial coordinate system, as shown in Figure 1 (a). ECEF also takes the center of the earth ellipsoid as the origin. The positive direction of the Z ecef axis points to the north pole of the earth, the positive direction of the Y ecef axis points to the intersection of the prime meridian and the equator, and the positive direction of the X ecef axis is perpendicular to the other two axes and points to the equator.

[0056] (3) Navigation Coordinate System NED(O ned -X ned Y ned Z ned ): Figure 1 The red coordinate system with the origin at point K in Figure (a) and Figure 1 (b) the red coordinate system with the origin at the center of mass of the UAV are the same coordinate system, that is, the NED coordinate system. NED takes the center of mass of the UAV as the reference origin. The positive direction of the X ned axis points to the north of the earth, the positive direction of the Y ned points to the east of the earth, and the Zned The positive direction of the axis is perpendicular to the Earth's surface and downward.

[0057] (4) Body coordinate system HRD(O hrd -X hrd Y hrd Z hrd ):As Figure 1 shown in figure (b) below, the origin of the green coordinate system located on the UAV is the HRD coordinate system, and its origin coincides with NED. The positive direction of the X hrd axis points to the front of the carrier aircraft, the positive direction of the Y hrd axis points to the right wing direction of the carrier aircraft, and the positive direction of the Z hrd axis is along the vertical axis of the carrier aircraft and downward. The relationship between HRD and NED is as Figure 1 shown in figure (b) below. The three-axis attitude angles λ, θ, and κ are measured by the inertial navigation system. The dark green coordinate system (O' hrd -X' hrd Y h ' rd Z' hrd ) is the coordinate system after translating the origin of HRD to the optical center of the camera in the optoelectronic platform, and the translation vector is t.

[0058] (5) Camera coordinate system C(O c -X c Y c Z c ): Figure 1 shown in figure (c) below, the origin of the orange coordinate system located in the pod is the C coordinate system, and its origin is the optical center of the camera. The positive direction of the X c axis and the positive direction of the Y c axis are parallel to the image plane and point to the lower right and lower left of the camera respectively. The positive direction of the Z c axis points along the optical axis to the imaging direction. The attitude angle xx of the pod is measured by the optoelectronic encoder.

[0059] (6) Image physical coordinate system W(o-x w y w ): Figure 1 (c) The dark blue coordinate system with the origin at the center of the image plane is the W coordinate system, and the x w axis and the y w axis are parallel to the X c axis and the Y c axis respectively.

[0060] (7) Image coordinate system I(I-xy): Figure 1 (c) The light blue coordinate system with the origin at the upper left corner of the image plane is the I coordinate system, and the x-axis and y-axis are also parallel to the X c axis and the Y c axis.

[0061] 1.2 Target Position Estimation Model Based on Optimizing Particle Filter with Improved Cuckoo Algorithm

[0062] The azimuth and elevation angle data of the target relative to the pod obtained by the airborne optoelectronic system will be transmitted to the carrier aircraft. Combining the geographical coordinates and flight attitude information provided by the carrier aircraft through GPS / INS, after a series of coordinate transformation and conversion steps, the positioning of the target is completed. Since the measurement data of the pod is obtained based on its own coordinate system, and the final position of the target needs to be represented in the geodetic coordinate system, corresponding coordinate transformation must be carried out, as Figure 2 shown by the dashed box in

[0063] The process of the UAV determining the target position in the geodetic coordinate system through the optoelectronic platform is as follows: First, the camera scans the target area. Once the target is detected, its pixel coordinates (u, v) in the image coordinate system are obtained through the image tracker. Combining the known camera focal length f, the LOS vector in the camera coordinate system C can be calculated as [x C , y C , f, 1] T . During the target search process, the rotation of the camera in the reference coordinate system is represented by the azimuth and pitch angles (α c , β c ). In addition, the flight attitude of the UAV will also affect the LOS vector, and its attitude angles are (ψ, φ, θ). Translating operations between different coordinate systems will not change the direction of the line of sight (LOS). In the camera coordinate system, the LOS vector can be rotated multiple times to be converted into the vector pointing from the UAV to the target in the navigation coordinate system NED. Therefore, through the coordinate transformation A, the optoelectronic platform can achieve the relative angle measurement of the optical axis pointing in NED in the inertial state with the above parameters. The measurement accuracy of these parameters determines the accuracy of the arrival angle (AOA) estimation of the target relative to the UAV. Finally, GPS provides the position (L, B, H) of the UAV in the geodetic coordinate system, and the geodetic coordinates of the final target can be obtained through the coordinate transformation B.

[0064] The C coordinate system is the starting coordinate system for positioning the target in the coordinate transformation A. Then the LOS vector is represented in homogeneous coordinates as

[0065]

[0066] In the formula, c x , c y are the central pixel points of the image, that is, the pixel coordinates of the intersection of the optical axis and the image plane; d x , d y are the horizontal and vertical dimensions of the pixel.

[0067] Through Figure 1The four rotations in Figures (b) and (c) can yield the vector from the camera optical center to the target in the ECEF coordinate system (hereinafter referred to as the pointing vector) as follows:

[0068]

[0069] wherein, is the rotation matrix, and the expressions of these rotation matrices are given in Appendix A. Then, the azimuth angle and pitch angle of the target observed by the UAV pod in the navigation coordinate system are respectively

[0070]

[0071] By estimating the AOA through the pointing vector with the help of Equations (2) and (3), the coordinates v′ of the target relative to the UAV in the navigation coordinate system NED can be obtained NED =[x′ NED ,y′ NED ,z′ NED ,1] T . As shown in Figure 1 (a), combined with the GPS position (L, B, H) of the UAV and the installation deviation t = [t x ,t y ,t z ,1] of the pod relative to the aircraft centroid, the navigation coordinate system NED can be converted to the Earth-centered rectangular coordinate system ECEF through the offset of H along the X ned axis, the rotation of L around Y ned , the rotation of -B around Z ned , and the offset of -Ne 2 sin L along the Z ned axis:

[0072]

[0073] wherein, is the rotation matrix, and the expression of this rotation matrix is given in Appendix B. The formula for converting the target point v ECEF to geodetic coordinates can also be found in Appendix B.

[0074] Appendix A Rotation matrices involved in coordinate transformation A

[0075] Rotation matrix from the camera coordinate system to the body coordinate system:

[0076]

[0077] wherein, Rotation matrix from the body coordinate system to the navigation coordinate system:

[0078]

[0079] In the formula,

[0080] So far, the AOA estimation of the target relative to the UAV and the position estimation of the UAV in the geodetic coordinate system are completed. Obviously, the accuracy of the AOA estimation directly affects the positioning accuracy of the UAV for the target. Therefore, the main contribution of the filter proposed in the present invention lies in quickly and accurately estimating the relative position through the azimuth angle and elevation angle of the pointing vector.

[0081] 1.3 UAV measurement equation based on AOA

[0082] The geometric description of the UAV AOA target tracking problem is as Figure 3 shown. And are the position and velocity of the UAV at time k, k ∈ {0, 1, 2,...}; the position and velocity of the i-th ground target at time k are unknown parameter vectors that need to be estimated for 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. The relative position and velocity are selected as the motion state vector of the i-th target at time k, that is

[0083]

[0084] Then the state transition equation of N targets at time k is

[0085] x k = F k-1 x k-1 + ω k-1 , k ≥ 0. (6)

[0086] In the formula, is the state vector; F k-1 = blkdiag(F 1,k-1 , F 2,k-1 , …, F i,k-1 , …, F N,k-1 ) is the state transition matrix, which is related to the kinematic model of the target, and blkdiag represents a block diagonal matrix; the system process noise ω k-1 obeys an independent zero-mean additive Gaussian distribution, and its covariance matrix Q k-1 has the same element order as F k-1 , which will not be elaborated here.

[0087] Assuming that the target moves at a constant speed and the influence of altitude is ignored, the expressions of the transfer matrix F i,k-1 and the process noise covariance matrix Q i,k are

[0088]

[0089] In the formula, T s is the sampling interval; Q ρ = diag(σ x , σ y , σ z ), where σ x , σ y , and σ z are the components of the power spectral density of the process noise on the x, y, and z coordinate axes, respectively.

[0090] The AOA of the i-th ground target relative to the UAV at time k includes the azimuth angle β k and the pitch angle ε k , and β k ∈ (-π, π) and ε k ∈ (-π / 2, π / 2). Its observation equation is

[0091]

[0092] In the formula: and are the observed values affected by noise; the observation noise vector n k = [n β,k , n ε,k T obeys an independent zero-mean Gaussian random distribution and is independent of the system process noise. Its covariance matrix The nonlinear observation function h(·) = [h β (·), h ε (·)] T The calculation formula is

[0093]

[0094] Therefore, the observation equation of the UAV for N targets at time k is

[0095]

[0096] In the formula, is the observation vector; h(x k ) and n k are the corresponding sets of nonlinear functions and the observation noise vector, respectively; the covariance matrix of n k is R k = blkdiag(R 1,k-1 , R 2,k-1 , …, R N,k-1 ).

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

[0098]

[0099] From the characteristics of the air environment, it is known that n β,k << 1, then sin n β,k ≈ n β,k , cos n β,k ≈ 1, n ε,k Similarly.

[0100] Then

[0101]

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

[0103]

[0104] Where M = [I 3×3 , 0 3×3 , ηk is k the equivalent measurement error at time, and this error follows a normal distribution with a mean of 0 and a variance of R η,k The formula is:

[0105]

[0106] Then the final UAV state space model based on AOA consists of equations (6) and (15). Under the condition of obtaining the initial state estimate The UAV target tracking problem can be equivalent to estimating the target state x using the state transition equation and the pseudo-observations at historical times k .

[0107] 1.4 Observability analysis

[0108] The measurement model based on AOA can use the LOS axis azimuth information of the optoelectronic pod to estimate the target state. However, due to the inherent non-linear characteristics of using the azimuth angle β k and the elevation angle ε k for ground target positioning, even if the filtering structure is correct and the parameter measurements are accurate, in some cases, the target state estimate may still not converge. This requires solving the problem of the complete observability of the target state in pure azimuth measurement, that is, whether there is a unique tracking solution for the system.

[0109] According to the observability criterion: for the n-dimensional vector x in the initial set S 0, there is a Grammian matrix

[0110] Γ(k,k+N-1) = [H k H k+1 Φ…H k+n-1 Φ n-1 T (17)

[0111] In the formula If there exists a positive integer N such that the rank of Γ satisfies

[0112] rank(Γ(k,k+N-1)) = n (18)

[0113] Then the system is completely observable at s.

[0114] Without considering measurement noise, when k = 1, 2, …, k, k≥2, there is:

[0115] z β,k = H β,k x β,k (19)

[0116] In the formula, x β,k = [x k , y k T is the state variable corresponding to the azimuth measurement equation. Then the pseudo-observation matrix is defined as

[0117]

[0118] In the formula, N = [I 2×2 , O 2×1 T

[0119] Let Then there is

[0120]

[0121] After derivation, the determinant value of Γ β is:

[0122]

[0123] According to the Cauchy inequality, det(Γ β )≥0, and det(Γ β ) = 0 if and only if tanβ is always a constant. Therefore, as long as the UAV does not always fly in the azimuth direction of the target, that is, the radial motion, the state variable x βi,k is observable. Similarly, using the pitch angle information, it can be proved that when and only when tanε is not always a constant, the state variable x εi,k ​​​It is observable. However, drones often observe targets at large inclination angles and long distances, and their limited power results in slow climbing, with very small pitch angles and pitch angle change rates. Although the system is observable, the observability is weak, and the target positioning convergence speed is slow and prone to divergence.

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

[0125] 2. The method of the present invention

[0126] 2.1 Pseudo-linear Kalman filter

[0127] This section will introduce the basic principle of the pseudo-Kalman filter algorithm and analyze its deviation.

[0128] The pseudo-linear equation (15) converts the non-linear observation equation (8) into a linear form. Then, the Kalman filter is applied to the pseudo-linear system composed of equations (6) and (15), and the following steps of the pseudo-linear Kalman filter algorithm (PLKF) are obtained: Step 1 State update phase:

[0129]

[0130] Step 2 Measurement update phase:

[0131]

[0132]

[0133] Where, and P k|k-1 are the state prediction value vector and the prediction error covariance matrix respectively; K k is the Kalman gain matrix; and P k|k are the state estimate value vector and the estimate error covariance matrix respectively; R k is the measurement noise matrix, related to d i,k|k-1 , related.

[0134] Conduct a deviation analysis on PLKF. Substitute equation (25) into equation (27), and the equivalent expression of P k∣k can be obtained

[0135]

[0136] Then apply the matrix inversion formula to equation (25), and the following can be obtained

[0137]

[0138] Substituting formula (29) into formula (26), we can get

[0139]

[0140] Formula (30) minus the true state x k The instantaneous estimated deviation is

[0141]

[0142] Therefore, the deviation of PLKF estimation can be obtained by taking the expectation of equation (31) as

[0143] γ k =E{σ k}=E{σ k1}+E{σ k2}+E{σ k3} (32)

[0144] Where:

[0145]

[0146] Therefore, the estimation bias consists of three parts: ① The bias E{σ k1};②By process noise ω k-1 With P k|k The deviation caused by correlation E{σ k2}; ③ Due to the pseudo-linearity of the system, the measurement matrix H k and the pseudo-linear noise vector η k There is a correlation, which causes the deviation E{σ k3}. Among them, E{σ k1} is the inherent deviation of the system. Secondly, since the target process with constant speed has less noise, ω k-1 With P k|k The correlation is weak, E{σ k2}≈0. However, H k Both E{σ k3}≠0. Therefore, through the above deviation analysis of the pseudo-linear system, it can be seen that H k With η k The correlation of is the fundamental reason for the bias of PLKF. The size of its deviation is only related to the size of the measurement noise, and has nothing to do with the observed amount. The increase in the observed amount does not reduce the estimation deviation. In addition, when the measurement noise is small, the deviation is not obvious, but when the measurement noise increases, the deviation caused by pseudo-linearization increases rapidly, causing the algorithm estimation performance to degrade significantly. In order to ensure the accuracy of target tracking, the deviation must be processed.

[0147] 2.2 Two-stage Unbiased Pseudo-linear Kalman Filter Algorithm

[0148] Regarding the bias problem of PLKF, the current solutions are mainly divided into two categories: one is the bias-compensated PLKF (BC-PLKF), and the other is the instrumental variable-based PLKF (IV-PLKF). BC-PLKF improves the target tracking accuracy by calculating the estimation bias of PLKF and performing re-compensation. However, this method is only applicable to the case of small measurement noise. When the measurement noise is large, the debiasing effect drops significantly. And IV-PLKF uses instrumental variables to reduce the correlation between H k and η k , and effectively reduces the bias caused by pseudo-linearization in principle. The instrumental variable is constructed by the BC-PLKF method. As the observation noise increases, the correlation between the instrumental variable and H k weakens, significantly reducing the performance of IV-PLKF. Therefore, aiming at the above problems, the present invention integrates the idea of separating noise and true value into the PLKF framework, and proposes a two-stage unbiased pseudo-linear Kalman filter algorithm (2S-UPLKF) for UAV ground target tracking. First, an EKF is designed as the angle filter in the first stage to perform posterior estimation on the azimuth angle and pitch angle of the UAV LOS, and solve the problem of algorithm divergence caused by noise coupling in long-distance scenarios. Then, by separating the noise term from the observation matrix, a second-stage filter - unbiased pseudo-linear Kalman filter (UPLKF) is constructed, realizing the unbiasedness of the algorithm in principle. The workflow diagram of 2S-BPLKF is as shown in Figure 2 the green box in

[0149] 2.2.1 The First Stage: Angle Estimation Based on EKF

[0150] EKF is a non-linear filtering algorithm without principle bias, with small computational complexity and stable angle estimation. According to the original state models of equations (6) and (10), using the Taylor formula to approximate the non-linear model as a linear model, the linear observation matrix can be obtained as

[0151]

[0152] where is the distance prediction value.

[0153] Then, the steps of angle estimation based on EKF in the first stage are as follows.

[0154] Angle Estimation Algorithm Based on EKF in the First Stage

[0155] Input: The state vector input into the first-stage algorithm at time k-1 is the covariance matrix at time k-1, Q is the state noise matrix, and R is the measurement noise matrix.

[0156] For k = 1, 2, …, N

[0157] Prediction stage: Calculate the prior estimate of the state quantity and the prior estimate of the error covariance

[0158] Step 1: F k is the state transition matrix.

[0159] Step 2:

[0160] Update stage: Calculate the Kalman gain, the posterior estimate of the state, and the posterior estimate of the error covariance

[0161] Step 1:

[0162] Step 2: z k is the actual measurement value

[0163] Step 3:

[0164] Angle estimation stage: Calculate the posterior estimate values of the azimuth angle and the elevation angle

[0165]

[0166] Output:

[0167] 2.2.2 Second stage: State estimation based on UPLKF

[0168] UPLKF separates the angular measurement noise from the observation matrix, thus avoiding the operation of bias compensation. The unbiasedness achieved at the principle level enables the algorithm to effectively ensure the convergence of the target state estimation. For the azimuth measurement equation, there is

[0169]

[0170] Substituting equations (34) and (35) into equation (15) gives

[0171]

[0172] where

[0173]

[0174] m k1 = [-sinβ k , -cosβ k , 0]Mx k + ‖d k ‖cosε k (38)

[0175]

[0176] Multiplying both sides of Equation (37) by the diagonal matrix diag([1 / m k1 , 1 / m k2 ), we can obtain

[0177]

[0178] It should be noted that the state vector x k in Equation (40) is independent of the observation noise, and the noise in the equation is the angular measurement noise n k , rather than the pseudo-linear noise η k .

[0179] According to the result of Equation (32), the principle deviation

[0180]

[0181] Since and are respectively independent of the angular measurement noises n β,k and n ε,k , so E{σ k3} = 0, indicating that the algorithm is unbiased. In the actual application process, since the true values of the observed angles β k , ε k and the distance vector d k are unknown, only the estimated values can be used instead. This idea is similar to IV-PLKF, both trying to construct a noise-free observation vector. And ensuring that the estimation matrix has a strong correlation with the true value G k is the main convergence condition of IV-PLKF. If the angle estimation value of this method is directly used as the true value input, it may lead to a lack of correlation between and the true value G k due to inaccurate initial estimation, resulting in the divergence of the target motion state estimation. Therefore, the posteriori estimation values of the azimuth angle and elevation angle obtained by the first-stage EKF algorithm are used as the angle true values of this algorithm to obtain

[0182]

[0183] Then, the state estimation steps based on UPLKF in the second stage are organized as follows.

[0184] Input of the state estimation algorithm based on UPLKF in the second stage: is the state vector re - input to the second - stage algorithm at time k - 1.

[0185] For k = 1, 2, …, N

[0186] Prediction stage: Calculate the prior estimate of the state quantity and the prior estimate of the error covariance

[0187] Step 1:

[0188] Step 2: Update stage: Calculate the Kalman gain, the posterior estimate of the state, and the posterior estimate of the error covariance

[0189] Step 1: is the actual pseudo - measurement value, is the equivalent pseudo - measurement value is the equivalent observation matrix.

[0190] Step 1:

[0191] Step 2:

[0192] Step 3:

[0193] Output:

[0194] 2.3 Analysis of the time complexity of the algorithm

[0195] 3 Performance metrics

[0196] 3.1 Precision metrics

[0197] Assume and represent the true state vector and the estimated state vector at time k in the j - th experiment respectively. Then the expressions for the bias and RMSE are as follows:

[0198]

[0199] where M is the number of experiments. Further, the time - averaged bias and RMSE of target tracking can be obtained:

[0200]

[0201] Where \(U = N - L + 1\), \(N\) is the total number of time instants in the tracking process, and \(L\) is the number of time instants when the error starts to be recorded.

[0202] 3.2 CRLB

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

[0204]

[0205] Where \(J\) k is the Fisher information matrix, and its calculation formula is

[0206]

[0207] Where \(Q\) k-1 and \(R\) k are the covariance matrices of the process noise and the observation noise respectively; \(F\) k-1 is the Jacobi matrix of the system state equation (6) at \(x\) k-1 and is equal to the state transition matrix; is the Jacobi matrix of the nonlinear observation equation (10) at \(x\) k and its calculation formula is

[0208]

[0209] Where

[0210]

[0211] Therefore, the time - averaged PCRLB of target tracking is

[0212]

[0213] Appendix A Rotation matrices involved in coordinate transformation A

[0214] Rotation matrix from the camera coordinate system to the body coordinate system:

[0215]

[0216] Where Rotation matrix from the body coordinate system to the navigation coordinate system:

[0217]

[0218] Where The rotation matrix involved in coordinate transformation B in Appendix B

[0219] The rotation matrix from the navigation coordinate system to the geocentric rectangular coordinate system

[0220]

[0221] where

[0222]

[0223] L, B, H, N, and e are the longitude, latitude, altitude of the UAV, the radius of curvature of the earth's prime vertical, and the first eccentricity of the meridian ellipse, respectively.

[0224] 4.2 Simulation Analysis

[0225] 4.2.1 Simulation Comparison under Different Angular Measurement Noises

[0226] The simulation scenario is as Figure 4 shown. The starting point of the UAV is [200, 200, 500] T , and the speed is [20, 0, 0] T , and it circles around the center [200, 300, 500] T with a circling radius of 100 m. The sampling interval T = 0.1 s, and the optoelectronic system makes observations at each fixed moment (t = kT, k ∈ {0, 1, …, 499}). The number of Monte Carlo simulations is 5000 times. The observation noises of the UAV inertial navigation system and the optoelectronic system at each moment both follow independent zero-mean Gaussian distributions, and the standard deviations of the observation angle noises are all σ, σ = {0.1°, 0.15°, …, 0.5°}. The variation laws of the observation angles under different standard deviation conditions are as Figure 5 shown.

[0227] The target always remains at the center of the field of view. The starting point is the coordinate origin, and the speed is v 0 = [0, 5, 0] T , and the power spectral density of the process noise is set to q x = 2m 2 / s 3 , q y = 2m 2 / s 3 , q z = 0.2m 2 / s 3 . Due to the initial state of the target Affected by multivariate Gaussian noise, the initial condition setting of the filtering algorithm needs to focus on solving the parameter initialization problem without prior information constraints. Based on the error characteristics of the optoelectronic 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 P 0∣-1 = diag([4 2 , 4 2 , 4 2 , 0.5 2 , 0.5 2 , 0.5 2 ). The observation noise covariance matrix R and the process noise covariance matrix Q are both set to the actual system noise parameters.

[0228] To evaluate the tracking performance of the 2S-UPLKF algorithm, EKF, CKF, PLKF, and IVKF are selected as comparison algorithms, and the AOA target tracking errors of different algorithms are compared and analyzed through Monte Carlo simulation. Figure 6 The error variations of position and velocity estimations of different algorithms under different measurement noise conditions are compared.

[0229] From Figure 6 Figure (a) in, it can be seen that the EKF algorithm shows the largest position estimation deviation and has a rapid growth trend. Its deviation magnitude is positively correlated with the noise intensity σ; the PLKF algorithm maintains sub-meter positioning accuracy in the low-noise range of σ ≤ 0.35°, but the deviation grows rapidly under strong noise conditions, showing typical strong-noise failure characteristics; the CKF reduces the error amplitude by 42.3% through the third-order spherical-radial cubature rule, but due to the consistency failure caused by noise interference, its position deviation still gradually increases; the IVKF and 2S-UPLKF have relatively close position estimation accuracies under low-noise conditions, both controlled below 6m, but the deviation value of 2S-UPLKF at σ = 0.5° is 46% lower than that of IVKF, demonstrating a significant strong robustness advantage.

[0230] Figure 6The RMSE evolution curve of the position in Fig. (b) can reveal deeper characteristics of each algorithm. The position RMSE of the EKF algorithm is still the largest, because the sensitivity to the initial value and the divergence effect caused by the first-order nonlinearization; the PLKF algorithm is relatively close to the posterior Cramer-Rao bound in the low-noise range, but due to the coupling effect of the position deviation and the random error, the estimation error shows a non-linear growth trend when the noise is large; the CKF significantly reduces the position RMSE compared with the EKF, but is still slightly inferior to the IVKF and 2S-UPLKF under strong noise conditions; the IVKF solves the problem of noise correlation caused by pseudo-linearity through the bias compensation mechanism, and the position RMSE is reduced by 31.5% on average compared with the PLKF; it should be particularly noted that the RMSE index of the 2S-UPLKF continuously outperforms the comparison scheme under strong noise conditions of σ≥0.35°, especially when σ = 0.5°, it is reduced by 17.4% compared with the sub-optimal algorithm, indicating its superior anti-noise ability in the state estimation process.

[0231] Figure 6 Figs. (c) and (d) further verify the state estimation characteristics of each algorithm. The speed estimation index shows a similar change law to the position estimation, but the performance difference between algorithms slightly decreases. The 2S-UPLKF significantly outperforms the EKF and PLKF in terms of speed estimation performance, and the RMSE index is reduced by 30% and 20% compared with the CKF and IVKF respectively, demonstrating the technical advantages of two-stage collaborative estimation. From Figure 6 the comparative analysis, it can be seen that even when the speed estimation accuracy difference of each algorithm decreases, the 2S-UPLKF can still maintain a lower position tracking error, verifying the effectiveness of its non-linear noise debiasing ability in strong noise scenarios. The width of the 95% confidence interval of the performance indicators of different algorithms when σ = 0.5° is shown in Table 1. Among them, the interval width of the EKF is the largest, significantly higher than other algorithms. The position confidence interval and the speed confidence interval of the 2S-UPLKF are both the smallest, verifying the improvement of the algorithm's unbiasedness and anti-noise ability from a statistical level. In addition, the ratio of the confidence interval width to Figure 3-4 the error magnitude shown is stable within a small range, indicating Figure 6 the effectiveness of the error analysis results.

[0232] Table 1

[0233]

[0234] 5.2.2 Simulation Comparison under Different Initial Distance Conditions

[0235] Under the condition of a certain relative speed, the initial distance between the UAV and the target will affect the angle measurement. Therefore, this section mainly studies the change of the target state estimation accuracy under different initial distance conditions. Take the starting position vector [200, 200, 500] of the UAV and the target in Section 5.2.1 TAs the reference position vector, keeping the altitude of the UAV unchanged, different starting points are set within the range of 1 to 4 times the distance, and the interval step size is set to 0.25 times. During the experiment, the remaining experimental parameters are kept constant, and a unified standard deviation of the angular observation error σ = 0.1° is adopted to ensure the validity of the observation data. The variations of the position and velocity RMSE of different algorithms with the initial distance are as Figure 6 shown.

[0236] As can be Figure 7 seen, as the initial distance continuously increases, the estimation accuracy of each algorithm shows a gradually decreasing trend. When the initial distance is 1.75 times, the order of the position RMSE is EKF > PLKF > CKF > IVKF > 2S-UPLKF, and the velocity RMSE is EKF > CKF > PLKF > IVKF > 2S-UPLKF. Except for the EKF algorithm, the performance degradation of the position RMSE and velocity RMSE of other tracking algorithms is relatively small. When the initial distance is further extended to 2.5 times the reference value, the errors of each algorithm increase significantly and show obvious divergence. Among them, the errors of the EKF and CKF algorithms show significant fluctuations. In the range of 1.75 - 2.5 times the initial distance, their position RMSE indicators exceed those of other algorithms by up to 50 m, indicating that the ability to estimate the target motion parameters has been lost under this condition. In contrast, PLKF, IVKF, and 2S-UPLKF show significant robustness advantages, and their error growth shows an asymptotic characteristic. Among them, the error increase of 2S-UPLKF is the smallest, and it can always approach the PCRLB. In addition, when the velocity estimation error is large, the PLKF algorithm can still achieve more accurate position estimation accuracy. This result shows that: under the condition of increasing the initial distance, the pseudo-linearization strategy is more suitable for the UAV AOA target tracking task.

[0237] The comparison data of the computational time consumption of each algorithm are shown in Table 2. To facilitate the quantitative comparative analysis of the cross-algorithm performance, in this paper, the running time of the EKF algorithm is used as the reference benchmark, and the running times of other algorithms are normalized. The results show that the pseudo-linear calculation of PLKF slightly increases the computational overhead compared with EKF; due to the non-linear transformation of the equal-weight sampling point set, the computational overhead of CKF increases significantly; IVKF reduces the correlation between the pseudo-linear noise and the observation coefficient matrix through the instrumental variable, and its running time further increases compared with PLEKF; 2S-UPLKF needs to construct a two-stage filter, and its computational overhead is slightly higher than that of IVKF, but it is significantly lower than the running time of CKF.

[0238] Table 2 Comparison of relative running times of different algorithms

[0239]

[0240] Through the comparative analysis of the simulation experiments of the positioning algorithm under different measurement error levels and initial distance conditions, it can be seen that the 2S-UPLKF algorithm proposed in this paper not only shows higher tracking accuracy, but also exhibits significant advantages in terms of estimation stability, and its performance indicators do not show obvious fluctuations in multiple repeated experiments. It is worth noting that when the measurement noise variance σ ≤ 0.35° or the initial distance is relatively close, the RMSE of the 2S-UPLKF algorithm can converge to the theoretical optimal value of the Cramer-Rao lower bound. In addition, in a high-noise environment with σ ≥ 0.35° or a long-distance scenario, the position tracking accuracy is improved by about 31.6% on average compared with other algorithms, and the 2S-UPLKF algorithm shows strong environmental adaptability, further verifying its robustness advantages in complex environments.

[0241] 5 Conclusions

[0242] Aiming at the problems of strong nonlinear observations, large-angle noise interference and long-distance ill-conditioned equations in the geographical target tracking and positioning of unmanned aerial vehicles, a 2S-UPLKF robust tracking algorithm for effectively reducing estimation bias is proposed. The inherent bias problem of traditional pseudo-linear filtering is effectively solved through a two-stage collaborative optimization mechanism. In the first stage, the angle estimation based on EKF realizes noise decoupling and robust initialization, and in the second stage, the true value-noise separation mechanism eliminates the correlation between the observation matrix and noise at the principle level. This innovative design enables the algorithm to significantly improve the unbiasedness in dynamic tracking scenarios while maintaining computational efficiency. The simulation comparison results show that under the condition of 0.5° strong angle noise, the position RMSE of 2S-UPLKF is reduced by 17.4% compared with IVKF, the speed estimation error is reduced by 20%, and the width of the 95% confidence interval is the best among the comparison algorithms. Secondly, in the long-distance observation scenario with 2.5 times the initial distance, its tracking accuracy can still approach the theoretical lower bound of PCRLB, verifying the robustness of the algorithm under extreme observation conditions and providing a theoretical basis and algorithm foundation for the precise geographical tracking of moving targets in high-dynamic environments.

[0243] The above is an example of the best implementation mode of the present invention, and the parts not described in detail are all common general knowledge of those skilled in the art. The protection scope of the present invention shall be subject to the content of the claims, and any equivalent transformation based on the technical inspiration of the present invention is also within the protection scope of the present invention.

Claims

1. A UAV target positioning method based on two-stage unbiased pseudo-linear Kalman filtering, characterized in that: The following steps are involved: Step 1: Collect the target state x at time k-1 k-1|k-1 and 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 performs the prediction phase, the update phase, and the angle estimation phase in sequence. The final output is the target state prediction value based on the EKF angle estimation algorithm at time k. Prediction stage: Calculate the prior estimate of the state quantity at time k and the error covariance prior estimate P k|k-1 ; F k is the state transfer matrix at time k; Update phase: Calculate the state posterior estimate based on the EKF angle estimation algorithm at time k and the posterior estimate of the error covariance In the formula, J k is the linear observation matrix at time k, z k is 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 the nonlinear observation function Output: Step 2: The target state x collected at time k-1 k-1|k-1 and the covariance matrix P k-1|k-1 , the state noise matrix Q and the measurement noise matrix R and the target state prediction value based on the EKF angle estimation algorithm at time k The state estimation algorithm based on UPLKF is input together. The state estimation algorithm based on UPLKF performs the prediction phase, update phase and angle estimation phase in sequence. The final output is the target state prediction value based on UPLKF at time k. and the target state error covariance Prediction stage: calculate the prior estimate of state quantity and the prior estimate of error covariance F k is the state transfer matrix at time k; Update phase: Calculate the UPLKF-based state posterior estimate at time k and the posterior estimate of the error covariance is the actual pseudo-measurement value of the target position at time k, is 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 are the target azimuth and elevation angle at time k, and is the target azimuth and pitch angle observation value affected by noise at time k, M = [I 3×3 ,0 3×3 ], x k is the state vector of the target at time k; is the equivalent observation matrix of the target position at time k, G k An estimated value of Output: Step 3: As the target state x at time k k|k , As the covariance matrix P at time k k|k , substitute into step 1 and step 2 and iterate to get the target state at time k+1.

2. The method for unmanned aerial vehicle target positioning based on two-stage unbiased pseudo-linear Kalman filtering according to claim 1 is characterized in that: The initial position of the target is a preset fixed value.

Images