IGS-SRCPF electrically-driven unmanned aerial vehicle pose estimation method and system based on model parameter ratio

Abstract

The invention discloses an IGS-SRCPF electrically-driven unmanned aerial vehicle pose estimation method and system based on a model parameter ratio, and relates to the technical field of unmanned aerial vehicle pose estimation, and the method comprises the following steps: obtaining particles, carrying out the non-Gaussian decomposition of the initial state of the particles based on a Gaussian sum particle filtering algorithm, and carrying out the fitting to generate a nonlinear particle Gaussian subitem; linearizing the nonlinear particle Gaussian subitems based on a pre-constructed pseudo matrix of an SRCKF sub-filter to obtain linear particle Gaussian subitems, and generating a nonlinear non-Gaussian system model MPR based on the linear particle Gaussian subitems; and solving the nonlinear non-Gaussian system model MPR based on the MPR existence theorem to obtain a model adjustment coefficient, and estimating the pose of the IGS-SRCPF electrically driven unmanned aerial vehicle through a preset multi-target HPO-DOA algorithm in combination with the model adjustment coefficient to obtain an estimation result.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle posture estimation, and in particular to an IGS-SRCPF electric-driven unmanned aerial vehicle posture estimation method and system based on a model parameter ratio. Background Art

[0002] Many commonly used filtering algorithms have been developed for nonlinear non-Gaussian systems in various practical applications (including attitude estimation of electric-driven UAVs, motion state estimation of electric-driven unmanned ships, power grid state estimation and fault detection, etc.), such as extended Kalman filter (Extended Kalman Fiulter, EKF), unscented Kalman filter (Unscented Kalman Fiulter, UKF) and strong tracking cubature Kalman filter (cubature Kalman filter, CKF), etc. However, these algorithms are still difficult to fully and accurately describe nonlinear non-Gaussian systems, and there is a significant deviation between them and the real model. Although methods such as predictive-fifth-order CKF and improved Gaussian sumcubature Kalman filter (Gaussian sumcubature Kalman filter, GS-CKF) are used to try to improve the estimation accuracy, these methods still face the problem of inaccurate filter parameter estimation when dealing with nonlinear non-Gaussian systems, which affects the accuracy of filter estimation. Therefore, finding high-performance filter conditions suitable for such systems is still a key challenge in the current research field. Summary of the invention

[0003] In order to solve the deficiencies mentioned in the above background technology, the purpose of the present invention is to provide an IGS-SRCPF electric-driven UAV pose estimation method and system based on model parameter ratio.

[0004] In the first aspect, the purpose of the present invention can be achieved by the following technical solution: an IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio, the method comprising the following steps:

[0005] Obtain particles, perform non-Gaussian decomposition on the initial state of the particles based on the Gaussian and particle filter algorithms, and fit and generate nonlinear particle Gaussian sub-items;

[0006] Based on the pseudo-matrix of the pre-constructed SRCKF sub-filter, the nonlinear particle Gaussian sub-item is linearized to obtain the linear particle Gaussian sub-item, and the nonlinear non-Gaussian system model MPR is generated based on the linear particle Gaussian sub-item;

[0007] Based on the MPR existence theorem, the nonlinear non-Gaussian system model MPR is solved to obtain the model adjustment coefficient. The position and posture of the IGS-SRCPF electric-driven UAV is estimated by the preset multi-objective HPO-DOA algorithm combined with the model adjustment coefficient to obtain the estimation result.

[0008] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the nonlinear non-Gaussian system model is as follows:

[0009] x k+1 =f k (x k )+w k+1,k , (1)

[0010] z k =h k (x k )+v k , (2)

[0011] Among them, k represents the time, x k ∈R n is the system state vector, f k :R n →R n 、h k :R n →R p Respectively represent the system state matrix and measurement matrix, w k+1,k ∈R n represents non-Gaussian process noise, z k ∈R p is the observation vector, v k ∈R p represents non-Gaussian observation noise, and the state noise and measurement noise are independent of each other.

[0012] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the Gaussian and particle filter algorithm process includes the following steps:

[0013] Initialization: Set the initial time point k = 0 and define the initial state p(x0|z -1 )=p(x0), and as N k|k-1 The sum of Gaussian subterms;

[0014] Using the EM algorithm, the initial probability density function p(x0), the process noise probability density function p(ω k ) and the measurement noise probability density function p(v k ) are expressed in the form of Gaussian sum as follows:

[0015]

[0016]

[0017] in, and is the weight of its corresponding Gaussian sub-item, and there is

[0018] Distributed filtering: The Gaussian sum form of equation (6) is used to approximate the filtering probability density function:

[0019]

[0020] Among them, measurement And: N k|k =N k|k-1 ·r k . In addition, based on the predicted state mean Prediction covariance matrix and the mean value of the measurement noise and the measurement variance matrix The SRCKF filtering algorithm is used to calculate the i-th Gaussian sub-term The filtered mean and the covariance matrix

[0021] At this time, the measurement likelihood function of the state estimation of the i-th Gaussian sub-item is expressed as:

[0022]

[0023] The calculation method of indicators j and l is:

[0024]

[0025] Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates the rounding down operation, filter weight Comprehensive prediction filter weights are required The weight of the probability density function of the measurement noise And the likelihood function of the state estimate Therefore The adaptive calculation method is:

[0026]

[0027] Global point estimation: After each Gaussian sub-item obtains the filtering state at time k, the global filtering mean at time k can be calculated using equations (10) and (11): and the covariance matrix P k|k ;

[0028]

[0029] Similarly, the state estimates at other time points are calculated by equations (10) to (11);

[0030] Reduction control: After calculating the estimated mean and covariance of each sub-item at time k, first retain the Gaussian component with a weight greater than G' and delete the other components;

[0031]

[0032] Among them, G' is the weight of the Gth Gaussian component after the weights are sorted from large to small;

[0033] Among the retained Gaussian components, find the component with the largest weight:

[0034]

[0035] And find the Gaussian components that meet the conditions according to the following criteria:

[0036]

[0037] in,

[0038] The Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are:

[0039]

[0040] in,

[0041] Delete all Gaussian components that have been fused, and then repeat the above steps until all Gaussian components have completed the operation, retain the first S sub-Gaussian distributions with large weights, and re-normalize the corresponding weights;

[0042] Prediction update: The predicted state probability density function can be expressed as follows:

[0043]

[0044] in,

[0045]

[0046] It can be seen that the prediction weight The calculation combines two pieces of information, namely the filter weights and the weight of the process noise probability density function In addition, based on the estimated state mean Estimated variance matrix Process noise mean and the process noise covariance matrix Calculated using SRCKF The predicted mean and the covariance matrix c and d are:

[0047]

[0048] Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k .

[0049] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the multi-objective HPO-DOA algorithm uses a combination of the hunter-prey optimization algorithm HPO and the wild dog optimization algorithm DOA to achieve simultaneous estimation of multiple covariances, thereby obtaining the required The improvement methods are as follows:

[0050] By applying the hunter-prey distinction strategy in the HPO algorithm, the prey of the DOA algorithm is transformed from the optimal value of the previous iteration to the individual acting as the prey found by the hunter-prey algorithm; the hunter chases the prey and the prey flees to a safe point to obtain better global optimization capabilities;

[0051] The diversified predation strategies of the DOA algorithm replace the single predation method in the original HPO algorithm;

[0052] set up The orders are q and r respectively. In a q k ×q+r k In ×r-dimensional space, a population X consisting of n individuals is randomly generated. n ), the i-th individual corresponds to a vector Indicates that in q k ×q+r k ×r-dimensional space, calculate the fitness value corresponding to each individual according to the fitness function;

[0053] Generation of initial population: The position of each member in the initial population is randomly generated in the search space according to formula (20):

[0054] x i =rand(d,1)×(ub-lb)+lb, (20)

[0055] Among them, x iis the position of the hunter or prey, lb is the minimum value of the problem variable, ub is the maximum value of the problem variable, and d is the number of problem variables;

[0056] The fitness function for calculating the fitness value of each individual is:

[0057] Distinguishing hunters from prey: According to the hunter-prey optimization algorithm, the criterion for distinguishing whether an individual is a hunter or a prey is that the individual with the largest average distance position is regarded as the prey P pos :

[0058]

[0059] The position average is The distance between each individual and the average position is D i =|x i -μ|, otherwise it will be regarded as a hunter;

[0060] Update of prey position: According to the hunter prey optimization algorithm, the prey position is updated using the following formula (22):

[0061] x i (t+1)=x * (t)+CZcos(2πR4)(x * (t)-x i (t)), (22)

[0062] Among them, x i (t) is the current position of the prey; x i (t+1) is the next iteration position of the prey; x * (t) is the global optimal position; Z is the adaptive parameter calculated by equation (23); R4 is a random number in the range [-1,1]; C is the balance parameter between exploration and exploitation, whose value decreases during the iteration of the algorithm and is calculated by equation (24):

[0063] P=R1<C; IDX=(P==0);

[0064]

[0065] R1 and R3 are random vectors in [0,1], P is the index value of R1 < C, R2 is a random number in [0,1], IDX is the index value of vector R1 that satisfies the condition (P == 0), and C is the balance parameter between exploration and exploitation, calculated as follows:

[0066] C=1-it(0.98 / MaxIt), (24)

[0067] Among them, it is the current iteration number, MaxIt is the maximum iteration number;

[0068] Updates on Wild Dog Locations:

[0069] Strategy 1: Wild dogs have the ability to find prey and cleverly surround it. Their behavior is expressed in the following formula (25):

[0070]

[0071] Among them, x i (t+1) is the new position of the individual, indicating the movement of the wild dog; na is a random integer generated in the reverse order of [2, SizePop / 2], where SizePop is the size of the population, are all the wild dogs that will attack, among which X randomly generated wild dog population, x i (t) is the current position of the individual; β1 is a random number uniformly generated within [-2,2], which is the proportional factor;

[0072] Strategy 2: Persecution

