Unmanned aerial vehicle state estimation method based on square root lattice smooth variable structure Tobit filtering

Abstract

The invention provides an unmanned aerial vehicle state estimation method based on square root lattice smooth variable structure Tobit filtering, and the method comprises the steps: firstly constructing a non-cooperative unmanned aerial vehicle motion model under the condition that model parameters are uncertain and a non-linear deletion measurement model under the Tobit-II type truncation constraint, and carrying out the initialization; secondly, under a Bayesian theory framework and censoring measurement, calculating unmanned aerial vehicle state prediction based on a lattice sampling technology, and calculating measurement prediction based on cumulative distribution of censoring measurement; then, calculating a time-varying smooth boundary layer, state estimation gain, state estimation update and state estimation error covariance square root update under motion model uncertainty and nonlinear censoring measurement through a square root lattice method; and finally, outputting a position estimation result of the non-cooperative unmanned aerial vehicle. According to the method provided by the invention, the problem of high-precision robust state estimation of the non-cooperative unmanned aerial vehicle under the conditions of uncertainty of a motion model, occlusion deletion and measurement nonlinearity can be effectively solved.

Description

Technical Field

[0001] The present invention relates to the technical field of non-cooperative unmanned aerial vehicle state estimation, and in particular to a method for estimating the state of an unmanned aerial vehicle based on a square root lattice smooth variable structure Tobit filter. Background Art

[0002] Due to the advantages of low operating cost, high efficiency and easy portability, drones have been widely used in enemy reconnaissance, aerial photography, agriculture, environmental protection and power inspection. At the same time, due to factors such as safety and air traffic control, it is particularly important to estimate the position state of non-cooperative drones in the air. In the actual drone state estimation system, the radar measurement model is usually nonlinear; and due to obstructions such as obstacles, the measurement unit has the phenomenon of missing measurements; at the same time, the drone motion model parameters are uncertain. These interference factors will reduce or diverge the accuracy of traditional state estimation methods based on precise models and linear measurements, restricting the application effect of state estimation methods in non-cooperative drone state estimation. Therefore, the present invention studies the drone state estimation problem with missing measurements and uncertain interference of motion model parameters under nonlinear measurement, aiming to design a new non-cooperative drone state estimation method based on square root lattice smooth variable structure Tobit filtering. Summary of the invention

[0003] In response to the technical problems existing in the above-mentioned background technology, the present invention proposes a UAV state estimation method based on square root lattice smooth variable structure Tobit filtering, which can effectively overcome the interference of nonlinear measurement, missing measurement and uncertainty of motion model parameters in the non-cooperative UAV state estimation system, and provide high-precision state estimation.

[0004] In order to achieve the above object, the technical solution of the present invention is achieved as follows:

[0005] A method for estimating the state of a UAV based on square root lattice smoothed variable structure Tobit filtering, the steps are as follows:

[0006] S1. Construct a non-cooperative UAV motion model with unknown model parameters, a radar nonlinear measurement model under ideal measurement, and a radar censored measurement model under Tobit-Ⅱ type constraints;

[0007] S2. Initialization: Assume that the state of the drone at the initial k-1 moment is estimated to be The square root of the estimated error covariance is The convergence factor of the filtering algorithm is γ, and the a posteriori measurement estimation error is e z,k-1|k-1 ;

[0008] S3, calculate the next moment, i.e. the drone state prediction at moment k and the square root of the state prediction error covariance

[0009] S4. Based on the radar nonlinear measurement model and the censored measurement model, a nonlinear measurement model under censored constraints is constructed:

[0010] S5. Calculate measurement predictions under censoring constraints and the square root of the covariance of the measurement prediction errors

[0011] S6. Calculate the measurement prediction error covariance State and measurement prediction error cross-covariance

[0012] S7. Calculate the square root lattice smooth variable structure Tobit filter gain K k ;

[0013] S8, according to the filter gain K k Calculate the updated value of the drone state estimate at time k And the square root of the state estimation error covariance update value And the a posteriori measurement estimation error e at time k z,k|k ;

[0014] S9, output state estimation update value State estimation error covariance square root update value and the a posteriori measurement estimation error e z,k|k .

[0015] Preferably, the non-cooperative UAV motion model is:

