A method for estimating the state of a UAV, a readable storage medium and a navigation device

Abstract

The application discloses a UAV state estimation method, a readable storage medium and a navigation device, and belongs to the technical field of UAV navigation. The method comprises the following steps: establishing a linear state space model with multiple cluster measurement noises, modeling the measurement noises as a multivariate Gaussian distribution, and introducing measurement noise covariance matrix coefficients; performing joint prior updating based on the posterior information at the previous moment to obtain prior estimation of the state, state covariance, measurement noise covariance matrix coefficients and generalized inverse Gaussian (GIG) distribution parameters; pre-clustering the measurement values; using the EM algorithm to realize online adaptive clustering, which can dynamically identify and divide unknown outdoor scene noises; performing joint posterior updating without fixed point iteration, so as to obtain the updated measurement noise covariance matrix; updating the state, and outputting the UAV state. Through accurate noise modeling, online clustering and adaptive adjustment of the measurement noise covariance matrix, the positioning accuracy and system robustness of the UAV are significantly improved.

Description

Technical Field

[0001] This application belongs to the field of unmanned aerial vehicle (UAV) navigation technology, and particularly relates to a UAV state estimation method, a readable storage medium, and a navigation device. Background Technology

[0002] Accurate and reliable positioning is crucial for drones operating in open outdoor environments. Visual inertial odometry (VIO) has become one of the mainstream positioning solutions for drones due to its ability to achieve accurate and reliable positioning without relying on external global information.

[0003] While numerous mature VIO (Vacuum-Independent Localization) methods exist, they suffer from a heavy reliance on computational resources. In contrast, the Multi-State Constrained Kalman Filter (MSCKF) method is widely used for UAV localization due to its high computational efficiency and satisfactory accuracy. However, in dynamically changing open outdoor environments, the noise distribution of visual feature points in VIO not only varies over time but also differs between different measurement clusters. For example, different observation distances, viewpoints, lighting, and texture conditions can lead to significant and time-varying differences in noise distribution between different types of feature points. This results in a significant mismatch between the actual and standard Measurement Noise Covariance Matrix (MNCM), introducing measurement errors into state estimation through the MNCM, thereby reducing the localization accuracy and robustness of VIO.

[0004] Existing methods assume that all measurements have the same noise variance distribution. However, in open outdoor environments, the measurement noise of different feature point clusters is not exactly distributed. Estimating the noise covariance using an ideal model will lead to a mismatch between the estimated measurement noise covariance matrix and the actual value, thereby reducing the positioning accuracy of the UAV.

[0005] Furthermore, existing VIO methods based on robust filters can handle measurements with different noise variances and dynamically adjust weights, but they cannot continuously track the noise variance of measurements, can only process the current measurement, and discard historical information, resulting in unstable positioning performance. Summary of the Invention

[0006] This application aims to address the problems existing in the prior art by providing a new method for UAV state estimation, a readable storage medium, and a navigation device. This method can continuously process and track measurements with different noise variances by clustering measurement values ​​online, thereby improving the accuracy and robustness of UAV positioning when operating in open outdoor scenes with time-varying noise.

[0007] In a first aspect, embodiments of this application provide a method for estimating the state of an unmanned aerial vehicle (UAV), including: establishing a state space model, joint prior update, pre-clustering, adaptive online clustering based on the EM algorithm, and joint posterior update;

[0008] The above state-space model is a linear state-space model with multiple clusters of measurement noise;

[0009] The aforementioned joint prior update refers to calculating the prior estimates of the current state, state covariance matrix, measurement noise covariance matrix coefficients, and GIG distribution parameters based on the state transition matrix, process noise driving matrix, process noise covariance matrix, and posterior estimates of the state, state covariance matrix, and GIG distribution parameters of the previous time step.

[0010] The above pre-clustering is based on the measurement values ​​of the inertial measurement unit (IMU), as well as the prior estimates of the state, state covariance matrix, and GIG distribution parameters, to cluster the measurement values.