[0073] Wild dogs hunt small prey until they catch it alone, and the following equation models this behavior:

[0074]

[0075] Among them, the value of β1 is the same as that of formula (25), β2 is a random number uniformly generated in the interval [-1,1], and r1 is a random number generated in the interval [1,SizePop]. is the r1th individual selected randomly, where i≠r1;

[0076] Strategy 3: Scavenging

[0077] This behavior is simulated using the following equation (27):

[0078]

[0079] Among them, β2 is the same as the value in formula (26), and the selection method of σ is: generate a random number in [0,1], and return 0 if it is less than or equal to 0.5, otherwise return 1;

[0080] Strategy 4: Wild Dog Survival Rate

[0081] In the DOA algorithm, the survival rate of wild dogs is given by:

[0082]

[0083] Among them, fitness max and fitness minare the worst and best fitness values ​​in the current generation, respectively, and fitness(i) is the current fitness value of the i-th individual. Equation (29) is applied to low survival rates through the algorithm, and the survival rate value is equal to or less than 0.3:

[0084]

[0085] Among them, x i (t) is the individual with a lower survival rate to be updated, r1 and r2 are random numbers generated in the interval [1, SizePop], r1≠r2, and are the r1th and r2th individuals selected randomly.

[0086] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the process of the HPO-DOA algorithm is as follows:

[0087] In the defined interval [x min ,x max ] Randomly generate population individuals and put the estimated individuals into the randomly generated initial population;

[0088] Constructing the fitness function Substitute the population individuals as the noise covariance corresponding to each Gaussian sub-item into the filtering algorithm to obtain the corresponding And calculate the fitness value;

[0089] Find the global optimal position according to each fitness value;

[0090] distinguishing between hunters and prey among individuals in a population;

[0091] Update the individuals in the population according to equations (21) to (29);

[0092] Calculate the fitness value of the updated individual and find the global optimal position;

[0093] Determine whether the number of iterations has reached the upper limit. If yes, output the corresponding optimal individual as the covariance estimate of each Gaussian sub-item Thus, an accurate non-Gaussian noise estimate is obtained; otherwise, hunters and prey are re-distinguished among individuals in the population;

[0094] according to Calculate t(k).

[0095] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the pseudo matrix of the pre-constructed SRCKF sub-filter is constructed based on the particle Gaussian sub-item of the improved GS-SRCPF, and the process includes: square root volume Kalman filtering, improved Gaussian and square root volume particle filtering, and constructing a pseudo matrix;

[0096] Among them, the process of improving Gaussian and square root volumetric particle filtering is as follows:

[0097] Using Gaussian and particle filter algorithms, the advantages of the two algorithms are combined to improve the state estimation accuracy of the filter:

[0098]

[0099] IGS-SRCKF is used as the importance function of PF. On the basis of this, non-Gaussian decomposition is performed on the initial state of a single particle and a simplified fusion strategy is added. Formula (30) is modified as follows:

[0100]

[0101] The specific steps of the IGS-SRCPF algorithm are as follows:

[0102] Initialization: Let x0 be the initial state and generate H particles from the prior distribution p(x0) Recalculate the initial state and the estimated error covariance P0;

[0103]

[0104] The process noise probability density function p(ω k,k-1 ) and the measurement noise probability density function p(ν k ) are expressed in the form of Gaussian sum, as shown in Equation (4) and Equation (5), where the unsupervised robust EM algorithm process is as follows:

[0105] Assume there is a set of measurement noise data V k =(ν1,ν2,…,ν n ), where v k is a p-dimensional vector generated by a Gaussian mixture model:

[0106]

[0107] In the formula, are the weight, mean and covariance of the i-th Gaussian term in the Gaussian mixture model, r k Indicates the number of components of the mixed model;

[0108] Initialization parameters: Let r k =n, β = 1; Take an arbitrary value, that is, the value of θ, and set ε>0 as the condition for judging the end of the loop, where β is:

[0109]

[0110] E-step: Calculate the implicit parameter z ki :

[0111]

[0112] M-step: Update weights by equations (37), (38) and (35) respectively. and β, let β = 1 for the first iteration;

[0113]

[0114] when When , the i-th Gaussian component is invalid and the mixing coefficient at this time is removed At the same time, the Gaussian component parameters When the number of iterations is greater than 60, let β = 0;

[0115] Updated by equation (39) and equation (40) and z ki' , and updated by equations (38) and (41) and

[0116]

[0117] Update z according to formula (36) ki ;

[0118] like Then the iteration stops, otherwise go to M-step to continue execution;

[0119] Improvement process: First, delete the weight less than The Gaussian component of , the Gaussian component fusion strategy is performed on the interval of formula (42),

[0120]

[0121] Among the Gaussian components of the interval, find the one with the largest weight,

[0122]

[0123] And find the Gaussian components that meet the conditions according to the following criteria:

[0124]

[0125] in,

[0126] The Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are:

[0127]

[0128] in,

[0129] Importance Sampling:

[0130] Gaussian Fitting

[0131] The improved unsupervised robust EM algorithm is used to transform the initial probability density function of H particles into It can be expressed in the form of Gaussian sum as follows:

[0132]

[0133] in, is the weight of the corresponding Gaussian sub-item, and:

[0134]

[0135] Distributed filtering

[0136] For H particles, the filtering probability density function is approximated using the Gaussian sum form as in equation (49):

[0137]

[0138] Among them, the measurement And: N k|k =N k|k-1 ·r k In addition, based on the predicted state mean Prediction covariance matrix Measurement noise mean and the measurement variance matrix The SRCKF filtering algorithm is used to calculate the i-th Gaussian subterm of the h-th particle. The filtered mean and the covariance matrix

[0139] At this time, the measurement likelihood function of the state estimation of the i-th Gaussian sub-term of the h-th particle can be expressed as:

[0140]

[0141] The calculation method of indicators j and l is:

[0142]

[0143] Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates rounding down operation; filter weight Comprehensive prediction filter weights are required The weight of the probability density function of the measurement noise And the likelihood function of the state estimate Therefore The adaptive calculation method is:

[0144]

[0145] Global point estimation: After the Gaussian sub-items of each particle obtain the filtering state at time k, the global filtering mean at time k can be calculated using equations (53) and (54): and the covariance matrix

[0146]

[0147] Similarly, the state estimates at other time points are calculated by equations (53) to (54);

[0148] Simplified control:

[0149]

[0150] Among them, G' is the weight of the Gth Gaussian component after the weights are sorted from large to small;

[0151] Among the remaining Gaussian components, find the one with the largest weight:

[0152]

[0153] And find the Gaussian components that meet the conditions according to the following criteria:

[0154]

[0155] in,

[0156] The Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are:

[0157]

[0158] in,

[0159] Delete all Gaussian components that have been fused, and then repeat the steps until all Gaussian components have completed the operation, retaining only the first S sub-Gaussian distributions with large weights, and re-normalize the corresponding weights;

[0160] Forecast update, the predicted state probability density function can be expressed as follows:

[0161]

[0162] in,

[0163]

[0164] It can be seen that the prediction weight Calculation of comprehensive filter weights and the weight of the process noise probability density function In addition, based on the estimated state mean Estimated variance matrix Process noise mean and the process noise covariance matrix Calculated using SRCKF The predicted mean and the covariance matrix In addition, c and d are:

[0165]

[0166] Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k ;

[0167] Particle Update:

[0168] Based on the IGS-SRCKF importance function and Generate new particles

[0169]

[0170] Calculate particle weights and normalize:

[0171] Repeat step 2-step 3 for the other H-1 particles to obtain the state estimates of all particles, calculate the particle weights and normalize them:

[0172]

[0173] In the formula, the particle With weight One to one correspondence;

[0174] Random resampling:

[0175] The roulette algorithm is used to copy particles. A random number that is uniformly distributed in [0,1] is generated in advance. For particles with large weights, the probability that the random number falls into the interval corresponding to the particle weight is higher, and the particles are more likely to be copied. Particles with small weights are more likely to be abandoned, and the above particle copying process is repeated;

[0176] Calculate the state estimator and the estimation error covariance matrix;

[0177] According to the resampled particles, the final target posterior state estimator and the estimation error covariance matrix at time k are calculated:

[0178]

[0179] At this point, the state filtering process at time k has been completed.

[0180] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the pseudo-matrix construction process:

[0181] The filter is split into H×N by IGS-SRCPF filtering algorithm k|k For a sub-filter, the pseudo-matrix is ​​constructed by CKF filtering to obtain the pseudo-observation matrix of the SRCPF sub-filter:

[0182]

[0183] The pseudo-state transfer matrix is:

[0184]

[0185] in,

[0186]

[0187] Thus, the iterative process of the sub-filter is simplified to:

[0188]

[0189] Where i, j, l, c, and d are calculated as follows:

[0190]

[0191] Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates floor operation;

[0192]

[0193] Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k .

[0194] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the nonlinear non-Gaussian system model MPR parameter ratio:

[0195] The particle Gaussian subterms do not match the noise covariance

[0196] For each particle Gaussian sub-term, we have:

[0197]

[0198]

[0199] In the formula, represents the true observed noise covariance of the ith Gaussian subterm of each particle, represents the true process noise covariance of the i-th Gaussian subterm of each particle, represents the observation noise covariance of the ith Gaussian subterm assumption for each particle in practical applications, represents the process noise covariance of the ith Gaussian subterm assumption for each particle in practical applications, and both are denoted by superscript u. and represents the deviation of the particle Gaussian subterm;

[0200] It is known that there is a Kalman filter with mismatched noise covariance in the Gaussian subterms of each particle:

[0201]

[0202] Wherein, the superscript f represents the actual calculated value of the sub-filter, It is not the Gaussian sub-item filter estimate of the particle at time k TMSE, because:

[0203]

[0204] Available The TMSE is:

[0205]

[0206] in,

[0207]

[0208] Among them, the superscript m represents the true value of the filter. In addition, the indicators e and g are calculated as follows:

[0209]