[0016]

[0017] The radar nonlinear measurement model under ideal measurement is:

[0018]

[0019] in, represents the state vector of the drone in n-dimensional space at time k, x k-1 represents the state vector of the drone at time k-1, n is the state dimension, The superscript T indicates the matrix transpose, x k represents the eastward position of the UAV, η k Indicates the north position of the drone, represents the eastward speed, represents the north velocity; process noise ω k It has a mean of 0 and a variance of Q k Gaussian white noise; represents the potential ideal measurement value of the distance and azimuth of the UAV from the radar measurement point, m represents the dimension of the measurement vector, and the measurement noise υ k The mean is 0 and the standard deviation is r = [r1 r2] T , the variance is R k Gaussian white noise, and is the state transfer matrix under the uncertainty of the UAV model parameters, h k (x k ) is the nonlinear measurement matrix, and its specific expression is:

[0020]

[0021] The radar censored measurement model under Tobit-Ⅱ type censoring constraint is:

[0022]

[0023] Among them, z k represents the actual measured value under deletion; τ represents the deletion threshold; in this embodiment, the measurement dimension m = 2, then τ = [τ1, τ2] T , τ1 represents the distance deletion threshold, and τ2 represents the azimuth deletion threshold.

[0024] Preferably, in step S3, the drone state prediction at time k is calculated and the square root of the state prediction error covariance The calculation method is:

[0025] S31. Calculate the drone state prediction at time k

[0026]

[0027] Where N represents the number of lattice points, represents the j-th state prediction lattice point, which is calculated as follows:

[0028]

[0029] In the formula, Represents the state transfer matrix under uncertain model parameters The estimated value of k-1|k-1,j , j = 0, 1, ..., N-1 represents the initial state lattice point, and the calculation is as follows:

[0030]

[0031] Where Y j represents the cumulative distribution function The inverse mapping is calculated as follows:

[0032]

[0033] in, represents the transformation lattice point, represents the cumulative distribution function, which is calculated as follows:

[0034]

[0035] Among them, △ is the n-dimensional space [0,1) n Independent uniform distribution within, n is the state dimension; represents the initial lattice points generated by the rank 1 lattice rule, which is defined as follows:

[0036]

[0037] In the formula, g = [a 0 ,a 1 ,…,a n-1 ] T is a vector containing n integers, a is an integer coprime to N; j modN represents the remainder after j is divided by N;

[0038] S32. Calculate the square root of the state prediction error covariance

[0039]

[0040] in, represents the lower triangular decomposition, is a lower triangular matrix, Denotes the process noise covariance matrix Q k The square root of Cholesky decomposition is Represents the central weight matrix of the state prediction error:

[0041]

[0042] In the formula, let Represents the lattice point weight.

[0043] Preferably, in step S4, based on the radar nonlinear measurement model under ideal measurement and the radar censored measurement model under Tobit-Ⅱ type constraint, a nonlinear measurement model under censored constraint is constructed:

[0044]

[0045] Among them, I m represents the m-dimensional identity matrix, Represents the missing parameter; for the mth missing measurement parameter like This means that the measurement is not deleted. like This means that the measurement is censored. The specific form is as follows:

[0046]

[0047] Preferably, in step S5, the measurement prediction under the censoring constraint is calculated and the square root of the covariance of the measurement prediction errors The specific calculation method is:

[0048] S51. Calculate measurement predictions under censored measurements

[0049]

[0050] in, indicates that the cumulative distribution value for which the measurement is less than the censoring value, indicates that the cumulative distribution value of the measurement is greater than the censoring value, represents the potential ideal measurement prediction under uncensored measurement, Represents the inverse Mills ratio value; the specific calculation steps are as follows:

[0051] S51.1. Calculate the measurement prediction without deletion

[0052]

[0053] In the formula, is the jth measured predicted lattice point; k|k-1,j is the measured initial lattice point, calculated as follows:

[0054]

[0055] S51.2. Order The inverse Mills ratio The calculation is as follows:

[0056]

[0057] in, is the cumulative distribution of measures less than the censoring threshold; is the probability density of measuring less than the missing threshold, calculated as follows:

[0058]

[0059] S52. Calculate the square root of the measurement prediction error covariance

[0060]