[0011] The above adaptive online clustering based on the EM algorithm outputs the cluster sequence number to which each measurement value belongs;

[0012] The aforementioned joint posterior update utilizes the cluster sequence number, the observation matrix of the state-space model, and the prior estimates of the state, state covariance matrix, measurement noise covariance matrix coefficients, and GIG distribution parameters to calculate the posterior GIG parameters and posterior measurement noise covariance matrix coefficients, further obtaining the measurement noise covariance matrix. Then, Kalman filtering is used for posterior state update, thereby obtaining the optimized UAV state.

[0013] In some implementations, pre-clustering includes:

[0014] Calculate the prior mean of the coefficients of the measurement noise covariance matrix:

[0015]

[0016] in, Prior mean of the coefficients of the measurement noise covariance matrix and The GIG distribution parameters at the current time. and The prior estimate;

[0017] Calculate the test statistic for the m-th measurement belonging to the n-th cluster:

[0018]

[0019] in, This represents the m-th measurement information. yes The covariance matrix, The conditional distribution can be obtained in the following way:

[0020]

[0021] in, Represents the m-th measurement. This represents the observation matrix corresponding to the m-th measurement. Represents x k The prior estimate at time k, represent The conditional probability density distribution, Represents a Gaussian distribution. P represents k The prior estimate at time k;

[0022] Calculate the test statistic Compare with the chi-square threshold to determine if the test statistic... If the value is greater than the chi-square threshold, the probability that the m-th measurement belongs to the n-th cluster is considered to be greater than the chi-square threshold. If the calculated test statistic is less than the chi-square threshold, then adaptive online clustering based on the EM algorithm is performed.

[0023] In some implementations, the linear state-space model with multi-cluster measurement noise is as follows:

[0024]

[0025] Where, x k and z k Φ represents the state vector and measurement vector at time k, respectively; k G k and H k The matrix consists of, in order, the state transition matrix, the process noise driving matrix, and the observation matrix, with process noise ω. k It is zero-mean Gaussian noise, i.e. Q k Let v be the process noise covariance matrix. k To measure noise, it is represented as:

[0026]

[0027] Among them, R k Represents the complete measurement vector z k The measurement noise covariance matrix, where Diag{·} denotes a block diagonal matrix. This represents the measurement noise covariance matrix of the nth cluster, where N represents the total number of clusters. This represents the coefficients of the measurement noise covariance matrix to be calculated. It is the identity matrix associated with the nth cluster, and its dimension is... same.

[0028] In some implementations, the joint prior update is:

[0029]

[0030] in, and The state x at the previous moment k State covariance matrix P k Measurement noise covariance matrix coefficients, GIG distribution parameters and The posterior estimate, and At the current moment, P k , and The prior estimate, where the superscript n represents the nth cluster, Φ k G k With Q k ρ represents the state transition matrix, process noise driving matrix, and process noise covariance matrix, respectively, and ρ is the forgetting factor.

[0031] In some implementations, adaptive online clustering based on the EM algorithm is divided into E-steps and M-steps, which are iterated repeatedly until the proportion of each cluster in the overall measurement is reached. The probability that the m-th measurement belongs to the n-th cluster Convergence, the m-th measurement will be assigned to In the highest cluster.

[0032] In some implementations, the E-step of the adaptive online clustering algorithm based on the EM algorithm includes:

[0033] Calculate latent variables It indicates whether the m-th measurement belongs to the n-th cluster:

[0034]

[0035] Calculate the probability that the m-th measurement belongs to the n-th cluster.

[0036]

[0037] Where E(·) represents expectation, N represents the proportion of each cluster in the overall measurement. m Let m be the number of candidate clusters to which the m-th measurement may belong.

[0038] In some implementations, the M-step of the adaptive online clustering algorithm based on the EM algorithm includes:

[0039] The M-step of the adaptive online clustering algorithm based on the EM algorithm includes:

[0040] To calculate the proportion of each cluster in the entire measurement:

[0041]

[0042] Where M represents the total number of current measurements.