[0210] Improved model parameter ratio of Gaussian and SRCPF filters

[0211] The MPR theoretical method of linear Gaussian, for the linear Gaussian subsystem, assume:

[0212]

[0213] in, is the process noise parameter ratio of the i-th Gaussian sub-item of each particle, is the process noise parameter ratio of the i-th Gaussian sub-item of each particle, is the model parameter ratio of the i-th Gaussian subsystem of each particle;

[0214] Using the IGS-SRCPF filter framework, under this framework, there are H×N k|k linear particle Gaussian subterms, for the noise covariance Gaussian subterms, there exists q k The process noise covariance Gaussian sub-terms are: Existence k The observation noise covariance Gaussian sub-terms are: Therefore, there is a total of q k × k The combinations are:

[0215]

[0216] Therefore, in the Gaussian and SRCPF filter framework,

[0217]

[0218] is the process noise parameter ratio,

[0219]

[0220] is the observation noise parameter ratio,

[0221]

[0222] is the MPR of the system. If the MPR of another model is:

[0223]

[0224] The two models are said to conform to the same model parameter ratio.

[0225] In combination with the first aspect, in some implementations of the first aspect, the method further includes: estimating the model adjustment coefficient:

[0226] Innovation of known sub-filters for:

[0227]

[0228] The covariance matrix of is:

[0229]

[0230] The superscript f indicates a calculated value, but Not really The true value of the covariance matrix is:

[0231]

[0232] The superscript m indicates the true value;

[0233] In the IGS-SRCPF filter framework, if the estimated value of the process noise covariance subterm The process noise parameter ratio shown in equation (95) satisfies the estimated value of the observation noise covariance sub-term The observation noise parameter ratio shown in formula (96) is satisfied, and the The minimum value of , then the system model parameter ratio shown in equation (97) must be satisfied. When the parameter ratios of each noise covariance sub-item are known, by finding The estimated value of the noise covariance sub-term obtained by the minimum value of b h,i is a scalar greater than 0. When , the noise covariance deviation of the sub-filter is:

[0234]

[0235] According to (83) and (89), we have:

[0236]

[0237] Then, according to the simplified filter parameter linear multiple theorem, in the linear Gaussian subsystem, for the steady-state system, if Established, when and Multiply by the same scalar K, when the system reaches a steady state, Substituting into the above formula (104), we can obtain:

[0238]

[0239] From the above formula (105), we can know Since in the estimation process, is not accurate, so and Stretch them into vector X h,i , Y h,i , use the least squares method to estimate b h,i :

[0240] b h,i =((X h,i ) T X h,i ) -1 (X h,i ) T Y h,i . (106)

[0241] b h,i Substituting into equations (102) and (103), we can obtain:

[0242]

[0243] In summary, the parameter estimation criteria under the improved Gaussian and SRCPF framework are obtained:

[0244] For each Gaussian subterm, The necessary and sufficient conditions are:

[0245]

[0246] st satisfies the ratio of process and observation noise parameters.(109).

[0247] In the second aspect, in order to achieve the above-mentioned purpose, the present invention discloses an IGS-SRCPF electric-driven UAV pose estimation system based on model parameter ratio, comprising:

[0248] The state decomposition module is used to obtain particles, perform non-Gaussian decomposition on the initial state of particles based on Gaussian and particle filter algorithms, and fit and generate nonlinear particle Gaussian sub-items;

[0249] A linearization module is used to linearize the nonlinear particle Gaussian sub-items based on the pseudo-matrix of the pre-constructed SRCKF sub-filter to obtain linear particle Gaussian sub-items, and generate a nonlinear non-Gaussian system model MPR based on the linear particle Gaussian sub-items;

[0250] The pose estimation module is used to solve the nonlinear non-Gaussian system model MPR based on the MPR existence theorem to obtain the model adjustment coefficient. The pose of the IGS-SRCPF electric-driven UAV is estimated through the preset multi-objective HPO-DOA algorithm combined with the model adjustment coefficient to obtain the estimation result.

[0251] Beneficial effects of the present invention:

[0252] The present invention is a posture estimation system for landing operation of an electric-driven unmanned aerial vehicle of a dynamic platform in complex waters under the influence of strong wind conditions. By strengthening the constraints of model features, the credibility filtering method is advanced to the model parameter ratio (MPR) theory, and an improved Gaussian sum square root cubature Kalman filter (IGS-SRCKF) posture estimation technology based on MPR is proposed. First, the particle filter algorithm and the Gaussian sum algorithm are combined to generate a Gaussian sum particle filter algorithm, and the Gaussian component fusion strategy idea in the literature is used to improve the unsupervised EM (Expectation-Maximum) algorithm and the Gaussian sum algorithm. At the same time, the initial state of a single particle is non-Gaussian decomposition is performed, and multiple nonlinear particle Gaussian sub-items are fitted to generate, thereby simplifying the MPR study of particle Gaussian sub-items. Secondly, the square root cubature Kalman filter (Square Root Cubature Kalman Filter, SRCKF) is introduced as a sub-filter of the particle Gaussian sum. Then, the pseudo matrix of the SRCKF sub-filter is constructed and approximately linearized, thereby further simplifying the problem to the MPR study of the linear Gaussian sub-item. Next, referring to the definition of linear Gaussian MPR, the MPR of the nonlinear non-Gaussian system is defined in the improved Gaussian sum squareroot cubature particle filter (IGS-SRCPF) framework. Combined with the MPR existence theorem, the model adjustment coefficient and the conditions for better design of high-performance filters in nonlinear non-Gaussian systems are obtained. Finally, a new IGS-SRCPF electric UAV pose estimation method based on model parameter ratio is proposed by combining the HPO algorithm and DOA (HPO-DOA) algorithm with the new multi-objective to combine the model adjustment coefficient and the conditions for designing high-performance filters. This method significantly enhances the accuracy of pose estimation, thereby ensuring that the flight control system of the electric UAV can more accurately and in real time adjust the control parameters such as motor speed when it is disturbed by strong winds, ensuring the stability and safety of the UAV flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0253] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative work.

[0254] Figure 1 It is a schematic flow chart of the method of the present invention;

[0255] Figure 2 It is a schematic diagram of the HPO-DOA algorithm flow of the present invention;

[0256] Figure 3 It is a framework diagram of the research idea of ​​the present invention;

[0257] Figure 4 It is a schematic diagram of the SRCKF filtering process of the present invention;

[0258] Figure 5 It is a schematic diagram of the iterative process of the unsupervised robust EM algorithm of the present invention;

[0259] Figure 6 It is a schematic diagram of the IGS-SRCKF algorithm flow of the present invention;

[0260] Figure 7 It is a flow chart of IGS-SRCPF algorithm of the present invention;

[0261] Figure 8 It is a schematic diagram of the system structure of the present invention;

[0262] Fig. 9 is a diagram of the estimation error of the process noise covariance component of the present invention. DETAILED DESCRIPTION

[0263] 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.

[0264] Embodiment 1:

[0265] like Figure 1 As shown, the IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio includes the following steps:

[0266] S101: Obtain particles, perform non-Gaussian decomposition on the initial state of the particles based on Gaussian and particle filter algorithms, and fit and generate nonlinear particle Gaussian sub-items;

[0267] S102: linearizing the nonlinear particle Gaussian sub-items based on the pseudo-matrix of the pre-constructed SRCKF sub-filter to obtain linear particle Gaussian sub-items, and generating a nonlinear non-Gaussian system model MPR based on the linear particle Gaussian sub-items;

[0268] S103: Based on the MPR existence theorem, the nonlinear non-Gaussian system model MPR is solved to obtain the model adjustment coefficient, and the position and posture of the IGS-SRCPF electric-driven UAV is estimated by the preset multi-objective HPO-DOA algorithm and the model adjustment coefficient to obtain the estimation result.

[0269] Specifically, the present invention is further described below by way of embodiments:

[0270] 1.1 System Description

[0271] The model of the discrete nonlinear non-Gaussian system is as follows:

[0272] x k+1 =f k (x k )+w k+1,k , (1)

[0273] z k =h k (x k )+v k , (2)

[0274] Among them, k represents the time, x k ∈R n is the system state vector, f k :R n →R n 、h k :R n →R p Respectively represent the system state matrix and measurement matrix, w k+1,k ∈R n represents non-Gaussian process noise, z k ∈R p is the observation vector, v k ∈R p represents non-Gaussian observation noise, and the state noise and measurement noise are independent of each other.

[0275] 1.2 Improved Gaussian Sum Filter

[0276] By using the Gaussian sum filtering algorithm, the complex non-Gaussian credibility theory research can be simplified to the credibility theory research of multiple Gaussian sub-items. In the Gaussian sum filtering algorithm, the state estimation process not only needs to solve the state mean and variance, but also needs to calculate the weights and other parameters corresponding to the Gaussian sub-items. The following is a detailed description of the algorithm flow of the Gaussian sum cubature Kalman filter in 5 steps, in which the Gaussian component fusion strategy of the robust Gaussian sum ensemble Kalman filter algorithm is added in step 4 to optimize and improve the GSSRCKF.

[0277] Step 1 Initialization. Set the initial time point k = 0 and define the initial state p(x0|z -1 )=p(x0), and use it as N k|k-1 The sum of Gaussian subterms.

[0278] Using the EM algorithm, the initial probability density function p(x0), the process noise probability density function p(ω k ) and the measurement noise probability density function p(v k ) are expressed in the form of Gaussian sum as follows:

[0279]

[0280] in, and is the weight of its corresponding Gaussian sub-item, and there is

[0281] Step 2: Distributed filtering. The Gaussian sum form of equation (6) is used to approximate the filtering probability density function:

[0282]

[0283] Among them, measurement And: N k|k =N k|k-1 ·r k .. In addition, based on the predicted state mean Prediction covariance matrix and the mean value of the measurement noise and the measurement variance matrix The SRCKF filtering algorithm is used to calculate the i-th Gaussian sub-term The filtered mean and the covariance matrix