[0061] in, represents the lower triangular decomposition, is a lower triangular matrix; Z k|k-1 The central weight matrix representing the measurement prediction error is calculated as follows:

[0062]

[0063] In the formula, Denotes the measurement noise covariance σ under the censoring constraint k The square root obtained by Cholesky decomposition is σ k The calculation of is as follows:

[0064]

[0065] Preferably, in step S6, the measurement prediction error covariance is calculated and the cross-covariance of state and measurement prediction errors The specific calculation method is:

[0066] S61. Calculate the measurement prediction error covariance

[0067]

[0068] S62. Calculate the cross-covariance of state and measurement prediction errors

[0069]

[0070] Preferably, the square root lattice smooth variable structure Tobit filter gain K k The expression is:

[0071]

[0072] in, Indicates H k The pseudo-inverse of z,k represents the prior measurement prediction error e z,k|k-1 and the a posteriori measurement estimation error e z,k-1k-1 The sum of the absolute values ​​of represents the multiplication of corresponding elements of the matrix, represents the saturation function, ψ k represents the time-varying smooth boundary layer; the specific calculation steps are as follows:

[0073] S71. Calculate the measurement matrix H k :

[0074]

[0075] in, represents the state prediction error covariance, and

[0076] S72. Calculate the priori measurement prediction error e z,k|k-1 and the a posteriori measurement estimation error e z,k-1|k-1 The sum of the absolute values ​​of :

[0077] e z,k =|e z,k|k-1 |+γ|e z,k-1|k-1 | (29)

[0078] in,

[0079] S73. Calculate saturation function

[0080]

[0081] in, represents the inverse of the time-varying smooth boundary layer and is calculated as follows:

[0082]

[0083] Preferably, the filter gain K k Calculate the updated value of the drone state estimate at time k And the square root of the state estimation error covariance update value And the a posteriori measurement estimation error e at time k z,k|k , respectively calculated as follows:

[0084]

[0085] in, represents the lower triangular decomposition, is a lower triangular matrix.

[0086] Beneficial effects of the present invention:

[0087] 1) The present invention constructs a non-cooperative UAV kinematic model, a radar nonlinear measurement model and a Tobit-Ⅱ type truncation constraint model under model parameter uncertainty interference, and solves the radar nonlinear modeling problem under censored measurement through the nonlinear measurement model and Tobit-Ⅱ type constraint.

[0088] 2) In the framework of Bayesian filtering, the present invention uses lattice sampling technology to predict the UAV status and radar potential ideal measurement, and calculates the cumulative distribution value of the missing measurement prediction, which avoids the error caused by the linearization process of nonlinear measurement and describes the probability of missing measurement through cumulative distribution.

[0089] 3) Under the Bayesian theoretical system and lattice sampling technology, the present invention calculates the square root of the state prediction error covariance, the square root of the measurement prediction error covariance, and the square root of the cross-covariance of the state and measurement prediction errors through the square root algorithm, reconstructs the calculation process of the time-varying smooth boundary layer under lattice sampling, and improves the stability of the algorithm.

[0090] 4) Based on the lattice sampling technology, the present invention proposes an equivalent form of the nonlinear measurement matrix under censored measurement, and solves the problem of calculating the gain of the nonlinear smoothly variable structure Tobit filter under censored measurement.

[0091] 5) Based on the proposed square root lattice smooth variable structure Tobit filtering algorithm, the present invention overcomes the interference of motion model parameter uncertainty, missing measurements and nonlinear measurements in the state estimation of non-cooperative UAVs, and provides an accurate and robust state estimation strategy for the state estimation of non-cooperative UAVs. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0093] Figure 1 Flowchart for UAV trajectory estimation.

[0094] Figure 2 This is the flow chart of the Tobit filtering algorithm based on square root lattice smoothing variable structure.

[0095] Figure 3 Distribution map of the true value of radar distance measurement.

[0096] Figure 4 This is the true value distribution diagram of radar azimuth measurement.

[0097] Figure 5 This is a comparison diagram of the actual trajectory of the UAV and the estimated trajectory obtained by using the method of the present invention.

[0098] Figure 6 It is the position root mean square error result obtained by using the method of the present invention. DETAILED DESCRIPTION