[0043] In some implementations, calculating the posterior GIG parameters and the coefficients of the posterior measurement noise covariance matrix includes:

[0044] Calculate the transition amount

[0045]

[0046] in, The measurement vector is composed of all measurements of the nth cluster. yes The corresponding observation matrix, It is an identity matrix, whose dimension is the sum of the dimensions of all measurements in the nth cluster. The state x at the previous moment k State covariance matrix P k The prior estimate. `trace` represents the trace of the matrix. This is for intermediate computational costs.

[0047] Calculate the posterior estimates of the GIG distribution parameters:

[0048]

[0049] in, and The GIG distribution parameters at the current time. and The posterior estimate, and It is the current moment. and The prior estimate, where the superscript n represents the nth cluster. express Dimensions.

[0050] Calculate the posterior estimates of the coefficients of the measurement noise covariance matrix:

[0051]

[0052] in, Posterior mean of the coefficients of the measurement noise covariance matrix.

[0053] Secondly, embodiments of this application provide a readable storage medium storing a computer program, which, when executed, performs the UAV state estimation method described above.

[0054] Thirdly, embodiments of this application provide a navigation device, including a memory, a processor, and a terminal program stored in the memory and executable in the processor. The terminal program includes steps corresponding to the above-described UAV state estimation method.

[0055] As can be seen from the above technical solution, the beneficial effects of this application are as follows:

[0056] 1. The method of this invention establishes a linear state-space model with multi-cluster measurement noise and models the measurement noise vector as a multivariate Gaussian distribution. By introducing measurement noise covariance coefficients into the measurement noise vector modeling, it achieves differentiation of UAV observations under different distances, viewing angles, and lighting conditions, thus improving the accurate description of measurement noise. Based on the posterior generalized inverse Gaussian (GIG) distribution parameters of the previous time step and introducing a forgetting factor ρ, a prior model of the measurement noise covariance matrix coefficients is established when the trend of measurement noise variation is unknown and the multi-cluster noise covariance matching coefficients are slowly varying parameters. By performing a chi-square test on the measurement values ​​for pre-clustering, the computational cost is reduced, and autonomous cluster generation is achieved. The EM algorithm enables online clustering of unknown measurement noise in open outdoor scenes. Through joint posterior updates without fixed-point iterations, the measurement noise distribution of each cluster is tracked and the measurement noise covariance matrix is ​​automatically adjusted, significantly improving the positioning accuracy and robustness of the UAV.

[0057] 2. The readable storage medium of this application stores the above-mentioned state estimation method in the form of a computer program, providing a storage medium that can be applied to various electronic devices. In this way, the state estimation method can also be applied to different usage scenarios, expanding the application scope of UAV state estimation and facilitating its application.

[0058] 3. The navigation device of this application stores the steps corresponding to the above-mentioned state estimation method in its memory. When the processor is working, the navigation device can apply the state estimation method to quickly output the position and attitude of the UAV, providing hardware support for positioning of various intelligent agents or unmanned vehicles, and is suitable for navigation and positioning in multiple fields. Attached Figure Description

[0059] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced one by one below. Obviously, the accompanying drawings described below are some embodiments of this application. For those skilled in the art, other embodiments and drawings can be obtained based on these drawings without creative effort. The flowcharts shown in the accompanying drawings are merely illustrative and do not necessarily include all content and operations / steps, nor do they necessarily have to be performed in the described order. For example, some operations / steps can be decomposed, while some operations / steps can be combined or partially combined. Therefore, the actual execution order may change according to the actual situation.

[0060] Figure 1 A schematic flowchart of an embodiment of the UAV state estimation method of the present invention is shown;

[0061] Figure 2 A schematic diagram of an embodiment of the components of the measurement noise vector of the present invention is shown;

[0062] Figure 3 A schematic diagram of the actual motion trajectory of the UAV of the present invention is shown;

[0063] Figure 4 This diagram illustrates a comparison between the actual pixel error and the estimated pixel error curves of the present invention.

[0064] Figure 5 The root mean square error curves of the positions of all methods in the simulation of this invention are shown.