[0284] At this time, the measurement likelihood function of the state estimation of the i-th Gaussian sub-item can be expressed as:

[0285]

[0286] The calculation method of indicators j and l is:

[0287]

[0288] Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates the rounding down operation. Filter weight It is necessary to integrate three pieces of information, namely the prediction filter weights The weight of the probability density function of the measurement noise And the likelihood function of the state estimate Therefore The adaptive calculation method is:

[0289]

[0290] Step 3: Global point estimation. After each Gaussian sub-item obtains the filtering state at time k, the global filtering mean at time k can be calculated using equations (10) and (11): and the covariance matrix P k|k .

[0291]

[0292] Similarly, the state estimates at other time points can also be calculated using equations (10) to (11).

[0293] Step 4: Simplify control. Usually, the process and measurement noise distributed in the form of Gaussian sum will accumulate over time and lead to the continuous increase of the number of Gaussian sub-terms, such as N in equation (6). k|k and N given later k+1|k This is bound to significantly increase the computational overhead and reduce the practicality of filtering. Therefore, in order to improve the real-time performance of the algorithm and ensure the filtering accuracy, after calculating the estimated mean and covariance of each sub-item at time k, the Gaussian components with weights greater than G' are first retained and other components are deleted.

[0294]

[0295] Among them, G' is the weight of the Gth Gaussian component after the weights are sorted from large to small.

[0296] Among the retained Gaussian components, find the component with the largest weight:

[0297]

[0298] And find the Gaussian components that meet the conditions according to the following criteria:

[0299]

[0300] in,

[0301] These Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are:

[0302]

[0303]

[0304] in,

[0305] Delete all Gaussian components that have been fused, and then repeat the above steps until all Gaussian components have completed the operation. Finally, only the first S sub-Gaussian distributions with relatively large weights are retained, and the corresponding weights are normalized again to reduce the computational complexity.

[0306] Step 5: Update prediction. The predicted state probability density function can be expressed as follows:

[0307]

[0308] in,

[0309]

[0310] It can be seen that the prediction weight The calculation combines two pieces of information, namely the filter weights and the weight of the process noise probability density function In addition, based on the estimated state mean Estimated variance matrix Process noise mean and the process noise covariance matrix Calculated using SRCKF The predicted mean And the covariance matrix In addition, c and d are:

[0311]

[0312] Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k .

[0313] 1.3HPO-DOA algorithm

[0314] The combination of Hunter-prey optimizer (HPO) and Dingo Optimization Algorithm (DOA) is used to achieve simultaneous estimation of multiple covariances, thereby obtaining the required Combining the advantages of the above two algorithms, "distinguishing hunters from prey" and "diversifying predatory behavior" are combined. The specific improvement method is:

[0315] (1) The hunter-prey distinction strategy in the HPO algorithm is applied to change the "prey" of the DOA algorithm from the "optimal value of the previous iteration" to the individual that is found as the "prey" by the hunter-prey algorithm. By relying on the method of "the hunter chases the prey and the prey flees to a safe point", better global optimization capabilities are obtained, effectively avoiding the local optimal situation caused by directly using the optimal value of the individual of the previous iteration as the prey in the DOA algorithm.

[0316] (2) The diversified predation strategies of the DOA algorithm replace the single predation method in the original HPO algorithm, which can effectively improve the probability of searching for the optimal value in a limited iterative process.

[0317] set up The orders of are q and r respectively. In a q k ×q+r k In ×r-dimensional space, a population X consisting of n individuals is randomly generated. n ). The i-th individual corresponds to a vector Indicates that it is in q k ×q+r k ×r-dimensional space. The fitness value corresponding to each individual is calculated according to the fitness function.

[0318] Step 1: Generation of the initial population. The position of each member in the initial population is randomly generated in the search space using equation (20). At the same time, the individuals estimated by the EM algorithm above are placed in the randomly generated initial population.

[0319] x i =rand(d,1)×(ub-lb)+lb, (20)

[0320] Among them, x i is the position of the hunter or prey, lb is the minimum value of the problem variable (lower bound), ub is the maximum value of the problem variable (upper bound), and d is the number of problem variables (dimension).

[0321] Step 2 Calculate the fitness value of each individual. The fitness function is:

[0322] Step 3: Distinguish hunters from prey. According to the hunter-prey optimization algorithm, the criterion for distinguishing whether an individual is a hunter or a prey is that the individual with the largest average distance position is regarded as the prey (P pos ):

[0323]

[0324] The position average is The distance between each individual and the average position is D i=|x i -μ|. Otherwise it will be considered a hunter.

[0325] Step 4: Update the prey position. According to the hunter-prey optimization algorithm, the prey position is updated using the following formula (22):

[0326] x i (t+1)=x * (t)+CZcos(2πR4)(x * (t)-x i (t)), (22)

[0327] Among them, x i (t) is the current position of the prey; x i (t+1) is the next iteration position of the prey; x * (t) is the global optimal position; Z is the adaptive parameter calculated by formula (23); R4 is a random number in the range [-1,1]; C is the balance parameter between exploration and exploitation, its value decreases during the iteration process of the algorithm and is calculated by formula (24).

[0328] P=R1<C; IDX=(P==0);

[0329]

[0330] R1 and R3 are random vectors in [0,1], P is the index value of R1 < C, R2 is a random number in [0,1], IDX is the index value of vector R1 that satisfies the condition (P == 0), and C is the balance parameter between exploration and development, whose value decreases from 1 to 0.02 during the iteration process, and is calculated as follows:

[0331] C=1-it(0.98 / MaxIt), (24)

[0332] Among them, it is the current iteration number and MaxIt is the maximum iteration number.

[0333] Step 5: Update the hunter (wild dog) position.

[0334] Step 5.1 Strategy 1: Collective attack Predators usually use highly intelligent hunting skills, while wild dogs tend to hunt small prey alone, such as rabbits. However, once faced with large prey such as kangaroos, they will quickly form a group and show collective behavior of coordinated hunting. Wild dogs have the ability to find prey and cleverly surround them. Their behavior can be expressed as the following formula (25):

[0335]

[0336] Among them, x i(t+1) is the new position of the individual (indicating the movement of the wild dog); na is a random integer generated in the reverse order of [2, SizePop / 2], where SizePop is the size of the population, are all the wild dogs that will attack, among which X randomly generated wild dog population, x i (t) is the current position of the individual; β1 is a random number uniformly generated within [-2,2], which is a proportional factor.

[0337] Step 5.2 Strategy 2: Persecution

[0338] Wild dogs usually hunt small prey until they are captured alone. The following equation models this behavior:

[0339]

[0340] Among them, the value of β1 is the same as that of formula (25), β2 is a random number uniformly generated in the interval [-1,1], and r1 is a random number generated in the interval [1,SizePop]. is the r1th individual selected randomly, where i≠r1.

[0341] Step 5.3 Strategy 3: Scavenging

[0342] The behavior of scavengers can be described as the behavior of wild dogs that stumble upon carrion and eat it while wandering in their natural habitat. This behavior is simulated using the following equation (27):

[0343]

[0344] Among them, β2 has the same value as that in formula (26), and σ is selected as follows: generate a random number in [0,1], and return 0 if it is less than or equal to 0.5, otherwise return 1.

[0345] Step 5.4 Strategy 4: Wild Dog Survival Rate

[0346] The Australian wild dog is facing the threat of extinction, mainly due to illegal hunting. In the DOA algorithm, the survival rate of the wild dog can be given by the following formula:

[0347]

[0348] Among them, fitness max and fitness minare the worst and best fitness values ​​in the current generation, respectively, and fitness(i) is the current fitness value of the i-th individual. Equation (29) is applied to low survival rates through the algorithm (if the survival rate is less than or equal to 0.3, the current low survival rate search agent is updated using Equation (29)), for example, the survival rate value is equal to or less than 0.3.

[0349]

[0350] Among them, x i (t) is the individual with a lower survival rate to be updated, r1 and r2 are random numbers generated in the interval [1, SizePop], r1≠r2, and are the r1th and r2th individuals selected randomly.

[0351] 1.4 Algorithm Flow

[0352] The process of HPO-DOA algorithm is as follows:

[0353] 1) In the defined interval [x min ,x max ] Randomly generate population individuals, and put the individuals estimated by the previous EM algorithm into the randomly generated initial population.

[0354] 2) Constructing fitness function Substitute the population individuals as the noise covariance corresponding to each Gaussian sub-item into the filtering algorithm to obtain the corresponding And calculate the fitness value.

[0355] 3) Find the global optimal position based on each fitness value.

[0356] 4) Distinguish between hunters and prey among individuals in a population.

[0357] 5) Update the individuals in the population according to equations (21) to (29).

[0358] 6) Calculate the fitness value of the updated individual and find the global optimal position.

[0359] 7) Determine whether the number of iterations has reached the upper limit. If yes, output the corresponding optimal individual as the covariance estimate of each Gaussian sub-item Thus, an accurate non-Gaussian noise estimation is obtained; otherwise, go to step 4).

[0360] 8) According to Calculate t(k).

[0361] The algorithm flow chart is as follows Figure 2 shown.

[0362] 2.1 Pseudo-matrix construction based on particle Gaussian sub-items of improved GS-SRCPF

[0363] In order to simplify the theoretical research of nonlinear non-Gaussian MPR, a pseudo matrix of SRCKF sub-filter is constructed in the IGS-SRCPF framework to solve the definition problem of nonlinear non-Gaussian MPR by using the definition idea of ​​linear Gaussian MPR. Figure 3 Shown

[0364] 2.1.1 Square Root Cubic Kalman Filter