[0099] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0100] like Figure 1 As shown, an embodiment of the present invention provides a method for estimating the state of a UAV based on a square root lattice smooth variable structure Tobit filter, and takes the state estimation of a non-cooperative UAV in a constant turning motion process with uncertain model parameters as an example for explanation, and the specific steps are as follows:

[0101] S1. Construct a non-cooperative UAV motion model with unknown model parameters (1), a radar nonlinear measurement model under ideal measurement (2), and a radar censored measurement model under Tobit-Ⅱ type constraints (4), as follows:

[0102]

[0103] in, represents the state vector of the drone in n-dimensional space at time k, x k-1 represents the state vector of the drone at time k-1, n is the state dimension, The superscript T indicates the matrix transpose, x k represents the eastward position of the UAV, η k Indicates the north position of the drone, represents the eastward speed, represents the north velocity; process noise ω k It has a mean of 0 and a variance of Q k Gaussian white noise; represents the potential ideal measurement value of the distance and azimuth of the UAV from the radar measurement point, m represents the dimension of the measurement vector, and the measurement noise υ k The mean is 0 and the standard deviation is r = [r1 r2] T , the variance is R k Gaussian white noise, and is the state transfer matrix under the uncertainty of the UAV model parameters, h k (x k ) is the nonlinear measurement matrix, and its specific expression is:

[0104]

[0105] The radar censored measurement model under Tobit-Ⅱ type censoring constraint is:

[0106]

[0107] Among them, z k represents the actual measured value under deletion; τ represents the deletion threshold. In this embodiment, the measurement dimension m = 2, then τ = [τ1, τ2] T , τ1 represents the distance deletion threshold, and τ2 represents the azimuth deletion threshold.

[0108] S2. Initialization: Assume that the state of the drone at the initial k-1 moment is estimated to be The square root of the estimated error covariance is The convergence factor of the filtering algorithm is γ, and the a posteriori measurement estimation error is e z,k-1|k-1 .

[0109] S3, calculate the next moment, i.e. the drone state prediction at moment k and the square root of the state prediction error covariance

[0110] S31. Calculate the drone state prediction at time k

[0111]

[0112] Where N represents the number of lattice points, represents the j-th state prediction lattice point, which is calculated as follows:

[0113]

[0114] In the formula, Represents the state transfer matrix under uncertain model parameters The estimated value of k-1|k-1,j , j = 0, 1, ..., N-1 represents the initial state lattice point, and the calculation is as follows:

[0115]

[0116] Where Y j represents the cumulative distribution function The inverse mapping is calculated as follows:

[0117]

[0118] in, represents the transformation lattice point, represents the cumulative distribution function, which is calculated as follows:

[0119]

[0120] Among them, △ is the n-dimensional space [0,1) nIndependent uniform distribution within, n is the state dimension; represents the initial lattice points generated by the rank 1 lattice rule, which is defined as follows:

[0121]

[0122] In the formula, g = [a 0 ,a 1 ,…,a n-1 ] T is a vector containing n integers, a is an integer coprime to N; j modN represents the remainder after j is divided by N;

[0123] S32. Calculate the square root of the state prediction error covariance

[0124]

[0125] in, represents the lower triangular decomposition, is a lower triangular matrix, Denotes the process noise covariance matrix Q k The square root of Cholesky decomposition is Represents the central weight matrix of the state prediction error:

[0126]

[0127] In the formula, let Represents the lattice point weight.

[0128] S4. Based on the radar nonlinear measurement model under ideal measurement (2) and the radar censored measurement model under Tobit-Ⅱ type constraint (4), a nonlinear measurement model under censored constraint is constructed:

[0129]

[0130] Among them, I m represents the m-dimensional identity matrix, Represents the missing parameter; for the mth missing measurement parameter like This means that the measurement is not deleted. like This means that the measurement is censored. The specific form is as follows:

[0131]

[0132] S5. Calculate measurement predictions under censoring constraints and the square root of the covariance of the measurement prediction errors

[0133] S51. Calculate measurement predictions under censoring constraints

[0134]

[0135] in, indicates that the cumulative distribution value of the measurement is less than the censoring value, indicates that the cumulative distribution value of the measurement is greater than the censoring value, represents the potential ideal measurement prediction under uncensored measurement, Represents the inverse Mills ratio value; the specific calculation steps are as follows:

[0136] S51.1. Calculate the measurement prediction without deletion

[0137]

[0138] In the formula, is the jth measured predicted lattice point; k|k-1,j is the measurement of the initial lattice point, calculated as follows:

[0139]

[0140] S51.2. Order The inverse Mills ratio The calculation is as follows:

[0141]

[0142] in, is the cumulative distribution of measures less than the censoring threshold; is the probability density of measuring less than the missing threshold, calculated as follows:

[0143]

[0144] S52. Calculate the square root of the covariance of the measurement prediction error

[0145]

[0146] in, represents the lower triangular decomposition, is a lower triangular matrix. k|k-1 The central weight matrix representing the measurement prediction error is calculated as follows:

[0147]

[0148] In the formula, Denotes the measurement noise covariance σ under the censoring constraintk The square root obtained by Cholesky decomposition is σ k The calculation of is as follows:

[0149]

[0150] S6. Calculate the measurement prediction error covariance and the cross-covariance of state and measurement prediction errors

[0151] S61. Calculate the measurement prediction error covariance

[0152]

[0153] S62. Calculate the cross-covariance of state and measurement prediction errors

[0154]

[0155] S7. Calculate the square root lattice smooth variable structure Tobit filter gain K k ;

[0156]

[0157] in, Indicates H k The pseudo-inverse of z,k represents the prior measurement prediction error e z,k|k-1 and the a posteriori measurement estimation error e z,k-1k-1 The sum of the absolute values ​​of represents the multiplication of corresponding elements of the matrix, represents the saturation function, ψ k represents the time-varying smooth boundary layer; the specific calculation steps are as follows:

[0158] S71. Calculate the measurement matrix H k :

[0159]

[0160] in, represents the state prediction error covariance, and

[0161] S72. Calculate the priori measurement prediction error e z,k|k-1 and the a posteriori measurement estimation error e z,k-1|k-1 The sum of the absolute values ​​of :

[0162] e z,k =|e z,k|k-1 |+γ|e z,k-1|k-1| (29)

[0163] in,

[0164] S73. Calculate saturation function

[0165]

[0166] in, represents the inverse of the time-varying smooth boundary layer and is calculated as follows:

[0167]

[0168] S8, according to the filter gain K k Calculate the updated value of the drone state estimate at time k And the square root of the state estimation error covariance update value And the a posteriori measurement estimation error e at time k z,k|k , respectively calculated as follows:

[0169]

[0170] in, represents the lower triangular decomposition, is a lower triangular matrix.

[0171] S9, output state estimation update value State estimation error covariance square root update value and the a posteriori measurement estimation error e z,k|k .

[0172] Simulation examples and results

[0173] In order to verify the effectiveness of the proposed method, a UAV turning motion under uncertain model parameters is taken as an example to simulate and verify the proposed method. It can be expressed as follows:

[0174]

[0175] Among them, △t represents the sampling period; θ k represents the turning rate; E error is the uncertain parameter of the model, assuming that E error It obeys Gaussian white noise with mean 0 and variance 0.001, and E(1+E error )=1, ·* means the parameter is multiplied by the elements of the matrix. And Estimated value of It can be expressed as

[0176] Assume that the sampling period △t = 1s, the convergence factor γ = 0.01, the number of lattice sampling points L = 4, the selected integer a = 3, and the turning rate θ k =10 degrees / second, the initial state is x0=[500m,200m,10m / s,0m / s] T ; The deletion threshold is τ = [538.5, 0] T , square root of state prediction error covariance Process noise variance Q k =diag([10 -5 10 -5 10 -5 10 -5 ]), measurement noise variance R k =diag([0.1 0.01]), the a posteriori measurement estimation error is e z,0|0 =[0.010.01] T .