[0065] Figure 6 The root mean square error curves of the positions of all methods in the simulation of this invention are shown. Detailed Implementation

[0066] The technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. The detailed description of the embodiments of this application provided in the drawings is not intended to limit the scope of the claimed application. The described embodiments are only a part of the embodiments of this application, not all of them. Based on the embodiments in this application, they can be arranged and designed in various different configurations. All other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0067] This application is described below with reference to the accompanying drawings and specific embodiments:

[0068] Please refer to Figure 1 According to a first aspect of this application, a method for estimating the state of a UAV is provided, comprising:

[0069] S1: Establish a linear state-space model with multiple clusters of measurement noise:

[0070]

[0071] Where, x k and z k Let x represent the state vector and measurement vector at time k, respectively, as follows: k The estimation includes errors in attitude, velocity, and position; deviations in the gyroscope and accelerometer readings; and the cloned state within the sliding window. The initial values ​​for attitude, velocity, and position estimation errors are all 0 (in rad / s, m). The deviations in the three-axis gyroscope and accelerometer readings are initially set to 1.9393e-3 rad / s and 3.0000e-2 m / s, respectively. 2 Here, the sliding window refers to the storage of historical camera measurements and poses within a fixed time period, which is called the sliding window, since measurement updates require the use of historical camera measurements and poses. The stored historical camera poses are called clone states. k G k and H k The matrix consists of, in order, the state transition matrix, the process noise driving matrix, and the observation matrix, with process noise ω. k It is zero-mean Gaussian noise, i.e. Q k The process noise covariance matrix is ​​initially set as follows:

[0072]

[0073] v k To measure noise, it is represented as:

[0074]

[0075] Among them, R k Represents the complete measurement vector z k The measurement noise covariance matrix, where Diag{·} denotes a block diagonal matrix. This represents the measurement noise covariance matrix of the nth cluster, where N represents the total number of clusters. In this embodiment, N = 15. This represents the coefficients of the measurement noise covariance matrix to be calculated. It is the identity matrix associated with the nth cluster, and its dimension is... same.

[0076] S2: Joint prior update, based on the state transition matrix Φ of the state-space model. k Process noise driving matrix G k The covariance matrix Q of process noise kAnd the posterior estimates of the previous state, the state covariance matrix, and the GIG distribution parameters. and The prior estimates of the previous state, state covariance matrix, measurement noise covariance matrix coefficients, and GIG distribution parameters were calculated. and The specific calculation formula is as follows:

[0077]

[0078] in, and The state x at the previous moment k State covariance matrix P k Measurement noise covariance matrix coefficients, GIG distribution parameters The posterior estimate, and It is the current time x k P k , and The prior estimates, where the superscript n represents the corresponding nth cluster, and the GIG distribution parameters. and The initial values ​​of α0 = 1, β0 = 6.75, γ0 = -0.5, and Φ are given. k G k With Q k These represent the state transition matrix, the process noise driving matrix, and the process noise covariance matrix, respectively. ρ is the forgetting factor, with a value of 0.95-0.99, such as 0.99.

[0079] S3: Please refer to Figure 2 Based on the measurements, states, state covariance matrix, and prior estimates of GIG distribution parameters from the inertial measurement unit (IMU), the measurements are clustered, including:

[0080] S31: Calculate the prior mean of the coefficients of the measurement noise covariance matrix:

[0081]

[0082] in, Prior mean of the coefficients of the measurement noise covariance matrix and The GIG distribution parameters at the current time. and The prior estimate;

[0083] S32: Calculate the test statistic that the m-th measurement belongs to the n-th cluster.

[0084]

[0085] in, This represents the m-th measurement information. yes The covariance matrix, The conditional distribution can be obtained in the following way:

[0086]

[0087] in, Represents the m-th measurement. This represents the observation matrix corresponding to the m-th measurement. Represents x k The prior estimate at time k, represent The conditional probability density distribution, Represents a Gaussian distribution. P represents k The prior estimate at time k;