[0365] The CKF filtering process is similar to that of UKF, but its theoretical derivation is more rigorous. However, in the recursive process of CKF, each step needs to calculate the square root of the state estimation covariance matrix and the one-step prediction covariance matrix, which brings the disadvantages of large computational complexity and numerical instability. In contrast, SRCKF directly propagates and updates the square root of the state variance matrix in the form of Cholesky decomposition based on CKF, thereby reducing the computational burden and improving computational efficiency. At the same time, SRCKF can ensure the non-negative definiteness of the covariance matrix, avoid the divergence of the filter, and thus improve the convergence speed and numerical stability of the filter. Therefore, SRCKF is selected as the sub-filter of the Gaussian sum. The filtering process of the initialized SRCKF is as follows: Figure 4 shown.

[0366] 2.1.2 Improved Gaussian and Square Root Volumetric Particle Filter

[0367] By using Gaussian and particle filter algorithms, the complex non-Gaussian MPR theory can be simplified to the MPR theory of multi-particle Gaussian sub-items. PF approximates the probability density function of random variables of any form by random sampling points, and replaces the integral operation with the sample mean to achieve the state estimation of the target. The estimation effect of PF is very dependent on the importance function (k is a discrete time point). Sampling a set of sample points from a reasonable importance function is very important for improving the accuracy of target state estimation. Taking GS-CKF as the importance function of PF, the advantages of the two algorithms are combined to improve the state estimation accuracy of the filter.

[0368]

[0369] It is known that SRCKF has higher computational efficiency and filtering accuracy than CKF, so this paper uses IGS-SRCKF as the importance function of PF. On this basis, non-Gaussian decomposition is performed on the initial state of a single particle and a simplified fusion strategy is added to improve the algorithm performance. Based on the above conclusions, formula (30) is modified as follows:

[0370]

[0371] On this basis, the IGS-SRCPF algorithm in this paper also uses an improved EM algorithm (referred to as the unsupervised robust EM algorithm in this paper). Compared with the traditional EM algorithm, this algorithm can adaptively estimate the number of optimal Gaussian components, but this algorithm is prone to waste of limited Gaussian sub-items. Therefore, an improved unsupervised robust EM algorithm is proposed based on the fusion strategy idea. The specific steps of the IGS-SRCPF algorithm are as follows:

[0372] Step 1 Initialization. First, assume x0 is the initial state variable and generate H particles from the prior distribution p(x0) Recalculate the initial state and the estimated error covariance P0.

[0373]

[0374] The improved EM algorithm in the literature

[27] is used, which is referred to as the unsupervised robust EM algorithm. The process noise probability density function p(ω k,k-1 ) and the measurement noise probability density function p(ν k ) are expressed in the form of Gaussian sum, as shown in equations (4) and (5). The unsupervised robust EM algorithm process is as follows:

[0375] Taking the measurement noise data as an example, suppose there is a set of measurement noise data V k =(ν1,ν2,…,ν n ), where v k is a p-dimensional vector generated by a Gaussian mixture model:

[0376]

[0377] In the formula, are the weight, mean and covariance of the i-th Gaussian term in the Gaussian mixture model, r k Represents the number of components in the mixture model.

[0378] Step 1: Initialize parameters. Let r k =n, β=1. Take any value, that is, the value of θ, and set a sufficiently small ε>0 as the condition for judging the end of the loop. Where β is:

[0379]

[0380] Step 2: E-step: Calculate the implicit parameter z ki :

[0381]

[0382] Step 3: M-step: Update the weights by equations (37), (38) and (35) respectively. and β, and β=1 for the first iteration.

[0383]

[0384] Step 4: When When , the i-th Gaussian component is invalid and the mixing coefficient at this time is removed At the same time, the Gaussian component parameters When the number of iterations is greater than 60, set β = 0.

[0385] Step 5: Update from equation (39) and equation (40) to obtain and z ki' , and updated by equations (38) and (41) and

[0386]

[0387] Step 6: Update z according to equation (36) ki .

[0388] Step 7: If If yes, the iteration stops; otherwise, go to step 3 to continue.

[0389] The iterative flow chart of the unsupervised robust EM algorithm is as follows Figure 5 shown.

[0390] Although the above algorithm can adaptively estimate the number of Gaussian subterms, there is a problem of wasting effective Gaussian subterms. Therefore, this paper proposes a fusion strategy to improve step 4 of the algorithm.

[0391] First, remove weights less than The Gaussian component of , the Gaussian component fusion strategy is performed on the interval of formula (42),

[0392]

[0393] Among the Gaussian components in this interval, find the one with the largest weight,

[0394]

[0395] And find the Gaussian components that meet the conditions according to the following criteria:

[0396]

[0397] in,

[0398] These Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are:

[0399]

[0400] in,

[0401] Delete all Gaussian components that have been fused, repeat the above steps until all Gaussian components have completed the operation, and finally retain only the first S sub-Gaussian distributions with relatively large weights. This algorithm can avoid the waste of a large number of valid Gaussian sub-items.

[0402] Step 2 Importance Sampling

[0403] Step 2.1 Gaussian fitting

[0404] The improved unsupervised robust EM algorithm is used to transform the initial probability density function of H particles into It can be expressed in the form of Gaussian sum (or approximate Gaussian sum) as follows:

[0405]

[0406] in, is the weight of the corresponding Gaussian sub-item, and:

[0407]

[0408] Step 2.2 Distributed Filtering

[0409] For H particles, the filtering probability density function is approximated using the Gaussian sum form as in equation (49):

[0410]

[0411] Among them, the measurement And: N k|k =N k|k-1 ·r k In addition, based on the predicted state mean Prediction covariance matrix Measurement noise mean and the measurement variance matrix The SRCKF filtering algorithm is used to calculate the i-th Gaussian subterm of the h-th particle. The filtered mean and the covariance matrix

[0412] At this time, the measurement likelihood function of the state estimation of the i-th Gaussian sub-term of the h-th particle can be expressed as:

[0413]

[0414] The calculation method of indicators j and l is:

[0415]

[0416] Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates the rounding down operation. Filter weight It is necessary to integrate three pieces of information, namely the prediction filter weights The weight of the probability density function of the measurement noise And the likelihood function of the state estimate Therefore The adaptive calculation method is:

[0417]

[0418] Step 2.3 Global point estimation. After the Gaussian sub-items of each particle obtain the filtering state at time k, the global filtering mean at time k can be calculated using equations (53) and (54): and the covariance matrix

[0419]

[0420] Similarly, the state estimates at other time points can also be calculated using equations (53) to (54).

[0421] Step 2.4 Simplified control. Usually, the process and measurement noise distributed in the form of Gaussian sum will accumulate over time and lead to the continuous increase of the number of Gaussian sub-terms, such as N k|k and N given later k+1|k This is bound to significantly increase the computational overhead and reduce the practicality of filtering. Therefore, in order to improve the real-time performance of the algorithm and ensure the filtering accuracy, this paper adopts the Gaussian component fusion strategy. After calculating the estimated mean and covariance of each sub-item at time k, the Gaussian component with a weight greater than G' is first retained and other components are deleted.

[0422]

[0423] Among them, G' is the weight of the Gth Gaussian component after the weights are sorted from large to small.

[0424] Among the remaining Gaussian components, find the one with the largest weight:

[0425]

[0426] And find the Gaussian components that meet the conditions according to the following criteria:

[0427]

[0428] in,

[0429] These Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are:

[0430]

[0431] in,

[0432] Delete all Gaussian components that have been fused, and then repeat the above steps until all Gaussian components have completed the operation. Finally, only the first S sub-Gaussian distributions with relatively large weights are retained, and the corresponding weights are normalized again to reduce the computational complexity.

[0433] Step 2.5 Prediction update. The predicted state probability density function can be expressed as follows:

[0434]

[0435] in,

[0436]

[0437] It can be seen that the prediction weight The calculation combines two pieces of information, namely the filter weights and the weight of the process noise probability density function In addition, based on the estimated state mean Estimated variance matrix Process noise mean and the process noise covariance matrix Calculated using SRCKF The predicted mean and the covariance matrix In addition, c and d are:

[0438]

[0439] Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k .

[0440] Step 3: Particle update.

[0441] Based on the IGS-SRCKF importance function and Generate new particles

[0442]

[0443] Step 4: Calculate particle weights and normalize them.

[0444] Repeat step 2-step 3 for the other H-1 particles to obtain the state estimates of all particles, calculate the particle weights and normalize them:

[0445]

[0446] In the formula, the particle With weight One to one correspondence.

[0447] Step 5: Random resampling.

[0448] The particles are randomly resampled, that is, the roulette algorithm is used to copy the particles. The algorithm needs to generate a random number that obeys the uniform distribution in [0,1] in advance. For particles with large weights, the probability that the random number falls into the interval corresponding to the particle weight is higher, and the particles are more likely to be copied, while particles with small weights are more likely to be abandoned, and the above particle copying process is repeated (the number of particle copies is equal to the total number of particles H).

[0449] Step 6 Calculate the state estimate and the estimation error covariance matrix.

[0450] The final target posterior state estimator and the estimation error covariance matrix at time k are calculated based on the resampled particles.

[0451]

[0452] At this point, the state filtering process at time k has been completed, and the above steps are repeated to complete the estimation of the state quantity and error covariance matrix at the next time. The schematic diagrams of the IGS-SRCKF filtering algorithm and the IGS-SRCPF filtering algorithm are shown in Figure 1. Figure 6 , Figure 7 shown.

[0453] The above algorithm uses SRCKF as a sub-filter to estimate the system state corresponding to each particle Gaussian sub-term separately, and then achieves global estimation by combining the Gaussian sub-terms of each particle. Next, the definition of MPR will be studied in depth in the sub-filter for the case where the system has mismatched noise covariance.

[0454] 2.1.3 Constructing a pseudo matrix

[0455] As can be seen above, the filter can be decomposed into H×N by the IGS-SRCPF filtering algorithm. k|k is a sub-filter. Next, we further construct the pseudo-matrix of the SRCKF sub-filter, which can simplify the complex nonlinear MPR theory research into the MPR theory research of multiple linear Gaussian sub-items. By using the method of constructing the pseudo-matrix through CKF filtering, the pseudo-observation matrix of the SRCPF sub-filter is obtained as:

[0456]