[0177] The simulation time is set to 200s. From the simulation results, it can be seen that the square root lattice smoothed variable structure Tobit filter (RLSVSTF) proposed in the present invention can provide high-precision UAV trajectory estimation results. Figure 3 is the true value of the radar distance measurement. It can be seen that there is deletion in the measurement process. Figure 4 is the true value of the radar angle, and there is no deletion at this time. Figure 5 It is a comparison between the actual trajectory and the estimated trajectory of the UAV. The results show that the estimated trajectory of the square root lattice smooth variable structure Tobit filter is very close to the actual trajectory, indicating that this method can still maintain a high estimation accuracy when the model parameters are uncertain and the measurement is missing. Figure 6 It is the root mean square error (RMSE) of the position estimation of 50 Monte Carlo simulations. Although the RMSE increases to a certain extent under the condition of missing measurements, it can be quickly reduced when there is no missing measurement, and the overall error is small. The simulation results show that the UAV state estimation method based on square root lattice smooth variable structure Tobit filter (RLSVSTF) proposed in this invention can provide high-precision UAV trajectory estimation results under the conditions of model parameter uncertainty and missing measurements.

[0178] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for estimating the state of an unmanned aerial vehicle based on square root lattice smoothed variable structure Tobit filtering, characterized in that: The steps are as follows: S1. Construct a non-cooperative UAV motion model with unknown model parameters, a radar nonlinear measurement model under ideal measurement, and a radar censored measurement model under Tobit-Ⅱ type constraints; S2. Initialization: Assume that the state of the drone at the initial k-1 moment is estimated to be The square root of the estimated error covariance is The convergence factor of the filtering algorithm is γ, and the a posteriori measurement estimation error is e z,k-1|k-1 ; S3, calculate the next moment, i.e. the drone state prediction at moment k and the square root of the state prediction error covariance S4. Based on the radar nonlinear measurement model and the censored measurement model, a nonlinear measurement model under censored constraints is constructed: S5. Calculate measurement predictions under censoring constraints and the square root of the covariance of the measurement prediction errors S6. Calculate the measurement prediction error covariance State and measurement prediction error cross-covariance S7. Calculate the square root lattice smooth variable structure Tobit filter gain K k ; S8, according to the filter gain K k Calculate the updated value of the drone state estimate at time k and the square root of the state estimation error covariance update value And the a posteriori measurement estimation error e at time k z,k|k ; S9, output state estimation update value State estimation error covariance square root update value and the a posteriori measurement estimation error e z,k|k .

2. The method for estimating the state of an unmanned aerial vehicle based on square root lattice smooth variable structure Tobit filtering according to claim 1 is characterized in that: The non-cooperative UAV motion model is: The radar nonlinear measurement model under ideal measurement is: in, represents the state vector of the drone in n-dimensional space at time k, x k-1 represents the state vector of the drone at time k-1, n is the state dimension, The superscript T indicates the matrix transpose, x k represents the eastward position of the UAV, η k Indicates the north position of the drone, represents the eastward speed, represents the north velocity; process noise ω k It has a mean of 0 and a variance of Q k Gaussian white noise; represents the potential ideal measurement value of the distance and azimuth of the UAV from the radar measurement point, m represents the dimension of the measurement vector, and the measurement noise υ k The mean is 0 and the standard deviation is r = [r1 r2] T , the variance is R k Gaussian white noise, and is the state transfer matrix under the uncertainty of the UAV model parameters, h k (x k ) is the nonlinear measurement matrix, and its specific expression is: The radar censored measurement model under Tobit-Ⅱ type censoring constraint is: Among them, z k represents the actual measured value under deletion; τ represents the deletion threshold; in this embodiment, the measurement dimension m = 2, then τ = [τ1, τ2] T , τ1 represents the distance deletion threshold, and τ2 represents the azimuth deletion threshold.

3. The method for estimating the state of an unmanned aerial vehicle based on square root lattice smooth variable structure Tobit filtering according to claim 1 is characterized in that: In step S3, the drone state prediction at time k is calculated and the square root of the state prediction error covariance The calculation method is: S31. Calculate the drone state prediction at time k Where N represents the number of lattice points, represents the j-th state prediction lattice point, which is calculated as follows: In the formula, Represents the state transfer matrix under uncertain model parameters An estimated value of represents the initial state lattice point, which is calculated as follows: Where Y j represents the inverse mapping of the cumulative distribution function θ(x), which is calculated as follows: in, represents the conversion lattice point, θ(x) represents the cumulative distribution function, and the specific calculation is as follows: Among them, △ is the n-dimensional space [0,1) n Independent uniform distribution within, n is the state dimension; represents the initial lattice points generated by the rank 1 lattice rule, which is defined as follows: In the formula, g = [a 0 ,a 1 ,…,a n-1 ] T is a vector containing n integers, a is an integer coprime to N; j modN represents the remainder after j is divided by N; S32. Calculate the square root of the state prediction error covariance in, represents the lower triangular decomposition, is a lower triangular matrix, Denotes the process noise covariance matrix Q k The square root of Cholesky decomposition is Represents the central weight matrix of the state prediction error: In the formula, let Represents the lattice point weight.