[0088] Calculate the test statistic Compare with the chi-square threshold to determine if the test statistic... If the value is greater than the chi-square threshold, the probability that the m-th measurement belongs to the n-th cluster is considered to be greater than the chi-square threshold. If the calculated test statistic is less than the chi-square threshold, then adaptive online clustering based on the EM algorithm is performed.

[0089] S4: Adaptive online clustering based on the EM algorithm:

[0090] S41: EM algorithm E-step, including:

[0091] S411: Calculate latent variables If the m-th measurement belongs to the n-th cluster Take 1, otherwise

[0092] S412: Calculate the probability that the m-th measurement belongs to the n-th cluster.

[0093]

[0094] Where E(·) represents expectation, N represents the proportion of each cluster in the overall measurement. m Let m be the number of candidate clusters to which the m-th measurement may belong.

[0095] S42: M steps of the EM algorithm, calculating the proportion of each cluster in the entire measurement:

[0096]

[0097] Where M represents the total number of current measurements.

[0098] S43: Iterate through the E-step and M-step until the proportion of each cluster in the overall measurement is reached. The probability that the m-th measurement belongs to the n-th cluster Convergence, the m-th measurement will be assigned to In the highest cluster, output the cluster sequence number to which each measurement value belongs;

[0099] S5: Joint posterior update, using the cluster sequence number, the observation matrix of the state-space model, and the prior estimates of the state, state covariance matrix, measurement noise covariance matrix coefficients, and GIG distribution parameters, employs a clustered adaptive Kalman filter algorithm to calculate the posterior GIG parameters and the posterior measurement noise covariance matrix coefficients, including:

[0100] S51: Calculate the transition amount

[0101]

[0102] in, The measurement vector is composed of all measurements of the nth cluster. yes The corresponding observation matrix, It is an identity matrix, whose dimension is the sum of the dimensions of all measurements in the nth cluster. The state x at the previous moment k State covariance matrix P k The prior estimate, where trace represents the trace of the matrix. This is for intermediate computational costs.

[0103] S52: Calculate the posterior estimates of the GIG distribution parameters:

[0104]

[0105] in, and The GIG distribution parameters at the current time. and The posterior estimate, and It is the current moment. and The prior estimate, where the superscript n represents the nth cluster. express Dimensions.

[0106] S53: Calculate the posterior estimates of the coefficients of the measurement noise covariance matrix:

[0107]

[0108] in, The posterior mean of the coefficients of the noise covariance matrix is ​​measured, and step S5 is iterated repeatedly within each cluster until... Convergence, at this point It will eventually converge to the coefficients of the measurement noise covariance matrix.

[0109] S54: Constructing the Measurement Noise Covariance Matrix And Kalman filtering is used for posterior state updates:

[0110]

[0111] Among them, K k For Kalman gain.

[0112] S55: Outputs the state of the UAV, the state covariance matrix, and the GIG distribution parameters for each cluster.

[0113] This embodiment designs a simulation experiment to simulate a real UAV scenario, comparing the UAV state estimation method proposed in this invention with three existing adaptive and robust filtering methods: VB-MSCKF, HKF, and ANGIG-KF. The parameter settings for all methods are shown in Table 1, where CAMSCKF represents the parameter settings for the method proposed in this invention, and α0, β0, and γ0 are the GIG distribution parameters. and The initial value of N is... max This represents the maximum value of the cluster.

[0114] Table 1 Experimental parameter settings

[0115]

[0116] In this embodiment, the implementation scheme is as follows: The simulation experiment platform is a laptop computer equipped with an 11th Gen Intel(R) Core(TM) i7-11800H@2.30GHz processor, and the simulation software is Matlab. The implementation steps are as follows:

[0117] Step 1: Generating the drone's motion trajectory. This involves generating a trajectory showing the drone moving in a circular motion on a horizontal plane while simultaneously moving vertically up and down along that plane. Figure 3 As shown.