[0457] The pseudo-state transfer matrix is:

[0458]

[0459] in,

[0460]

[0461] Thus, the iterative process of the sub-filter is simplified to:

[0462]

[0463] Where i, j, l, c, and d are calculated as follows:

[0464]

[0465] Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates floor operation.

[0466]

[0467] Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k .

[0468] 2.2 Defining model parameter ratio

[0469] Like the linear Gaussian Kalman filtering algorithm, for IGS-SRCPF filtering, if a more accurate noise covariance can be obtained in each particle Gaussian sub-item, the accuracy of the IGS-SRCPF filtering result can be effectively improved.

[0470] 2.2.1 Particle Gaussian subterms have mismatched noise covariance

[0471] It is known that the accuracy of the noise covariance in each particle Gaussian subterm is usually poor. That is, when there is a mismatched noise covariance in the subsystem, for each particle Gaussian subterm, there is:

[0472]

[0473] In the formula, represents the true observed noise covariance of the ith Gaussian subterm of each particle, Represents the true process noise covariance of the i-th Gaussian subterm of each particle. represents the observation noise covariance of the i-th Gaussian sub-term hypothesis of each particle in practical application, represents the process noise covariance of the i-th Gaussian sub-term hypothesis of each particle in practical application, and both are denoted by superscript u. and Represents the deviation of the particle's Gaussian subterm.

[0474] It is known that there is a Kalman filter with mismatched noise covariance in the Gaussian subterms of each particle:

[0475]

[0476]

[0477]

[0478] The superscript f represents the actual calculated value of the sub-filter. However, according to the literature, It is not the Gaussian sub-item filter estimate of the particle at time k TMSE. Because:

[0479]

[0480] Available The TMSE is:

[0481]

[0482] in,

[0483]

[0484] Among them, the superscript m represents the true value of the filter. In addition, the indicators e and g are calculated as follows:

[0485]

[0486] 2.2.2 Improved model parameter ratio of Gaussian and SRCPF filters

[0487] The MPR theoretical method of linear Gaussian, for the linear Gaussian subsystem, assume:

[0488]

[0489] in, is the process noise parameter ratio of the i-th Gaussian sub-item of each particle, is the process noise parameter ratio of the i-th Gaussian sub-item of each particle, is the model parameter ratio of the ith Gaussian subsystem for each particle.

[0490] This paper studies the MPR theory in nonlinear non-Gaussian systems, using the IGS-SRCPF filter framework. In this framework, there are H×N k|k linear particle Gaussian subterms. For the noise covariance Gaussian subterm, there exists q k The process noise covariance Gaussian sub-terms are: Existence k The observation noise covariance Gaussian sub-terms are: Therefore, there is a total of q k × k The combinations are:

[0491]

[0492] Therefore, in the Gaussian and SRCPF filter framework,

[0493]

[0494] is called the process noise parameter ratio,

[0495]

[0496] is called the observation noise parameter ratio,

[0497]

[0498] is called the MPR of the system. If the MPR of another model is:

[0499]

[0500] The two models are said to conform to the same model parameter ratio.

[0501] 2.3 Noise Estimation Based on Parameter Estimation Criteria

[0502] 2.3.1 Estimation of model adjustment coefficients

[0503] Innovation of known sub-filters for:

[0504]

[0505] The covariance matrix of is:

[0506]

[0507] The superscript f indicates a calculated value. Not really The true value of the covariance matrix is:

[0508]

[0509] The superscript m indicates the true value.

[0510] In each particle Gaussian subsystem, there is an existence theorem for the model parameter ratio, that is, if the estimated value of the process noise covariance subterm Satisfy the process noise sub-item parameter ratio of the system, the estimated value of the observation noise covariance sub-item Satisfies the observation noise sub-item parameter ratio of the system, and can be obtained minimum, then they must satisfy the model parameter ratio of the Gaussian subsystem of the particle.

[0511] According to the existence theorem of model parameter ratio of linear Gaussian system, this paper elevates it to the existence theorem of model parameter ratio of nonlinear non-Gaussian system. In the IGS-SRCPF filter framework, if the estimated value of the process noise covariance sub-term The process noise parameter ratio shown in equation (95) satisfies the estimated value of the observation noise covariance sub-term The observation noise parameter ratio shown in formula (96) is satisfied, and the The minimum value of , then they must satisfy the system model parameter ratio shown in equation (97). Therefore, when the parameter ratio of each noise covariance sub-item is known, we can find The estimated value of the noise covariance sub-term obtained by the minimum value of b h,i is a scalar greater than 0. When , the noise covariance deviation of the sub-filter is:

[0512]

[0513] According to (83) and (89), we can get:

[0514]

[0515] Then, according to the simplified filter parameter linear multiple theorem, in the linear Gaussian subsystem, for the steady-state system, if Established, when and Multiply by the same scalar K, when the system reaches a steady state, It can be seen that Substituting it into the above formula (104), we can get:

[0516]

[0517] From the above formula (105), we can know Since in the estimation process, is not accurate, so and Stretch them into vector X h,i , Y h,i . Use the least squares method to estimate b h,i :

[0518] b h,i =((X h,i ) T X h,i ) -1 (X h,i ) T Y h,i . (106)

[0519] b h,i Substituting into equations (102) and (103), we can obtain:

[0520]

[0521] In summary, we can get the parameter estimation criteria under the improved Gaussian and SRCPF framework:

[0522] For each Gaussian subterm, The necessary and sufficient conditions are:

[0523]

[0524] st satisfies the process and observation noise parameter ratio. (109)

[0525] 2.3.2HPO-DOA algorithm

[0526] The combination of HPO optimization algorithm and DOA optimization algorithm realizes the simultaneous estimation of multiple noise covariance sub-items, thus obtaining the required and The detailed HPO-DOA algorithm steps are shown in Step 1-Step 5. In Step 1, after the initial population is generated, the individuals estimated by the improved unsupervised robust EM algorithm are also placed in the randomly generated initial population.

[0527] 2.3.3 Algorithm Flow

[0528] Since each particle Gaussian sub-item has a different model adjustment coefficient, the algorithm uses a multi-objective function optimization method to solve it. The specific process of the multi-objective HPO-DOA algorithm is as follows:

[0529] (1) In the defined interval [x min ,x max ] Randomly generate population individuals, retain the weights calculated by the improved robust EM algorithm above, and the number of process noise Gaussian sub-items c q , the number of Gaussian sub-terms of observation noise c r .individual

[0530] The parameter ratio of each process noise sub-item and the parameter ratio of each measurement process noise should be satisfied (for the individuals estimated by the improved unsupervised robust EM algorithm in the randomly generated initial population, the first diagonal element of each noise covariance of the individual is taken, and the individual is also made to meet the parameter ratio condition according to the parameter ratio.). The estimation method steps of the noise covariance sub-item parameter ratio are as follows:

[0531] 1) Estimate the parameter ratio of the A-order noise covariance sub-term by improving the unsupervised robust EM algorithm.

[0532] 2) Keep the first c obtained from each estimation q process noise subterms and the first c r There are observation noise sub-items, and if they are insufficient, they will be filled with 0.

[0533] 3) Take the average of the A noise covariance sub-item parameter ratios.

[0534] 4) Repeat steps 1)-3) A times to obtain A new results.

[0535] 5) Take the average of the A results.

[0536] 6) Repeat steps 1)-5) A times to obtain A new results.

[0537] 7) Take the average of the A new results.

[0538] And so on..., we will find that the more cycles there are, the closer the estimated noise sub-item parameter ratio is to the true value.

[0539] (2) Construct H×N k|k Fitness function Substitute the population individuals as the noise covariance corresponding to the Gaussian sub-items of each particle into the filtering algorithm, and obtain the corresponding Gaussian sub-items of each particle through equations (100) and (101): And calculate the fitness value of the population individuals corresponding to each fitness function.

[0540] (3) According to each fitness value, find the global optimal position among the individuals in the population corresponding to each fitness function.

[0541] (4) In H×N k|k Distinguish between hunters and prey within a population.

[0542] (5) Update the individuals in the population according to equations (21) to (29).

[0543] (6) Calculate the fitness values ​​corresponding to the individuals after the fitness functions are updated and find the global optimal positions of the corresponding individuals in the population.

[0544] (7) Determine whether the number of iterations has reached the upper limit. If yes, output the optimal individual corresponding to each fitness function as the noise covariance estimate of each Gaussian sub-item Otherwise go to step (4).

[0545] (8) Calculate the adjustment coefficient b according to formula (106): h,i .

[0546] (9) Substitute the adjustment coefficient and calculate in each particle Gaussian subterm according to equations (107) and (108):

[0547] (10) Multiple Sum and average to get the final

[0548] (11) Substitute into the IGS-SRCPF filtering algorithm proposed in this paper.

[0549] Embodiment 2: In the second aspect, as Figure 8 As shown, in order to achieve the above purpose, the present invention discloses an IGS-SRCPF electric-driven UAV pose estimation system based on model parameter ratio, comprising:

[0550] The state decomposition module is used to obtain particles, perform non-Gaussian decomposition on the initial state of particles based on Gaussian and particle filter algorithms, and fit and generate nonlinear particle Gaussian sub-items;

[0551] A linearization module is used to linearize the nonlinear particle Gaussian sub-items based on the pseudo-matrix of the pre-constructed SRCKF sub-filter to obtain linear particle Gaussian sub-items, and generate a nonlinear non-Gaussian system model MPR based on the linear particle Gaussian sub-items;

[0552] The pose estimation module is used to solve the nonlinear non-Gaussian system model MPR based on the MPR existence theorem to obtain the model adjustment coefficient. The pose of the IGS-SRCPF electric-driven UAV is estimated through the preset multi-objective HPO-DOA algorithm combined with the model adjustment coefficient to obtain the estimation result.

[0553] Based on the same inventive concept, the present invention also provides a computer device, which includes: one or more processors, and a memory for storing one or more computer programs; the program includes program instructions, and the processor is used to execute the program instructions stored in the memory. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is used to implement one or more instructions, specifically for loading and executing one or more instructions in a computer storage medium to implement the above method.