4. The method for estimating the state of an unmanned aerial vehicle based on square root lattice smooth variable structure Tobit filtering according to claim 1 is characterized in that: In step S4, based on the radar nonlinear measurement model under ideal measurement and the radar censored measurement model under Tobit-Ⅱ type constraint, a nonlinear measurement model under censored constraint is constructed: Among them, I m represents the m-dimensional identity matrix, Represents the missing parameter; for the mth missing measurement parameter like This means that the measurement is not deleted. like This means that the measurement is censored. The specific form is as follows:

5. The method for estimating the state of an unmanned aerial vehicle based on square root lattice smooth variable structure Tobit filtering according to claim 3 is characterized in that: In step S5, the measurement prediction under the censoring constraint is calculated and the square root of the covariance of the measurement prediction errors The specific calculation method is: S51. Calculate measurement predictions under censored measurements in, indicates that the cumulative distribution value for which the measurement is less than the censoring value, indicates that the cumulative distribution value of the measurement is greater than the censoring value, represents the potential ideal measurement prediction under uncensored measurement, Represents the inverse Mills ratio value; the specific calculation steps are as follows: S51.

1. Calculate the measurement prediction without deletion In the formula, is the jth measured predicted lattice point; k|k-1,j is the measurement of the initial lattice point, calculated as follows: S51.

2. Order The inverse Mills ratio The calculation is as follows: in, is the cumulative distribution of measures less than the censoring threshold; is the probability density of measuring less than the missing threshold, calculated as follows: S52. Calculate the square root of the covariance of the measurement prediction error in, represents the lower triangular decomposition, is a lower triangular matrix; Z k|k-1 The central weight matrix representing the measurement prediction error is calculated as follows: In the formula, Denotes the measurement noise covariance σ under the censoring constraint k The square root obtained by Cholesky decomposition is σ k The calculation of is as follows:

6. The method for estimating the state of an unmanned aerial vehicle based on square root lattice smooth variable structure Tobit filtering according to claim 5 is characterized in that: In step S6, the measurement prediction error covariance is calculated and the cross-covariance of state and measurement prediction errors The specific calculation method is: S61. Calculate the measurement prediction error covariance S62. Calculate the cross-covariance of state and measurement prediction errors 7. The method for estimating the state of an unmanned aerial vehicle based on square root lattice smooth variable structure Tobit filtering according to claim 6 is characterized in that: The square root lattice smooth variable structure Tobit filter gain K k The expression is: in, Indicates H k The pseudo-inverse of z,k represents the prior measurement prediction error e z,k|k-1 and the a posteriori measurement estimation error e z,k-1k-1 The sum of the absolute values ​​of represents the multiplication of corresponding elements of the matrix, represents the saturation function, ψ k represents the time-varying smooth boundary layer; the specific calculation steps are as follows: S71. Calculate the measurement matrix H k : in, represents the state prediction error covariance, and S72. Calculate the priori measurement prediction error e z,k|k-1 and the a posteriori measurement estimation error e z,k-1|k-1 The sum of the absolute values ​​of : And z,k =|and z,k|k-1 |+γ|and z,k-1|k-1 | (29) in, S73. Calculate saturation function in, represents the inverse of the time-varying smooth boundary layer and is calculated as follows:

8. The method for estimating the state of an unmanned aerial vehicle based on square root lattice smooth variable structure Tobit filtering according to claim 7 is characterized in that: According to the filter gain K k Calculate the updated value of the drone state estimate at time k and the square root of the state estimation error covariance update value And the a posteriori measurement estimation error e at time k z,k|k , respectively calculated as follows: in, represents the lower triangular decomposition, is a lower triangular matrix.

Images