[0118] Step 2: Generate noisy data from airborne sensors such as the camera and inertial measurement unit (IMU). The frequencies of the IMU and camera are 100Hz and 10Hz, respectively. The random walk coefficients of the gyroscope and accelerometer in the IMU, as well as the random walk coefficients of the constant deviation, are set to be consistent with the parameter configuration given in the publicly available Euroc dataset. Figure 4 The True-sigma1 and True-sigma2 parameters set the pixel errors of camera feature points. These errors follow two Gaussian distributions with time-varying variances. This embodiment simulates an extreme case where the pixel errors of the feature points do not match the standard pixel errors, and even their error distribution trends do not change consistently over time. The total simulation time is 30,000 steps (i.e., 300 seconds).

[0119] Step 3: Using the noisy airborne sensor data generated in Step 2, substitute it into S1-S5 above to obtain the estimated UAV position and attitude, as well as the pixel error of the estimated camera feature points. Compare the estimated position and attitude with the actual UAV trajectory generated in Step 1 to obtain the root mean square error of position (PRMSE) and root mean square error of attitude (ORMSE). The curves of the root mean square error of position and the root mean square error of attitude are shown below. Figure 5 As shown in the diagram, the estimated pixel error of the camera feature points is compared with the pixel error of the camera feature points given in the second step. A schematic diagram of the actual pixel error and the estimated pixel error is shown below. Figure 4 As shown, CAMSCKF-sigma1, CAMSCKF-sigma2, and ANGIG-sigma demonstrate the comparison of the pixel error tracking performance of the proposed method and the compared method at feature points.

[0120] It is evident that, in scenarios with extremely non-uniform noise variations, the UAV state estimation method proposed in this invention exhibits higher robustness and accuracy compared to other algorithms. Even when feature points have different noise distributions and the noise changes abruptly over time, the UAV state estimation method proposed in this invention still demonstrates excellent robustness and accuracy. Because Tra-MSCKF and HKF cannot adaptively adjust the measurement noise covariance matrix, and VB-MSCKF is unstable due to large approximation errors, these algorithms perform poorly in scenarios where feature point noise varies with time and space. This embodiment focuses on comparing the visualization results of tracking pixel errors using ANGIGKF and the UAV state estimation method proposed in this invention, as ANGIGKF has the ability to adaptively adjust the measurement noise covariance matrix. Figure 6As shown, ANGIG-KF exhibits significant inaccuracies in estimating pixel errors because it cannot individually adjust the coefficients of the measurement noise covariance matrix for points with different noise distribution characteristics, leading to a decrease in its positioning accuracy and robustness. In contrast, the UAV state estimation method proposed in this invention, after a short iterative process, gradually converges to a value close to the true pixel error for points with different noise distribution characteristics, thus achieving the best results.

[0121] A second aspect of this application provides a readable storage medium storing a computer program. When executed, the computer program performs the autonomous positioning method as described in any of the above embodiments. If the integrated module / unit corresponding to this method is implemented as a software functional unit and sold or used as an independent product, it can be stored in a readable storage medium. Based on this understanding, all or part of the processes of the methods described in the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in the readable storage medium. When executed by a processor, the computer program can implement the steps of each method in the above embodiments. When the computer program is executed by the processor, the specific implementation of each step and the resulting technical effects are the same as in the aforementioned method embodiments. For the sake of brevity, any parts not mentioned in this embodiment can be referred to the corresponding content in the aforementioned method embodiments.

[0122] A third aspect of this application provides a navigation device, including a memory, a processor, and a terminal program stored in the memory and executable in the processor. The terminal program includes steps corresponding to the autonomous positioning method described above. When the processor executes the terminal program, it implements the steps in the method embodiments described above, or, when the processor executes the terminal program, it implements the functions of each module / unit in the embodiments described above. It should be understood that the apparatuses of the various embodiments of this application can be implemented based on a memory and a processor. Each memory is used to store a terminal program for executing the methods described above, and the processor executes the terminal program, causing the navigation device to implement the methods of the various embodiments described above.