[0554] It needs to be further explained that, based on the same inventive concept, the present invention also provides a computer storage medium, on which a computer program is stored, and the computer program is executed by a processor to execute the above method. The storage medium can adopt any combination of one or more computer-readable media. The computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electrical, magnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples (non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present invention, a computer-readable storage medium can be any tangible medium containing or storing a program, which can be used by an instruction execution system, device or device or used in combination with it.

[0555] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present disclosure. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0556] The above shows and describes the basic principles, main features and advantages of the present disclosure. Those skilled in the art should understand that the present disclosure is not limited by the above embodiments, and the above embodiments and descriptions are only for explaining the principles of the present disclosure. Without departing from the spirit and scope of the present disclosure, the present disclosure may have various changes and improvements, and these changes and improvements fall within the scope of the present disclosure to be protected.

Claims

1. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio is characterized by: The method comprises the following steps: Obtain particles, perform non-Gaussian decomposition on the initial state of the particles based on the Gaussian and particle filter algorithms, and fit and generate nonlinear particle Gaussian sub-items; Based on the pseudo-matrix of the pre-constructed SRCKF sub-filter, the nonlinear particle Gaussian sub-item is linearized to obtain the linear particle Gaussian sub-item, and the nonlinear non-Gaussian system model MPR is generated based on the linear particle Gaussian sub-item; Based on the MPR existence theorem, the nonlinear non-Gaussian system model MPR is solved to obtain the model adjustment coefficient. The position and posture of the IGS-SRCPF electric-driven UAV is estimated by the preset multi-objective HPO-DOA algorithm combined with the model adjustment coefficient to obtain the estimation result.

2. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio according to claim 1 is characterized in that: The nonlinear non-Gaussian system model is as follows: x k+1 =f k (x k )+w k+1,k , (1) z k =h k (x k )+v k , (2) Among them, k represents the time, x k ∈R n is the system state vector, f k :R n →R n 、h k :R n →R p Respectively represent the system state matrix and measurement matrix, w k+1,k ∈R n represents non-Gaussian process noise, z k ∈R p is the observation vector, v k ∈R p represents non-Gaussian observation noise, and the state noise and measurement noise are independent of each other.

3. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio according to claim 1 is characterized in that: The Gaussian and particle filter algorithm process includes the following steps: Initialization: Set the initial time point k = 0 and define the initial state p(x0|z -1 )=p(x0), and as N k|k-1 The sum of Gaussian subterms; Using the EM algorithm, the initial probability density function p(x0), the process noise probability density function p(ω k ) and the measurement noise probability density function p(v k ) are expressed in the form of Gaussian sum as follows: in, and is the weight of its corresponding Gaussian sub-item, and there is Distributed filtering: The Gaussian sum form of equation (6) is used to approximate the filtering probability density function: Among them, measurement And: N k|k =N k|k-1 ·r k . In addition, based on the predicted state mean Prediction covariance matrix and the mean value of the measurement noise and the measurement variance matrix The SRCKF filtering algorithm is used to calculate the i-th Gaussian sub-term The filtered mean and the covariance matrix At this time, the measurement likelihood function of the state estimation of the i-th Gaussian sub-item is expressed as: The calculation method of indicators j and l is: Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates the rounding down operation, filter weight Comprehensive prediction filter weights are required The weight of the probability density function of the measurement noise And the likelihood function of the state estimate Therefore The adaptive calculation method is: Global point estimation: After each Gaussian sub-item obtains the filtering state at time k, the global filtering mean at time k can be calculated using equations (10) and (11): and the covariance matrix P k|k ; Similarly, the state estimates at other time points are calculated by equations (10) to (11); Reduction control: After calculating the estimated mean and covariance of each sub-item at time k, first retain the Gaussian component with a weight greater than G' and delete the other components; Among them, G' is the weight of the Gth Gaussian component after the weights are sorted from large to small; Among the retained Gaussian components, find the component with the largest weight: And find the Gaussian components that meet the conditions according to the following criteria: in, The Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are: in, Delete all Gaussian components that have been fused, and then repeat the above steps until all Gaussian components have completed the operation, retain the first S sub-Gaussian distributions with large weights, and re-normalize the corresponding weights; Prediction update: The predicted state probability density function can be expressed as follows: in, It can be seen that the prediction weight The calculation combines two pieces of information, namely the filter weights and the weight of the process noise probability density function In addition, based on the estimated state mean Estimated variance matrix Process noise mean and the process noise covariance matrix Calculated using SRCKF The predicted mean and the covariance matrix c and d are: Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k .

4. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio according to claim 1 is characterized in that: The multi-objective HPO-DOA algorithm uses the combination of the hunter-prey optimization algorithm HPO and the wild dog optimization algorithm DOA to achieve simultaneous estimation of multiple covariances, thereby obtaining the required The improvement methods are as follows: By applying the hunter-prey distinction strategy in the HPO algorithm, the prey of the DOA algorithm is transformed from the optimal value of the previous iteration to the individual acting as the prey found by the hunter-prey algorithm; the hunter chases the prey and the prey flees to a safe point to obtain better global optimization capabilities; The diversified predation strategies of the DOA algorithm replace the single predation method in the original HPO algorithm; set up The orders are q and r respectively. In a q k ×q+r k In ×r-dimensional space, a population X consisting of n individuals is randomly generated. n ), the i-th individual corresponds to a vector Indicates that in q k ×q+r k ×r-dimensional space, calculate the fitness value corresponding to each individual according to the fitness function; Generation of initial population: The position of each member in the initial population is randomly generated in the search space according to formula (20): x i =rand(d,1)×(ub-lb)+lb, (20) Among them, x i is the position of the hunter or prey, lb is the minimum value of the problem variable, ub is the maximum value of the problem variable, and d is the number of problem variables; The fitness function for calculating the fitness value of each individual is: Distinguishing hunters from prey: According to the hunter-prey optimization algorithm, the criterion for distinguishing whether an individual is a hunter or a prey is that the individual with the largest average distance position is regarded as the prey P pos : The position average is The distance between each individual and the average position is D i =|x i -μ, otherwise it will be regarded as a hunter; Update of prey position: According to the hunter prey optimization algorithm, the prey position is updated using the following formula (22): x i (t+1)=x * (t)+CZcos(2πR4)(x * (t)-x i (t)), (22) Among them, x i (t) is the current position of the prey; x i (t+1) is the next iteration position of the prey; x * (t) is the global optimal position; Z is the adaptive parameter calculated by equation (23); R4 is a random number in the range [-1,1]; C is the balance parameter between exploration and exploitation, whose value decreases during the iteration of the algorithm and is calculated by equation (24): P=R1<C; IDX=(P==0); R1 and R3 are random vectors in [0,1], P is the index value of R1 < C, R2 is a random number in [0,1], IDX is the index value of vector R1 that satisfies the condition (P == 0), and C is the balance parameter between exploration and exploitation, calculated as follows: C=1-it(0.98 / MaxIt),(24) Among them, it is the current iteration number, MaxIt is the maximum iteration number; Updates on Wild Dog Locations: Strategy 1: Wild dogs have the ability to find prey and cleverly surround it. Their behavior is expressed in the following formula (25): Among them, x i (t+1) is the new position of the individual, indicating the movement of the wild dog; na is a random integer generated in the reverse order of [2, SizePop / 2], where SizePop is the size of the population, are all the wild dogs that will attack, among which X randomly generated wild dog population, x i (t) is the current position of the individual; β1 is a random number uniformly generated within [-2,2], which is the proportional factor; Strategy 2: Persecution Wild dogs hunt small prey until they catch it alone, and the following equation models this behavior: Among them, the value of β1 is the same as that of formula (25), β2 is a random number uniformly generated in the interval [-1,1], and r1 is a random number generated in the interval [1,SizePop]. is the r1th individual selected randomly, where i≠r1; Strategy 3: Scavenging This behavior is simulated using the following equation (27): Among them, β2 is the same as the value in formula (26), and the selection method of σ is: generate a random number in [0,1], and return 0 if it is less than or equal to 0.5, otherwise return 1; Strategy 4: Wild Dog Survival Rate In the DOA algorithm, the survival rate of wild dogs is given by: Among them, fitness max and fitness min are the worst and best fitness values ​​in the current generation, respectively, and fitness(i) is the current fitness value of the i-th individual. Equation (29) is applied to low survival rates through the algorithm, and the survival rate value is equal to or less than 0.3: Among them, x i (t) is the individual with a lower survival rate to be updated, r1 and r2 are random numbers generated in the interval [1, SizePop], r1≠r2, and are the r1th and r2th individuals selected randomly.

5. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio according to claim 4 is characterized in that: The process of the HPO-DOA algorithm is as follows: In the defined interval [x min ,x max ] Randomly generate population individuals and put the estimated individuals into the randomly generated initial population; Constructing the fitness function Substitute the population individuals as the noise covariance corresponding to each Gaussian sub-item into the filtering algorithm to obtain the corresponding And calculate the fitness value; Find the global optimal position according to each fitness value; distinguishing between hunters and prey among individuals in a population; Update the individuals in the population according to equations (21) to (29); Calculate the fitness value of the updated individual and find the global optimal position; Determine whether the number of iterations has reached the upper limit. If yes, output the corresponding optimal individual as the covariance estimate of each Gaussian sub-item Thus, an accurate non-Gaussian noise estimate is obtained; otherwise, hunters and prey are re-distinguished among individuals in the population; according to Calculate t(k).

6. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio according to claim 1 is characterized in that: The pseudo matrix of the pre-constructed SRCKF sub-filter is constructed based on the particle Gaussian sub-item of the improved GS-SRCPF, and the process includes: square root volume Kalman filtering, improved Gaussian and square root volume particle filtering and constructing a pseudo matrix; Among them, the process of improving Gaussian and square root volumetric particle filtering is as follows: Using Gaussian and particle filter algorithms, the advantages of the two algorithms are combined to improve the state estimation accuracy of the filter: IGS-SRCKF is used as the importance function of PF. On the basis of this, non-Gaussian decomposition is performed on the initial state of a single particle and a simplified fusion strategy is added. Formula (30) is modified as follows: The specific steps of the IGS-SRCPF algorithm are as follows: Initialization: Let x0 be the initial state and generate H particles from the prior distribution p(x0) Recalculate the initial state and the estimated error covariance P0; The process noise probability density function p(ω k,k-1 ) and the measurement noise probability density function p(ν k ) are expressed in the form of Gaussian sum, as shown in Equation (4) and Equation (5), where the unsupervised robust EM algorithm process is as follows: Assume there is a set of measurement noise data V k =(ν1,ν2,…,ν n ), where v k is a p-dimensional vector generated by a Gaussian mixture model: In the formula, are the weight, mean and covariance of the i-th Gaussian term in the Gaussian mixture model, r k Indicates the number of components of the mixed model; Initialization parameters: Let r k =n, β = 1; Take an arbitrary value, that is, the value of θ, and set ε>0 as the condition for judging the end of the loop, where β is: E-step: Calculate the implicit parameter z ki : M-step: Update weights by equations (37), (38) and (35) respectively. and β, in the first iteration, let β = 1; when When , the i-th Gaussian component is invalid and the mixing coefficient at this time is removed At the same time, the Gaussian component parameters When the number of iterations is greater than 60, let β = 0; Updated by equation (39) and equation (40) and z ki ', and updated by equations (38) and (41) and Update z according to formula (36) ki ; like Then the iteration stops, otherwise go to M-step to continue execution; Improvement process: First, delete the weight less than The Gaussian component of , the Gaussian component fusion strategy is performed on the interval of formula (42), Among the Gaussian components of the interval, find the one with the largest weight, And find the Gaussian components that meet the conditions according to the following criteria: in, The Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are: in, Importance Sampling: Gaussian Fitting The improved unsupervised robust EM algorithm is used to transform the initial probability density function of H particles into It can be expressed in the form of Gaussian sum as follows: in, is the weight of the corresponding Gaussian sub-item, and: Distributed filtering For H particles, the filtering probability density function is approximated using the Gaussian sum form as in equation (49): Among them, the measurement And: N k|k =N k|k-1 ·r k In addition, based on the predicted state mean Prediction covariance matrix Measurement noise mean and the measurement variance matrix The SRCKF filtering algorithm is used to calculate the i-th Gaussian subterm of the h-th particle. The filtered mean and the covariance matrix At this time, the measurement likelihood function of the state estimation of the i-th Gaussian sub-term of the h-th particle can be expressed as: The calculation method of indicators j and l is: Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates rounding down operation; filter weight Comprehensive prediction filter weights are required The weight of the probability density function of the measurement noise And the likelihood function of the state estimate Therefore The adaptive calculation method is: Global point estimation: After the Gaussian sub-items of each particle obtain the filtering state at time k, the global filtering mean at time k can be calculated using equations (53) and (54): and the covariance matrix Similarly, the state estimates at other time points are calculated by equations (53) to (54); Simplified control: Among them, G' is the weight of the Gth Gaussian component after the weights are sorted from large to small; Among the remaining Gaussian components, find the one with the largest weight: And find the Gaussian components that meet the conditions according to the following criteria: in, The Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are: in, Delete all Gaussian components that have been fused, and then repeat the steps until all Gaussian components have completed the operation, retaining only the first S sub-Gaussian distributions with large weights, and re-normalize the corresponding weights; Forecast update, the predicted state probability density function can be expressed as follows: in, It can be seen that the prediction weight Calculation of comprehensive filter weights and the weight of the process noise probability density function In addition, based on the estimated state mean Estimated variance matrix Process noise mean and the process noise covariance matrix Calculated using SRCKF The predicted mean and the covariance matrix In addition, c and d are: Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k ; Particle Update: Based on the IGS-SRCKF importance function and Generate new particles Calculate particle weights and normalize: Repeat step 2-step 3 for the other H-1 particles to obtain the state estimates of all particles, calculate the particle weights and normalize them: In the formula, the particle With weight One to one correspondence; Random resampling: The roulette algorithm is used to copy particles. A random number that is uniformly distributed in [0,1] is generated in advance. For particles with large weights, the probability that the random number falls into the interval corresponding to the particle weight is higher, and the particles are more likely to be copied. Particles with small weights are more likely to be abandoned, and the above particle copying process is repeated; Calculate the state estimator and the estimation error covariance matrix; According to the resampled particles, the final target posterior state estimator and the estimation error covariance matrix at time k are calculated: At this point, the state filtering process at time k has been completed.

7. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio according to claim 6 is characterized in that: The pseudo-matrix construction process: The filter is split into H×N by IGS-SRCPF filtering algorithm k|k For a sub-filter, the pseudo-matrix is ​​constructed by CKF filtering to obtain the pseudo-observation matrix of the SRCPF sub-filter: The pseudo-state transfer matrix is: in, Thus, the iterative process of the sub-filter is simplified to: Where i, j, l, c, and d are calculated as follows: Where i = 1, 2, ..., N k|k ; j = 1, 2, ..., N k|k-1 ; l=1,2,…,r k ;symbol Indicates floor rounding operation; Where i = 1, 2, ..., N k+1|k ,c=1,2,…,N k|k , d=1,2,…,q k .

8. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio according to claim 1 is characterized in that: The nonlinear non-Gaussian system model MPR parameter ratio: The particle Gaussian subterms do not match the noise covariance For each particle Gaussian sub-term, we have: In the formula, represents the true observed noise covariance of the ith Gaussian subterm of each particle, represents the true process noise covariance of the i-th Gaussian subterm of each particle, represents the observation noise covariance of the ith Gaussian subterm assumption for each particle in practical applications, represents the process noise covariance of the ith Gaussian subterm assumption for each particle in practical applications, and both are denoted by superscript u. and represents the deviation of the particle Gaussian subterm; It is known that there is a Kalman filter with mismatched noise covariance in the Gaussian subterms of each particle: Wherein, the superscript f represents the actual calculated value of the sub-filter, It is not the Gaussian sub-item filter estimate of the particle at time k TMSE, because: Available The TMSE is: in, Among them, the superscript m represents the true value of the filter. In addition, the indicators e and g are calculated as follows: Improved model parameter ratio of Gaussian and SRCPF filters The MPR theoretical method of linear Gaussian, for the linear Gaussian subsystem, assume: in, is the process noise parameter ratio of the i-th Gaussian sub-item of each particle, is the process noise parameter ratio of the i-th Gaussian sub-item of each particle, is the model parameter ratio of the i-th Gaussian subsystem of each particle; Using the IGS-SRCPF filter framework, under this framework, there are H×N kk linear particle Gaussian subterms, for the noise covariance Gaussian subterms, there exists q k The process noise covariance Gaussian sub-terms are: Existence k The observation noise covariance Gaussian sub-terms are: Therefore, there is a total of q k × k The combinations are: Therefore, in the Gaussian and SRCPF filter framework, is the process noise parameter ratio, is the observation noise parameter ratio, (g=1,2,…,q k ;l=1,2,…,r k ) (97) is the MPR of the system. If the MPR of another model is: (g=1,2,…,q k ;l=1,2,…,r k ) (98) The two models are said to conform to the same model parameter ratio.

9. The IGS-SRCPF electric-driven UAV pose estimation method based on model parameter ratio according to claim 1, characterized in that: The model adjustment coefficients are estimated: Innovation of known sub-filters for: The covariance matrix of is: The superscript f indicates a calculated value, but Not really The true value of the covariance matrix is: The superscript m indicates the true value; In the IGS-SRCPF filter framework, if the estimated value of the process noise covariance subterm The process noise parameter ratio shown in equation (95) satisfies the estimated value of the observation noise covariance sub-term The observation noise parameter ratio shown in formula (96) is satisfied, and the The minimum value of , then the system model parameter ratio shown in equation (97) must be satisfied. When the parameter ratios of each noise covariance sub-item are known, by finding The estimated value of the noise covariance sub-term obtained by the minimum value of b h,i is a scalar greater than 0. When , the noise covariance deviation of the sub-filter is: According to (83) and (89), we have: Then, according to the simplified filter parameter linear multiple theorem, in the linear Gaussian subsystem, for the steady-state system, if Established, when and Multiply by the same scalar K, when the system reaches a steady state, Substituting into the above formula (104), we can obtain: From the above formula (105), we can know Since in the estimation process, is not accurate, so and Stretch them into vector X h,i , Y h,i , use the least squares method to estimate b h,i : b h,i =((X h,i ) T X h,i ) -1 (X h,i ) T Y h,i .(106) b h,i Substituting into equations (102) and (103), we can obtain: In summary, the parameter estimation criteria under the improved Gaussian and SRCPF framework are obtained: For each Gaussian subterm, The necessary and sufficient conditions are: st satisfies the ratio of process and observation noise parameters.(109).

10. The IGS-SRCPF electric-driven UAV pose estimation system based on model parameter ratio is characterized by: include: The state decomposition module is used to obtain particles, perform non-Gaussian decomposition on the initial state of particles based on Gaussian and particle filter algorithms, and fit and generate nonlinear particle Gaussian sub-items; A linearization module is used to linearize the nonlinear particle Gaussian sub-items based on the pseudo-matrix of the pre-constructed SRCKF sub-filter to obtain linear particle Gaussian sub-items, and generate a nonlinear non-Gaussian system model MPR based on the linear particle Gaussian sub-items; The pose estimation module is used to solve the nonlinear non-Gaussian system model MPR based on the MPR existence theorem to obtain the model adjustment coefficient. The pose of the IGS-SRCPF electric-driven UAV is estimated through the preset multi-objective HPO-DOA algorithm combined with the model adjustment coefficient to obtain the estimation result.

Images