Pose estimation method and system based on credibility and improved Gaussian sum technology
By introducing square root volume Kalman filter and Sage-Husa adaptive filtering algorithm in drone position estimation, combined with the credibility theory, the problem of large amount of Kalman filter calculation and numerical instability in nonlinear non-Gaussian systems is solved. More efficient pose estimation and noise covariance accuracy are achieved, avoiding filter divergence and improving application.
Patent Information
- Application Number
- CN202411317730.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-20
- Publication Date
- 2024-12-27
AI Technical Summary
In nonlinear non-Gaussian systems, the application of Kalman filters has problems such as large calculation volume, numerical instability and non-negative qualitative characteristics of covariance matrix, resulting in divergence of filters, and the existing methods have failed to effectively solve the problem of nonlinear non-Gaussian systems. Pose estimation and noise covariance accuracy problems.
By introducing square root volume Kalman filter and Sage-Husa adaptive filtering algorithm, combining the credibility theory, a drone position estimation method based on credibility and improved Gaussian sum is designed, and a hunter prey optimization algorithm and wild dog search are used. Excellent algorithm, reduces the computational complexity and ensures the non-negative qualitativeness of the covariance matrix.
It achieves higher computing efficiency and higher pose estimation accuracy, avoids the divergence of the filter, and extends the linear Gaussian confidence theory to nonlinear non-Gaussian systems, improving practical application.
Smart Images
Figure CN119197527A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of design of unmanned aerial vehicle posture calculation methods, and in particular to a method and system for estimating the posture of an unmanned aerial vehicle based on credibility and improved Gaussian technology. Background Art
[0002] In linear Gaussian systems, the application research of Kalman filters has gradually matured, and can make the best estimate of the state of the target and obtain better tracking results. However, in nonlinear non-Gaussian systems, the application research of Kalman filters is still a hot topic and difficult point. For example, actual nonlinear non-Gaussian systems such as aircraft posture estimation, missile guidance systems, and satellite navigation systems all have the characteristics of complexity and uncertainty, which brings difficulties to the design and accurate estimation of nonlinear non-Gaussian Kalman filters. Therefore, it is crucial to measure the credibility of filters in nonlinear non-Gaussian systems and find conditions that can better design high-performance filters.
[0003] In the field of UAV pose estimation algorithm design, pose estimation filter algorithm design has attracted the attention of a large number of researchers and has been widely used. Zhao Xiangdan et al. proposed a prediction-fifth-order cubature Kalman filtering method, which can adjust the process noise and its variance matrix of the system model in real time through the prediction filtering method. It is suitable for strongly nonlinear process noise systems. However, in the recursive process, the state estimation covariance matrix and the square root of the one-step prediction covariance matrix must still be calculated at each step, which has the disadvantages of large computational complexity and numerical instability. The non-negative definiteness of the covariance matrix cannot be guaranteed, which easily leads to the divergence of the filter (Zhao Xiangdan, Wang Biao, Wang Zhisheng, Yang Zhong. Prediction-fifth-order cubature Kalman filtering method [J / OL]. Acta Aeronautica Sinica: 1-13 [2022-12-08].). Ge Quanbo et al. proposed an improved Gaussian and cubature Kalman filtering estimation method for nonlinear non-Gaussian systems to improve the accuracy of robot motion state estimation in complex environments. This method introduces the concept of weighted information to improve the penalty term of the objective function of the expectation maximization algorithm to reduce the number of algorithm iterations and improve the convergence speed. At the same time, based on the Gaussian term merging method based on Mahalanobis distance and Kullback-Leibler distance, a fusion mode that can effectively combine the two types of Gaussian term merging methods is proposed to improve the performance and fidelity of multi-Gaussian term merging in Gaussian and filtering. This method can achieve accurate estimation of the robot's motion state, but the expectation maximization (EM) method will have the problem of excessive computational complexity in the process of approximating the process noise probability density function and the measurement noise probability density function to the Gaussian fitting form, and the Gaussian term merging algorithm used is also relatively complex. At the same time, the accuracy of the process noise covariance and observation noise covariance in the Gaussian sub-terms is not considered (Ge Quanbo, Wang Hebin, Yang Qinmin, Zhang Xingguo, Liu Huaping. Robot motion state estimation based on improved Gaussian mixture model [J]. Acta Automatica Sinica, 2022, 48(08): 1972-1983.). QB Ge et al. studied the credibility theory of Kalman filter in linear Gaussian system, defined and constructed the trust factor based on filter calculated mean square error (FMSE) and true mean square error (TMSE) to measure the credibility of the model, and successfully corrected the estimate of the trust factor into an online estimate of the true mean square error (TMSE), and found the conditions for better design of high-performance Kalman filter in linear Gaussian system. However, this credibility theory is only applicable to linear Gaussian system, not nonlinear non-Gaussian system, while the actual situation is more about nonlinear non-Gaussian system. Summary of the invention
[0004] In order to solve the deficiencies mentioned in the above background technology, the purpose of the present invention is to provide a method and system for estimating the posture of a UAV based on credibility and improved Gaussian technology.
[0005] The object of the present invention can be achieved by the following technical solution: a method for estimating the posture of a UAV based on credibility and improved Gaussian and technology, the method comprising the following steps:
[0006] Fitting updates generate multiple nonlinear Gaussian sub-terms in the mixed Gaussian model;
[0007] Based on the nonlinear Gaussian sub-term, the square root volume Klaman filter is introduced as the sub-filter of the Gaussian sum, and the pseudo matrix of the square root volume Kalman sub-filter is constructed;
[0008] Define the trust factor of nonlinear non-Gaussian system, use Sage-Husa adaptive filtering algorithm to estimate the trust factor of nonlinear non-Gaussian system, and design a Kalman filtering framework based on credibility and improved Gaussian and square root cubature combined with credibility theory;
[0009] The hunter-prey optimization algorithm and the wild dog optimization algorithm are combined to obtain HPO-DOA, and the real-time pose results of the UAV during the landing process in the harsh ocean environment are obtained by using the credibility-based and improved Gaussian and square root cubature Kalman filter framework.
[0010] Optionally, the updating process of the nonlinear Gaussian sub-item is as follows:
[0011] N k|k Gaussian subterms according to or Arranged from large to small, and are the weights of the Gaussian sub-term of the ith measurement and process noise at time k, respectively. and is the corresponding standard deviation, and then the current value of the noise is compared with the r in the mixed Gaussian model k or q k Gaussian sub-terms are compared one by one. If the measured noise v k or process noise w k,k-1 and the mean of the i-th Gaussian subterm or The difference between them is less than δ times the standard deviation of the Gaussian subterm or Then the Gaussian subterm is v k or k,k-1 Updated, the rest of the nonlinear Gaussian sub-terms remain unchanged.
[0012] The optional Gaussian fit update equation for the measurement noise is:
[0013]
[0014] in is the weight of the ith Gaussian sub-term of the measurement noise at time k+1, τ r , τ is the learning rate, Gaussian subterm and x k When matching Takes 1, otherwise 0, is the posterior error covariance of the i-th component, is the estimated measurement noise of the i-th component at time k+1 and k; k and k+1 in the lower right corner represent the time, and i in the upper right corner represents the i-th sub-item.
[0015] Where optional, the Gaussian fit update equation for the process noise is:
[0016]
[0017] in is the weight of the i-th Gaussian sub-term of the process noise at time k+1, τ q is the learning rate.
[0018] Optional, to make In order to avoid falling into the situation where it is always 0, the discriminant is used for discrimination. If the discriminant is established, the Gaussian sub-item is deleted. The discriminant is as follows:
[0019] and
[0020] and
[0021] In the formula, and are the initial weight and initial standard deviation respectively, and the subscript k represents the kth moment.
[0022] Optionally, the pseudo matrix of the square root volume Kalman sub-filter is generated as follows:
[0023] After calculating the estimated mean and covariance of each Gaussian sub-item at time k, first retain all Gaussian components with magnitude greater than G and delete all other components. The formula is as follows:
[0024]
[0025] in is the filter weight corresponding to the Gaussian sub-item.
[0026] Among the remaining Gaussian components, find the one with the largest weight:
[0027]
[0028] And find all Gaussian components that meet the conditions according to the following criteria:
[0029]
[0030] in, The threshold U′ is set to 0.04, is the error covariance matrix of the i-th Gaussian component, k|k represents the posterior, and k|k-1 represents the prior.
[0031] These Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are as follows:
[0032]
[0033] in, is the estimated value of the state variable of the i-th Gaussian component, is the error covariance matrix estimate of the i-th Gaussian component.
[0034] Delete all Gaussian components that have been fused, repeat the above steps until all Gaussian components have completed the operation, and finally keep only the first S sub-Gaussian distributions with large weights, and renormalize the corresponding weights to reduce the computational complexity;
[0035] Finally, the pseudo-observation matrix of the square root volume Kalman subfilter is:
[0036]
[0037] in, is the cross-covariance matrix of the j-th Gaussian component, is the predicted covariance matrix of the jth Gaussian component.
[0038] The pseudo-state transfer matrix is:
[0039]
[0040] in, is the pseudo observation matrix, is the pseudo observation matrix
[0041]
[0042] in, is the square root of the prediction error covariance matrix of the jth Gaussian component, is the square root of the dth process noise covariance matrix item.
[0043] Thus the Gaussian subsystem is written in linear Gaussian form:
[0044] x k =Fk,k-1 x k-1 +w k,k-1
[0045] z k =H k x k +v k
[0046] where x k is the system state vector, F k,k-1 is the linear state transfer function, z k is the observation vector, H k is the measurement function.
[0047] The iterative process of the square root volume Kalman subfilter is simplified to:
[0048]
[0049] The number in the upper right corner represents the Gaussian component, the k in the lower right corner represents time, k|k represents the posterior, and k|k-1 represents the prior. represents the filter gain of the i-th component, z k Represents new information;
[0050] Optionally, the formula for defining the trust factor of a nonlinear non-Gaussian system is as follows:
[0051]
[0052] In the formula, t(k) is the trust factor, yes The 2-norm of The value range of is (0, +∞), and the value range of t(k) is 0~1, which is used to measure the closeness of FMSE and TMSE of each sub-filter. At the same time, t(k) will be affected by The impact of The larger is , the smaller is t(k).
[0053] Optionally, the process of estimating the trust factor of a nonlinear non-Gaussian system using the Sage-Husa adaptive filtering algorithm is as follows:
[0054] when hour,
[0055]
[0056] in and are the calculated measurement error covariance and the actual measurement error covariance of the i-th component, respectively, They represent Gaussian sub-terms satisfying The process noise covariance and observation noise covariance of Sage-Husa adaptive Kalman filtering algorithm are shown in hour, Converge to Converge to Right now is the convergence estimate of the Sage-Husa adaptive Kalman filter algorithm. In summary, by adjusting To satisfy:
[0057]
[0058] The same estimation effect as the Sage-Husa adaptive Kalman filter algorithm is guaranteed, and the constraints This ensures that the estimation process will not have noise covariance problems.
[0059] Optionally, the process of the HPO-DOA algorithm is as follows:
[0060] In the defined interval [x min ,x max ] Randomly generate population individuals;
[0061] Constructing the fitness function Substitute the population individuals as the noise covariance corresponding to each Gaussian sub-item into the Kalman filter to obtain the corresponding And calculate the fitness value;
[0062] distinguishing between hunters and prey among individuals in a population;
[0063] Update individuals in the population;
[0064] Calculate the fitness value of each individual;
[0065] 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 This results in an accurate non-Gaussian noise estimate; otherwise, it is difficult to distinguish hunters and prey in the population individuals.
[0066] Calculate the trust factor t(k).
[0067] The optional UAV pose estimation system based on credibility and improved Gaussian technology includes:
[0068] Gaussian sub-term generation module: used to fit, update and generate multiple nonlinear Gaussian sub-terms in the mixed Gaussian model;
[0069] Pseudo-matrix construction module: used to introduce square root volume Klaman filter as a sub-filter of Gaussian sum based on nonlinear Gaussian sub-item, and construct the pseudo-matrix of square root volume Kalman sub-filter;
[0070] Define the estimated trust factor module: It is used to define the trust factor of nonlinear non-Gaussian systems, estimate the trust factor of nonlinear non-Gaussian systems using the Sage-Husa adaptive filtering algorithm, and design a Kalman filtering framework based on credibility and improved Gaussian and square root volume in combination with credibility theory;
[0071] Real-time pose estimation module: It is used to combine the hunter prey optimization algorithm and the wild dog optimization algorithm to obtain HPO-DOA, and use the credibility-based and improved Gaussian and square root volume Kalman filter framework to obtain the real-time pose results of the UAV during the landing process in harsh ocean environments.
[0072] Beneficial effects of the present invention:
[0073] The present invention has the following advantages over the traditional nonlinear non-Gaussian filtering pose estimation method: the Gaussian sum algorithm is improved by a new noise Gaussian sub-item update method and a Gaussian component fusion strategy, which reduces the computational complexity compared to the traditional Gaussian sum algorithm. The square root volume Kalman filtering algorithm is used to reduce the computational burden and obtain higher computational efficiency compared to CKF, while ensuring the non-negative definiteness of the covariance matrix and avoiding the divergence of the filter. Considering that the actual system is more of a nonlinear non-Gaussian system, the linear Gaussian credibility theory is elevated to the nonlinear non-Gaussian credibility theory, so that the theory is more practical. The hunter prey optimization algorithm and the wild dog optimization algorithm are combined to obtain a new and better trust factor estimation method and a new and better performance UAV pose estimation method based on credibility theory and improved Gaussian and square root volume Kalman filtering. BRIEF DESCRIPTION OF THE DRAWINGS
[0074] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings required for use in the embodiments or the prior art description are briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative work.
[0075] Figure 1 It is a flow chart of the method of the present invention.
[0076] Figure 2 This is a calculation flow chart of the HPO-DOA algorithm proposed in the present invention. DETAILED DESCRIPTION
[0077] 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.
[0078] like Figure 1 As shown, the UAV pose estimation method based on credibility and improved Gaussian and technology includes the following steps:
[0079] Fitting updates generate multiple nonlinear Gaussian sub-terms in the mixed Gaussian model;
[0080] Based on the nonlinear Gaussian sub-term, the square root volume Klaman filter is introduced as the sub-filter of the Gaussian sum, and the pseudo matrix of the square root volume Kalman sub-filter is constructed;
[0081] Define the trust factor of nonlinear non-Gaussian system, use Sage-Husa adaptive filtering algorithm to estimate the trust factor of nonlinear non-Gaussian system, and design a Kalman filtering framework based on credibility and improved Gaussian and square root cubature combined with credibility theory;
[0082] The hunter-prey optimization algorithm and the wild dog optimization algorithm are combined to obtain HPO-DOA, and the real-time pose results of the UAV during the landing process in the harsh ocean environment are obtained by using the credibility-based and improved Gaussian and square root cubature Kalman filter framework.
[0083] It should be further explained that, in a specific implementation process, the updating process of the nonlinear Gaussian sub-item is as follows:
[0084] N k|k Gaussian subterms according to or Arranged from large to small, and are the weights of the Gaussian sub-term of the ith measurement and process noise at time k, respectively. and is the corresponding standard deviation, and then the current value of the noise is compared with the r in the mixed Gaussian model k or q k Gaussian sub-terms are compared one by one. If the measured noise v k or process noise w k,k-1 and the mean of the i-th Gaussian subterm or The difference between them is less than δ times the standard deviation of the Gaussian subterm or Then the Gaussian subterm is v k ork,k-1 Updated, the rest of the nonlinear Gaussian sub-terms remain unchanged.
[0085] It should be further explained that, in the specific implementation process, the Gaussian fitting update equation of the measurement noise is:
[0086]
[0087] in is the weight of the ith Gaussian sub-term of the measurement noise at time k+1, τ r , τ is the learning rate,
[0088] Gaussian subterm and x k When matching Takes 1, otherwise 0, is the posterior error covariance of the i-th component, is the estimated measurement noise of the i-th component at time k+1 and k; k and k+1 in the lower right corner represent the time, and i in the upper right corner represents the i-th sub-item.
[0089] It should be further explained that, in the specific implementation process, the Gaussian fitting update equation of the process noise is:
[0090]
[0091] in is the weight of the i-th Gaussian sub-term of the process noise at time k+1, τ q is the learning rate.
[0092] It needs to be further explained that in the specific implementation process, In order to avoid falling into the situation where it is always 0, the discriminant is used for discrimination. If the discriminant is established, the Gaussian sub-item is deleted. The discriminant is as follows:
[0093] and
[0094] and
[0095] In the formula, and are the initial weight and initial standard deviation respectively. The subscript k indicates the kth moment.
[0096] It should be further explained that, in a specific implementation process, the pseudo matrix of the square root volume Kalman sub-filter is generated as follows:
[0097] After calculating the estimated mean and covariance of each Gaussian sub-item at time k, first retain all Gaussian components with magnitude greater than G and delete all other components. The formula is as follows:
[0098]
[0099] in is the filter weight corresponding to the Gaussian sub-item
[0100] Among the remaining Gaussian components, find the one with the largest weight:
[0101]
[0102] And find all Gaussian components that meet the conditions according to the following criteria:
[0103]
[0104] in, ; The threshold U′ is set to 0.04, is the error covariance matrix of the i-th Gaussian component.
[0105] These Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are as follows:
[0106]
[0107] in, is the estimated value of the state variable of the i-th Gaussian component, is the error covariance matrix estimate of the i-th Gaussian component.
[0108] Delete all Gaussian components that have been fused, repeat the above steps until all Gaussian components have completed the operation, and finally keep only the first S sub-Gaussian distributions with large weights, and renormalize the corresponding weights to reduce the computational complexity;
[0109] Finally, the pseudo-observation matrix of the square root volume Kalman subfilter is:
[0110]
[0111] in, is the cross-covariance matrix of the j-th Gaussian component, is the predicted covariance matrix of the jth Gaussian component
[0112] The pseudo-state transfer matrix is:
[0113]
[0114] in, is the pseudo observation matrix, is the pseudo observation matrix.
[0115]
[0116] in, is the square root of the prediction error covariance matrix of the jth Gaussian component, is the square root of the dth process noise covariance matrix item.
[0117] Thus the Gaussian subsystem is written in linear Gaussian form:
[0118] x k =F k,k-1 x k-1 +w k,k-1
[0119] z k =H k x k +v k
[0120] where x k is the system state vector, F k,k-1 is the linear state transfer function, z k is the observation vector, H k is the measurement function.
[0121] The iterative process of the square root volume Kalman subfilter is simplified to:
[0122]
[0123] The number in the upper right corner represents the Gaussian component, and the number k in the lower right corner represents time. represents the filter gain of the i-th component, z k Represents new information.
[0124] Optionally, the formula for defining the trust factor of a nonlinear non-Gaussian system is as follows:
[0125]
[0126] In the formula, t(k) is the trust factor, yes The 2-norm of The value range of is (0, +∞), and the value range of t(k) is 0~1, which is used to measure the closeness of FMSE and TMSE of each sub-filter. At the same time, t(k) will be affected by The impact of The larger is , the smaller is t(k).
[0127] It should be further explained that, in the specific implementation process, the process of estimating the trust factor of the nonlinear non-Gaussian system by the Sage-Husa adaptive filtering algorithm is as follows:
[0128] when hour,
[0129]
[0130] in and are the calculated measurement error covariance and actual measurement error covariance of the i-th component, respectively, where They represent Gaussian sub-terms satisfying The process noise covariance and observation noise covariance of Sage-Husa adaptive Kalman filtering algorithm are shown in hour, Converge to Converge to Right now is the convergence estimate of the Sage-Husa adaptive Kalman filter algorithm. In summary, by adjusting To satisfy:
[0131]
[0132] The same estimation effect as the Sage-Husa adaptive Kalman filter algorithm is guaranteed, and the constraints This ensures that the estimation process will not have noise covariance problems.
[0133] It should be further explained that, in the specific implementation process, the process of the HPO-DOA algorithm is as follows:
[0134] In the defined interval [x min ,x max ] Randomly generate population individuals;
[0135] Constructing the fitness function Substitute the population individuals as the noise covariance corresponding to each Gaussian sub-item into the Kalman filter to obtain the corresponding And calculate the fitness value;
[0136] distinguishing between hunters and prey among individuals in a population;
[0137] Update individuals in the population;
[0138] Calculate the fitness value of each individual;
[0139] 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 This results in an accurate non-Gaussian noise estimate; otherwise, it is difficult to distinguish hunters and prey in the population individuals.
[0140] Calculate the trust factor t(k).
[0141] UAV pose estimation system based on credibility and improved Gaussian technology, including:
[0142] Gaussian sub-term generation module: used to fit, update and generate multiple nonlinear Gaussian sub-terms in the mixed Gaussian model;
[0143] Pseudo-matrix construction module: used to introduce square root volume Klaman filter as a sub-filter of Gaussian sum based on nonlinear Gaussian sub-item, and construct the pseudo-matrix of square root volume Kalman sub-filter;
[0144] Define the estimated trust factor module: It is used to define the trust factor of nonlinear non-Gaussian systems, estimate the trust factor of nonlinear non-Gaussian systems using the Sage-Husa adaptive filtering algorithm, and design a Kalman filtering framework based on credibility and improved Gaussian and square root volume in combination with credibility theory;
[0145] Real-time pose estimation module: It is used to combine the hunter prey optimization algorithm and the wild dog optimization algorithm to obtain HPO-DOA, and use the credibility-based and improved Gaussian and square root volume Kalman filter framework to obtain the real-time pose results of the UAV during the landing process in harsh ocean environments.
[0146] 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.
[0147] 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. A UAV pose estimation method based on credibility and improved Gaussian technology, characterized in that: The method comprises the following steps: Fitting updates generate multiple nonlinear Gaussian sub-terms in the mixed Gaussian model; Based on the nonlinear Gaussian sub-term, the square root volume Klaman filter is introduced as the sub-filter of the Gaussian sum, and the pseudo matrix of the square root volume Kalman sub-filter is constructed; Define the trust factor of nonlinear non-Gaussian system, use Sage-Husa adaptive filtering algorithm to estimate the trust factor of nonlinear non-Gaussian system, and design a Kalman filtering framework based on credibility and improved Gaussian and square root cubature combined with credibility theory; The hunter-prey optimization algorithm and the wild dog optimization algorithm are combined to obtain HPO-DOA, and the real-time pose results of the UAV during the landing process in the harsh ocean environment are obtained by using the credibility-based and improved Gaussian and square root cubature Kalman filter framework.
2. The method for estimating the posture of a UAV based on credibility and improved Gaussian technology according to claim 1, characterized in that: The updating process of the nonlinear Gaussian sub-item is as follows: N k|k Gaussian subterms according to or Arranged from large to small, and are the weights of the Gaussian sub-term of the ith measurement and process noise at time k, respectively. and is the corresponding standard deviation, and then the current value of the noise is compared with the r in the mixed Gaussian model k or q k Gaussian sub-terms are compared one by one. If the measured noise v k or process noise w k,k-1 and the mean of the i-th Gaussian subterm or The difference between them is less than δ times the standard deviation of the Gaussian subterm or Then the Gaussian subterm is v k or k,k-1 Updated, the rest of the nonlinear Gaussian sub-terms remain unchanged.
3. The method for estimating the posture of a UAV based on credibility and improved Gaussian technology according to claim 2, characterized in that: The Gaussian fit update equation for the measurement noise is: in is the weight of the ith Gaussian sub-term of the measurement noise at time k+1, τ r , τ is the learning rate, Gaussian subterm and x k When matching Takes 1, otherwise 0, is the posterior error covariance of the i-th component, is the estimated measurement noise of the i-th component at time k+1 and k; k and k+1 in the lower right corner represent the time, and i in the upper right corner represents the i-th sub-item.
4. The method for estimating the posture of a UAV based on credibility and improved Gaussian technology according to claim 2, characterized in that: Gaussian fit update equation for process noise: in is the weight of the i-th Gaussian sub-term of the process noise at time k+1, τ q is the learning rate.
5. The method for estimating the posture of a UAV based on credibility and improved Gaussian technology according to claim 4, characterized in that: To make In order to avoid falling into the situation where it is always 0, the discriminant is used for discrimination. If the discriminant is established, the Gaussian sub-item is deleted. The discriminant is as follows: and and In the formula, and are the initial weight and initial standard deviation respectively, and the subscript k represents the kth moment.
6. The method for estimating the posture of a UAV based on credibility and improved Gaussian technology according to claim 1, characterized in that: The pseudo matrix generation process of the square root volume Kalman sub-filter is as follows: After calculating the estimated mean and covariance of each Gaussian sub-item at time k, first retain all Gaussian components with magnitude greater than the unit matrix G and delete all other components. The formula is as follows: in is the filter weight corresponding to the Gaussian sub-item; Among the remaining Gaussian components, find the component with the largest weight: And find all Gaussian components that meet the conditions according to the following criteria: in, The threshold U′ is set to 0.04, is the error covariance matrix of the i-th Gaussian component, k|k represents the posterior, and k|k-1 represents the prior; The Gaussian components are combined into a Gaussian distribution by weighted averaging, and its mean and covariance are as follows: in, is the estimated value of the state variable of the i-th Gaussian component, is the error covariance matrix estimate of the i-th Gaussian component; Delete all Gaussian components that have been fused, repeat the above steps until all Gaussian components have completed the operation, and finally keep only the first S sub-Gaussian distributions with large weights, and renormalize the corresponding weights to reduce the computational complexity; It is concluded that the pseudo-observation matrix of the square root volume Kalman subfilter is: in, is the cross-covariance matrix of the j-th Gaussian component, is the predicted covariance matrix of the jth Gaussian component; The pseudo-state transfer matrix is: in, is the pseudo observation matrix, is the pseudo observation matrix; in, is the square root of the prediction error covariance matrix of the jth Gaussian component, is the square root of the dth process noise covariance matrix item; Thus the Gaussian subsystem is written in linear Gaussian form: x k =F k,k-1 x k-1 +w k,k-1 z k =H k x k +v k where x k is the system state vector, F k,k-1 is the linear state transfer function, z k is the observation vector, H k is the measurement function; The iterative process of the square root volume Kalman subfilter is simplified to: The numbers i and j in the upper right corner represent the i-th and j-th Gaussian components, and the k in the lower right corner represents time. represents the filter gain of the i-th component, z k Represents new information.
7. The method for estimating the posture of a UAV based on credibility and improved Gaussian technology according to claim 1, characterized in that: The formula for defining the trust factor of a nonlinear non-Gaussian system is as follows: In the formula, t(k) is the trust factor, is the calculated a posteriori error covariance of the i-th component, is the actual a posteriori error covariance of the ith component, yes The 2-norm of The value range of is (0, +∞), and the value range of t(k) is 0~1, which is used to measure the closeness of FMSE and TMSE of each sub-filter. At the same time, t(k) will be affected by The impact of The larger is , the smaller is t(k).
8. The method for estimating the posture of a UAV based on credibility and improved Gaussian technology according to claim 1, characterized in that: The process of the Sage-Husa adaptive filtering algorithm estimating the trust factor of a nonlinear non-Gaussian system is as follows: when hour, in and are the calculated measurement error covariance and the actual measurement error covariance of the i-th component, respectively, They represent Gaussian sub-terms satisfying The process noise covariance and observation noise covariance of Sage-Husa adaptive Kalman filtering algorithm are shown in hour, Converge to Converge to Right now is the convergence estimate of the Sage-Husa adaptive Kalman filter algorithm. In summary, by adjusting To satisfy: The same estimation effect as the Sage-Husa adaptive Kalman filter algorithm is guaranteed, and the constraints This ensures that the estimation process will not have noise covariance problems.
9. The method for estimating the posture of a UAV based on credibility and improved Gaussian technology according to claim 1, 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; Constructing the fitness function Substitute the population individuals as the noise covariance corresponding to each Gaussian sub-item into the Kalman filter to obtain the corresponding And calculate the fitness value; distinguishing between hunters and prey among individuals in a population; Update individuals in the population; Calculate the fitness value of each individual; 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 This results in an accurate non-Gaussian noise estimate; otherwise, it is difficult to distinguish hunters and prey in the population individuals. Calculate the trust factor t(k).
10. UAV pose estimation system based on credibility and improved Gaussian technology, characterized in that: include: Gaussian sub-term generation module: used to fit, update and generate multiple nonlinear Gaussian sub-terms in the mixed Gaussian model; Pseudo-matrix construction module: used to introduce square root volume Klaman filter as a sub-filter of Gaussian sum based on nonlinear Gaussian sub-item, and construct the pseudo-matrix of square root volume Kalman sub-filter; Define the estimated trust factor module: It is used to define the trust factor of nonlinear non-Gaussian systems, estimate the trust factor of nonlinear non-Gaussian systems using the Sage-Husa adaptive filtering algorithm, and design a Kalman filtering framework based on credibility and improved Gaussian and square root volume in combination with credibility theory; Real-time pose estimation module: It is used to combine the hunter prey optimization algorithm and the wild dog optimization algorithm to obtain HPO-DOA, and use the credibility-based and improved Gaussian and square root volume Kalman filter framework to obtain the real-time pose results of the UAV during the landing process in harsh ocean environments.