[0123] In some embodiments, the navigation device described above can be a desktop computer, laptop, industrial computer, PDA, tablet computer, or other mobile terminal, as well as a cloud server or other computer device, and is not limited to running any particular operating system. If the navigation device is a mobile terminal, it can be installed in a vehicle as hardware for the vehicle's intelligent driving system. Those skilled in the art will understand that the examples of navigation devices are not intended to limit its functionality; a navigation device may include more or fewer components than described above, or a combination of certain components, or different components. For example, a navigation device may also include input devices, output devices, network access devices, buses, displays, etc.

[0124] Regarding the specific implementation methods of this application, it should be noted that:

[0125] In the description of this application, the terms "comprising," "including," or any other variations thereof are intended to cover a non-exclusive inclusion, such that a process, method, apparatus, or readable storage medium that comprises a list of elements includes not only those elements but also other elements not expressly listed that conform to the concept of this application, or elements inherent to such a process, method, apparatus, or readable storage medium. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of additional elements in the process, method, apparatus, or readable storage medium that includes said element.

[0126] In the description of this application, the use of terms such as "some embodiments," "optional embodiments," "example," "specific example," "optional example," or "optional embodiment," etc., indicates that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application, but does not imply that these embodiments illustrate and describe all possible forms of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Furthermore, those skilled in the art can combine and integrate the different embodiments or examples described in this specification.

[0127] The present invention has been described in detail above with reference to specific embodiments and exemplary examples. The above description is exemplary and not exhaustive, and is not limited to the disclosed embodiments; the above description should not be construed as a limitation of the present invention. Technical solutions between various embodiments can be combined with each other, but must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application. Although embodiments of the present application have been shown and described, various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present application. Those skilled in the art will understand that various other specific changes and combinations of embodiments based on the technical teachings disclosed in this application, without departing from the essence of the present application, are still within the scope of protection defined by the claims of the present invention and their equivalent technical solutions.

Claims

1. A method for estimating the state of a UAV, characterized in that, include: Establish a state-space model, joint prior update, pre-clustering, adaptive online clustering based on the EM algorithm, and joint posterior update; The state-space model is a linear state-space model with multiple clusters of measurement noise; The joint prior update is based on the state transition matrix, process noise driving matrix, process noise covariance matrix of the state space model, and posterior estimates of the state, state covariance matrix, and GIG distribution parameters at the previous time step, to calculate the prior estimates of the state, state covariance matrix, measurement noise covariance matrix coefficients, and GIG distribution parameters at the current time step. The pre-clustering is based on the measurement values ​​of the inertial measurement unit, as well as the prior estimates of the state, state covariance matrix, and GIG distribution parameters, to cluster the measurement values. The adaptive online clustering based on the EM algorithm outputs the cluster sequence number to which each measurement value belongs. The adaptive online clustering based on the EM algorithm consists of an E-step and an M-step, which are iterated repeatedly until the proportion of each cluster in the overall measurement is reached. With the m The measurement belongs to the first n The probability of each cluster Convergence, the first m Each measurement will be assigned to In the highest cluster; The joint posterior update utilizes the cluster sequence number, the observation matrix of the state-space model, and the prior estimates of the state, state covariance matrix, measurement noise covariance matrix coefficients, and GIG distribution parameters to calculate the posterior GIG parameters and posterior measurement noise covariance matrix coefficients, further obtaining the measurement noise covariance matrix. Then, Kalman filtering is used for posterior state update, thereby obtaining the optimized UAV state.

2. The UAV state estimation method according to claim 1, characterized in that, The pre-segmented clusters include: Calculate the prior mean of the coefficients of the measurement noise covariance matrix: in, To measure the prior mean of the coefficients of the noise covariance matrix, , and The GIG distribution parameters at the current time. , and The prior estimate; Calculate the first m The measurement belongs to the first n Test statistic for each cluster: in, Indicates the first m A new measurement information, yes The covariance matrix, The conditional distribution can be obtained in the following way: in, Represents the m-th measurement. This represents the observation matrix corresponding to the m-th measurement. represent exist The prior estimate at time 1. represent The conditional probability density distribution, Represents a Gaussian distribution. Represents the state covariance matrix exist The prior estimate at time 1. It is a unit diagonal matrix, and its dimensions are... same; Calculate the test statistic. The test statistic is compared with the chi-square threshold to determine if the test statistic... If the value is greater than the chi-square threshold, it is considered that the first... The measurement belongs to the first The probability of each cluster If the calculated test statistic is less than the chi-square threshold, then adaptive online clustering based on the EM algorithm is performed.

3. The UAV state estimation method according to claim 1, characterized in that, The linear state-space model with multiple clusters of measurement noise is as follows: in, and Representing time respectively The state vector and measurement vector; , and The matrix consists of, in order: state transition matrix, process noise driving matrix, and observation matrix; process noise. It is zero-mean Gaussian noise, i.e. ; The process noise covariance matrix; To measure noise, it is represented as: in, Represents the complete measurement vector The measurement noise covariance matrix, This represents a block-based diagonal matrix. Indicates the first The measurement noise covariance matrix of each cluster, Represents the total number of clusters. The coefficients represent the measurement noise covariance matrix to be calculated. Is with the first The identity matrix associated with each cluster has dimensions of... same.

4. The UAV state estimation method according to claim 1, characterized in that, The joint prior is updated as follows: in, , , , and The state at the previous moment State covariance matrix Measurement noise covariance matrix coefficients, GIG distribution parameters , and The posterior estimate, , , , and It is the current moment. , , and The prior estimate, superscript Represents the corresponding number Clusters, , and These represent the state transition matrix, the process noise driving matrix, and the process noise covariance matrix, respectively. It is a forgetting factor.

5. The UAV state estimation method according to claim 1, characterized in that, The E-step of the adaptive online clustering algorithm based on the EM algorithm includes: Calculate latent variables , which indicates the first Does the measurement belong to the first...? Clusters: Calculate the first The measurement belongs to the first The probability of each cluster : in, Represents expectations, This indicates the proportion of each cluster in the overall measurement. For the first Each measure can be assigned to the number of candidate clusters.

6. The UAV state estimation method according to claim 1, characterized in that, The M-step of the adaptive online clustering algorithm based on the EM algorithm includes: The M-step of the adaptive online clustering algorithm based on the EM algorithm includes: To calculate the proportion of each cluster in the entire measurement: in, Indicates the total number of measurements taken so far; This represents the proportion of each cluster in the entire measurement; It is a representation of the first The measurement belongs to the first latent characteristic variables of each cluster; Representing the Individual measurement, Representing the scalar coefficients of the measurement noise covariance matrix of each cluster; Representing expectation, due to the nature of indicator variables, It can be equivalently represented by the first The measurement belongs to the first The probability of each cluster.

7. The UAV state estimation method according to claim 1, characterized in that, The calculation of the posterior GIG parameters and the coefficients of the posterior measurement noise covariance matrix includes: Calculate the transition amount : in, For the first The measurement vector is composed of all measurements of a cluster. yes The corresponding observation matrix, It is an identity matrix with dimension 1. The sum of all measurement dimensions of a cluster, , The current state State covariance matrix The prior estimate; trace represents the trace of the matrix. , This is for intermediate calculations; Calculate the posterior estimates of the GIG distribution parameters: in, , and The GIG distribution parameters at the current time. , and The posterior estimate, , and It is the current moment. , and The prior estimate, superscript Represents the corresponding number Clusters, express The dimension; Calculate the posterior estimates of the coefficients of the measurement noise covariance matrix: in, Posterior mean of the coefficients of the measurement noise covariance matrix.

8. A readable storage medium storing a computer program, characterized in that, When the computer program is executed, it performs the UAV state estimation method as described in any one of claims 1-7.

9. A navigation device, comprising a memory, a processor, and a terminal program stored in the memory and executable in the processor, characterized in that, The terminal program includes the steps corresponding to the UAV state estimation method as described in any one of claims 1-7.

Images