Optical flow assisted unmanned aerial vehicle non-linear robust federated filtering navigation method and system

Abstract

The invention discloses an optical flow assisted unmanned aerial vehicle nonlinear robust federated filtering navigation method and system, and the method specifically comprises the steps: constructing a robust weight matrix based on observation residual errors, and constructing a robust robust filter based on the robust weight matrix; an SINS / GNSS sub-filter and an SINS / VIO sub-filter are constructed, the SINS / GNSS sub-filter is subjected to combined filtering through an IMU, a GNSS, a magnetometer and an altimeter, and the SINS / VIO sub-filter is subjected to combined filtering through an IMU, a GNSS, a VIO and an altimeter; solving the robust adversarial filter by using an iteration method; constructing a self-adaptive federated filter, and adopting a mahalanobis distance as an information distribution factor of the self-adaptive federated filter; and based on an adaptive federated filter, fusing an SINS / GNSS sub-filter and an SINS / VIO sub-filter to carry out unmanned aerial vehicle position estimation. According to the invention, robust autonomous operation of the unmanned aerial vehicle in a forest environment can be realized.

Description

Technical Field

[0001] The present invention relates to the technical field of UAV navigation, and in particular to a nonlinear robust federated filtering navigation method and system for UAVs assisted by optical flow. Background Technique

[0002] Forest tending, also known as stand tending, refers to the technology of improving the growth rate of target tree species and stand quality by directionally intervening in the stand structure, regulating the distribution of light, heat, water and fertilizer resources, and optimizing the interspecific competition relationship. Under the traditional operation mode, the transportation of forest area materials is restricted by the terrain complexity and the fragmentation characteristics of the operation surface, and has long relied on the animal-drawn transportation system and small and medium-sized wheeled tractors, and its efficiency bottleneck is significant. The intelligent aerial logistics system based on carrier UAVs can achieve the precise delivery of forest area materials and reconstruct the forestry production operation paradigm.

[0003] There are still the following problems when existing UAVs are applied in forestry environments: The forest roads are located in mountainous areas, and there are often mountains and trees on both sides, resulting in poor GNSS signals when the UAV takes off and lands, few satellite acquisition numbers, slow convergence speed, and easy drift. Moreover, the mountain roads are narrow, and such drift is very likely to cause the UAV to crash. Summary of the Invention

[0004] The purpose of the present invention is to provide a nonlinear robust federated filtering navigation method and system for UAVs assisted by optical flow, so as to provide a full-condition solution for the robust autonomous operation of UAVs in forest environments.

[0005] The technical solution for realizing the purpose of the present invention is: A nonlinear robust federated filtering navigation method for UAVs assisted by optical flow includes the following steps:

[0006] Step 1: Construct a robust weight matrix based on the observation residuals, and construct a robust filter based on the robust weight matrix;

[0007] Step 2: Construct a SINS / GNSS sub-filter and a SINS / VIO sub-filter, where the SINS / GNSS sub-filter performs combined filtering by IMU, GNSS, magnetometer, and altimeter, and the SINS / VIO sub-filter performs combined filtering by IMU, GNSS, VIO, and altimeter;

[0008] Step 3: Solve the robust filter using the iterative method;

[0009] Step 4: Construct an adaptive federated filter, and use the Mahalanobis distance as the information distribution factor of the adaptive federated filter;

[0010] Step 5: Based on the adaptive federated filter, fuse the SINS / GNSS sub-filter and the SINS / VIO sub-filter to estimate the UAV position.

[0011] An optical flow-assisted nonlinear robust federated filtering navigation system for unmanned aerial vehicles, which is used to implement the optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles. The system includes a first module to a fifth module, and the functions of each module are as follows:

[0012] The first module constructs a robust weight matrix based on the observation residuals and constructs a robust filter based on the robust weight matrix.

[0013] The second module constructs an SINS / GNSS sub-filter and an SINS / VIO sub-filter. The SINS / GNSS sub-filter performs combined filtering by an IMU, GNSS, magnetometer, and altimeter, and the SINS / VIO sub-filter performs combined filtering by an IMU, GNSS, VIO, and altimeter.

[0014] The third module solves the robust filter using the iterative method.

[0015] The fourth module constructs an adaptive federated filter and uses the Mahalanobis distance as the information distribution factor of the adaptive federated filter.

[0016] The fifth module estimates the position of the unmanned aerial vehicle by fusing the SINS / GNSS sub-filter and the SINS / VIO sub-filter based on the adaptive federated filter.

[0017] A mobile terminal includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles.

[0018] A computer-readable storage medium stores a computer program, and when the program is executed by a processor, it implements the steps in the optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles.

[0019] Compared with the prior art, the remarkable advantages of the present invention are:

[0020] (1) An adaptive federated filtering architecture based on robust estimation theory, by constructing a multi-source heterogeneous data fusion model of GNSS / INS / visual odometer, designing a dynamic noise covariance robust filtering algorithm, providing a full-condition solution for the robust autonomous operation of unmanned aerial vehicles in forest environments;

[0021] (2) Equivalent the Kalman filter derived based on the Bayesian formula to a least squares problem, and assign an adaptive robust weight to each term of the least squares, thereby enhancing the anti-interference ability;

[0022] (3) Based on the robust filter, the iterative method is used to solve the robust filter. By iteratively updating the state and covariance multiple times, it can better approximate the non-linear relationship, reduce the linearization error, improve the estimation accuracy, and thus exhibit stronger robustness and stability in a complex non-linear environment. Brief Description of the Drawings

[0023] Figure 1 is the flowchart of the optical flow-assisted non-linear robust federated filtering navigation method for unmanned aerial vehicles of the present invention.

[0024] Figure 2 is the structural diagram of the federated Kalman part of the present invention.

[0025] Figure 3 is the structural diagram of the SINS / GNSS sub-filter of the present invention.

[0026] Figure 4 is the structural diagram of the SINS / VIO sub-filter of the present invention.

[0027] Figure 5 is the structural diagram of the robust iterative filter of the present invention.

[0028] Figure 6 is the comparison diagram of the combined navigation trajectory and the true trajectory of the present invention.

[0029] Figure 7 is the comparison diagram of the northward displacement of the combined navigation trajectory of the present invention. Detailed Embodiment

[0030] The present invention will be further described in detail below with reference to the drawings and specific embodiments.

[0031] Combined with Figures 1 to 5 , the present invention provides an optical flow-assisted non-linear robust federated filtering navigation method for unmanned aerial vehicles, including the following steps:

[0032] Step 1: Construct a robust weight matrix based on the observation residual, and construct a robust filter based on the robust weight matrix;

[0033] Step 2: Construct a SINS / GNSS sub-filter and a SINS / VIO sub-filter. The SINS / GNSS sub-filter performs combined filtering by IMU, GNSS, magnetometer, and altimeter, and the SINS / VIO sub-filter performs combined filtering by IMU, GNSS, VIO, and altimeter;

[0034] Step 3: Solve the robust filter using the iterative method;

[0035] Step 4: Construct an adaptive federated filter, and use the Mahalanobis distance as the information distribution factor of the adaptive federated filter;

[0036] Step 5: Based on the adaptive federated filter, fuse the SINS / GNSS sub-filter and the SINS / VIO sub-filter for UAV position estimation.

[0037] As a specific example, in step 1, construct a robust weight matrix based on the observation residual, and construct a robust filter based on the robust weight matrix, as follows:

[0038] Step 1.1: Construct the state equation of the UAV system:

[0039]

[0040] where, x k is the state variable of the system at time k, A k-1 is the state matrix of the system at time k - 1, B k-1 is the input matrix of the system at time k - 1, h(x k ) is the observation matrix of the system at time k, w k-1 is the process noise of the system at time k - 1, v k is the observation noise of the system at time k;

[0041] Step 1.2: Define the observation residual V k as:

[0042]

[0043] Define the state residual V xk as:

[0044]

[0045] where, z k is the observation value of the system at time k, x k is the prior estimate of the system, is the posterior estimate of the system, is the posterior estimate of the system at time k;

[0046] Step 1.3: Construct a robust weight matrix k based on the noise variance of the observation noise v

[0047] Assume that the noise w k and v k follow a normal distribution, and their noise variances are R k and Q k , respectively, as follows:

[0048]

[0049] Let is the robust weight matrix, which is obtained by multiplying the system observation noise covariance matrix by the variance inflation factor. Let the observation noise covariance matrix be Q k , and is defined as follows:

[0050]

[0051] where σ ij is the element in the i-th row and j-th column of the observation noise covariance matrix;

[0052] The corresponding robust weight matrix is:

[0053]

[0054] where:

[0055]

[0056] where v i is the observation error value of the i-th state quantity in the observation residual V k , and can be used to measure the deviation degree of a certain observed quantity caused by the observation noise;

[0057] Step 1.4, the Kalman filter derived based on the Bayesian formula is equivalent to a least squares problem, as follows:

[0058]

[0059] where, is the estimation principle of the Kalman filter in the ideal state;

[0060] Perform maximum a posteriori estimation on bel(x k ), that is, find the minimum value of J k . The Kalman filter is essentially a weighted least squares of the state residual and the observation residual. However, the simple least squares method does not have anti-interference ability. When the observed values deviate from the normal distribution assumption, a single observation deviation will cause a huge impact on the objective function after squaring. To solve this problem, an adaptive robust weight is assigned to each term of the least squares to replace the traditional least squares estimation. The estimation principle of the new Kalman filter is defined as follows:

[0061]

[0062] where V k is the observation residual of the system at time k, is the state residual of the system at time k, and P k is the state covariance matrix of the system at time k.

[0063] As a specific example, in step 2, the SINS / GNSS sub-filter and the SINS / VIO sub-filter are constructed. The SINS / GNSS sub-filter performs combined filtering using an IMU, GNSS, magnetometer, and altimeter, and the SINS / VIO sub-filter performs combined filtering using an IMU, GNSS, VIO, and altimeter, as follows:

[0064] Step 2.1: In the SINS / GNSS sub-filter, the angular increment and velocity increment are updated using the IMU mechanical arrangement, and then time update is performed according to the system state equation. The GNSS provides measurement updates for velocity and position in the NE direction, the altimeter provides observation updates for the vertical height, and the magnetometer provides observation updates for magnetic force and attitude;

[0065] Step 2.2: In the SINS / VIO sub-filter, the angular increment and velocity increment are updated using the IMU mechanical arrangement. The optical flow and velocity sensors provide measurement updates for velocity in the NE direction, and the altimeter provides measurement updates for the vertical height. The altimeter provides measurement updates for the vertical height, and the magnetometer provides measurement updates for magnetic force and attitude.

[0066] As a specific example, in step 3, the robust filter against outliers is solved using the iterative method, as follows:

[0067] Step 3.1: Update the state of the sub-filter, and the formula is as follows:

[0068]

[0069] where, is the prior state estimate obtained by state update, is the posterior state estimate at the previous moment, and P k|k-1 is the prior covariance matrix from time k - 1 to k; R k is the system state noise covariance matrix.

[0070] Step 3.2: Calculate the Jacobian matrix of the observation equation, and the formula is as follows:

[0071]

[0072] Step 3.3: Perform iterative calculation using the robust iterative Kalman formula, and the formula is as follows:

[0073]

[0074] where, is the intermediate state quantity at the i-th iteration; is the prior estimate of the state quantity at time k; is the Jacobian matrix of the observation equation at the i-th iteration;

[0075] The minimum value of the estimation principle is solved using the Gauss-Newton method as follows:

[0076] The traditional robust Kalman filter (EKF) is often affected by linearization approximation errors when dealing with nonlinear systems, which may lead to inaccurate estimations, especially when the system is highly nonlinear or the noise is large. Therefore, an iterative method can be used for optimization. By iteratively updating the state and covariance multiple times, it can better approximate the nonlinear relationship, reduce the linearization error, improve the estimation accuracy, and thus exhibit stronger robustness and stability in complex nonlinear environments.

[0077] When the observation equation is a nonlinear equation, the minimum value of the estimation principle is solved using the Gauss-Newton method. The process is as follows:

[0078]

[0079] Solving it gives:

[0080]

[0081] Find its Jacobian matrix as follows:

[0082]

[0083] Where:

[0084]

[0085] According to the Gauss-Newton method:

[0086]

[0087] Rearranging the above formula, the iterative formula of the robust iterative Kalman can be obtained:

[0088]

[0089] Where, is the intermediate state quantity at the i-th iteration; is the prior estimate of the state quantity at time k; is the Jacobian matrix of the observation equation at the i-th iteration; in each round of iteration, will get closer to the true posterior estimate and improve the accuracy of the observer. If only iterated once, this method is equivalent to the ordinary extended Kalman.

[0090] Step 3.4: Record the number of iterations in this round. If the number is less than 3, jump to Step 3.2 for the next round of iteration; if the number is greater than or equal to 3, complete the iteration and enter Step 4.

[0091] As a specific example, in step 4, an adaptive federated filter is constructed, and the Mahalanobis distance is used as the information distribution factor of the adaptive federated filter, which is specifically as follows:

[0092] Step 4.1: Use the information distribution factor to construct an adaptive federated filter. The adaptive federated filter has a two-layer filtering structure, namely a sub-filter structure and a main filter structure. After the local optimal estimation value solved by the sub-filter is globally fused, the final optimal estimation value is obtained;

[0093] Step 4.2: Construct the information distribution factor based on the Mahalanobis distance, as shown below:

[0094]

[0095] where, is the posterior estimation of the main filter, is the posterior estimation of the i-th sub-filter, and β i is the information distribution coefficient of the i-th sub-filter.

[0096] Furthermore, use the information distribution factor to construct a federated Kalman filter, which is specifically as follows:

[0097] The federated filter has a two-layer filtering structure, namely a sub-filter structure and a main filter structure. After the local optimal estimation value solved by the sub-filter is globally fused, the final optimal estimation value can be obtained.

[0098] The federated Kalman filtering can be divided into several steps:

[0099] (1) Sub-filter time update

[0100]

[0101] (2) Sub-filter measurement update

[0102]

[0103] (3) Main filter information fusion

[0104]

[0105] (4) Main filter time update

[0106]

[0107] (5) Information distribution feedback

[0108]

[0109] where, β i and β m are information distribution coefficients, βi > 0, β m > 0, and satisfy the following equation:

[0110]

[0111] As a specific example, in step 5, based on the adaptive federated filter, the SINS / GNSS sub-filter and the SINS / VIO sub-filter are fused for UAV position estimation, as follows:

[0112] Step 5.1: Based on the information of the sub-filter, fuse the main filter, and the formula is:

[0113]

[0114] Where is the posterior covariance matrix of the main filter, is the posterior state estimate of the main filter, is the posterior state estimate of the i-th sub-filter;

[0115] Step 5.2: Update the main filter state, and the formula is as follows:

[0116]

[0117] Step 5.3: Perform allocation feedback based on the state update of the main filter, and the formula is as follows:

[0118]

[0119] Where β i and β m are information distribution coefficients, β i > 0, β m > 0, and satisfy the following equation:

[0120]

[0121] Step 5.4: Take the state output of the main filter as the position, velocity, and attitude estimation of the UAV.

[0122] The present invention also provides an optical flow-assisted UAV non-linear robust federated filtering navigation system, which is used to implement the optical flow-assisted UAV non-linear robust federated filtering navigation method. The system includes a first module to a fifth module, and the functions of each module are as follows:

[0123] The first module constructs a robust weight matrix based on the observation residuals and constructs a robust filter based on the robust weight matrix;

[0124] The second module constructs the SINS / GNSS sub-filter and the SINS / VIO sub-filter. The SINS / GNSS sub-filter performs combined filtering with an IMU, GNSS, magnetometer, and altimeter, and the SINS / VIO sub-filter performs combined filtering with an IMU, GNSS, VIO, and altimeter;

[0125] The third module uses the iterative method to solve the robust filter against outliers;

[0126] The fourth module constructs an adaptive federated filter and uses the Mahalanobis distance as the information distribution factor of the adaptive federated filter;

[0127] The fifth module estimates the UAV position by fusing the SINS / GNSS sub-filter and the SINS / VIO sub-filter based on the adaptive federated filter.

[0128] The present invention also provides a mobile terminal, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the optical flow-assisted non-linear robust federated filtering navigation method for UAVs.

[0129] The present invention also provides a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, it implements the steps in the optical flow-assisted non-linear robust federated filtering navigation method for UAVs.

[0130] The present invention equates the Kalman filter derived based on the Bayesian formula to a least squares problem, and assigns an adaptive outlier-resistant weight to each term of the least squares, enhancing the anti-interference ability; based on the robust filter against outliers, the iterative method is used to solve the outlier-resistant filter. By iteratively updating the state and covariance multiple times, it can better approximate the non-linear relationship, reduce the linearization error, improve the estimation accuracy, and thus show stronger robustness and stability in a complex non-linear environment. Figure 6 It is a comparison diagram of the combined navigation trajectory and the true trajectory of the present invention, Figure 7 It is a comparison diagram of the northward displacement of the combined navigation trajectory of the present invention. It can be seen that the present invention provides a full-condition solution for the robust autonomous operation of UAVs in the forest environment by constructing a multi-source heterogeneous data fusion model of GNSS / INS / visual odometer and designing a robust filtering algorithm for dynamic noise covariance.

[0131] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. An optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles, characterized in that: The following steps are involved: Step 1: construct a robust weight matrix based on the observation residual, and construct a robust filter based on the robust weight matrix; Step 2, constructing a SINS / GNSS sub-filter and a SINS / VIO sub-filter, wherein the SINS / GNSS sub-filter is combined and filtered by IMU, GNSS, magnetometer, and altimeter, and the SINS / VIO sub-filter is combined and filtered by IMU, GNSS, VIO, and altimeter; Step 3: Use an iterative method to solve the robust filter against errors; Step 4: construct an adaptive federated filter and use Mahalanobis distance as the information allocation factor of the adaptive federated filter; Step 5: Based on the adaptive federated filter, the SINS / GNSS sub-filter and the SINS / VIO sub-filter are fused to estimate the UAV position.

2. The optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles according to claim 1 is characterized in that: In step 1, a robust weight matrix is ​​constructed based on the observed residual, and a robust filter is constructed based on the robust weight matrix, as follows: Step 1.1: Construct the state equation of the UAV system: Among them, x k is the state variable of the system at time k, A k-1 is the state matrix of the system at time k-1, B k-1 is the input matrix of the system at time k-1, h(x k ) is the observation matrix of the system at time k, w k-1 is the process noise of the system at time k-1, v k is the observation noise of the system at time k; Step 1.2: Define the observation residual V k for: Define state residuals for: Among them, z k is the observed value of the system at time k, x k is the system prior estimate, is the system posterior estimate, is the a posteriori estimate of the system at time k; Step 1.3: Based on the observation noise v of the system at time k k The noise variance is used to construct the robust weight matrix It is obtained by multiplying the system observation noise covariance matrix by the variance expansion factor, and the observation noise covariance matrix is ​​Q k , defined as follows: Among them, σ ij is the element in the i-th row and j-th column of the observation noise covariance matrix; The corresponding robustness weight matrix for: in: Among them, v i is the observed residual V k The observation error value of the i-th state quantity in can be used to measure the degree of deviation of a certain observation quantity caused by observation noise; Step 1.4: The Kalman filter derived based on the Bayesian formula is equivalent to a least squares problem, as shown below: in, This is the estimation principle of Kalman filtering under ideal conditions; For bel(x k ) to make the maximum a posteriori estimate, that is, k To find the minimum value, Kalman filtering is essentially the weighted least squares of the state residual and the observation residual. Each item of the least squares is given an adaptive robustness weight. The new Kalman filter estimation principle is defined as follows: Among them, V k is the observed residual of the system at time k, is the state residual of the system at time k, P k is the state covariance matrix of the system at time k.

3. The optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles according to claim 2 is characterized in that: In step 2, a SINS / GNSS sub-filter and a SINS / VIO sub-filter are constructed, wherein the SINS / GNSS sub-filter is combined and filtered by IMU, GNSS, magnetometer, and altimeter, and the SINS / VIO sub-filter is combined and filtered by IMU, GNSS, VIO, and altimeter, as follows: Step 2.1, in the SINS / GNSS sub-filter, the IMU mechanical arrangement is used to update the angle increment and velocity increment, the GNSS provides the measurement update of the velocity and the position in the NE direction, the altimeter provides the observation update of the vertical height, and the magnetometer provides the observation update of the magnetism and attitude; Step 2.2: In the SINS / VIO sub-filter, the IMU mechanical arrangement is used to update the angle increment and velocity increment. The optical flow and velocity sensors provide measurement updates in the velocity NE direction, the altimeter provides measurement updates of the vertical height, and the magnetometer provides measurement updates of magnetism and attitude.

4. The optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles according to claim 3 is characterized in that: In step 3, the robust filter against errors is solved using an iterative method, as follows: Step 3.1: Update the state of the sub-filter. The formula is as follows: in, is the prior state estimate obtained by state update, is the posterior state estimate of the previous moment, P k|k-1 is the prior covariance matrix from k-1 to k; R k is the system state noise covariance matrix; Step 3.2, calculate the Jacobian matrix of the observation equation, the formula is as follows: Step 3.3: Use the robust iterative Kalman formula for iterative calculation. The formula is as follows: in, is the intermediate state quantity at the i-th iteration; is the prior estimate of the state quantity at time k; is the Jacobian matrix of the observation equation at the i-th iteration; Step 3.4: Record the number of iterations in this round. If the number is less than 3, jump to step 3.2 for the next round of iterations. If the number is greater than or equal to 3, the iteration is completed and go to step 4.

5. The optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles according to claim 4 is characterized in that: In step 4, an adaptive federated filter is constructed, and the Mahalanobis distance is used as the information allocation factor of the adaptive federated filter, as follows: Step 4.1, use the information allocation factor to construct an adaptive federated filter. The adaptive federated filter has two filter structures, namely, a sub-filter structure and a main filter structure. The local optimal estimation values ​​solved by the sub-filters are globally fused to obtain the final optimal estimation value; Step 4.2: Construct the information allocation factor based on the Mahalanobis distance as follows: in, is the a posteriori estimate of the main filter, is the posterior estimate of the i-th sub-filter, β i is the information allocation coefficient of the i-th sub-filter.

6. The optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles according to claim 5 is characterized in that: In step 5, based on the adaptive federated filter, the SINS / GNSS sub-filter and the SINS / VIO sub-filter are fused to estimate the position of the UAV, as follows: Step 5.1: Based on the information of the sub-filters, the main filter is fused. The formula is: in, is the posterior covariance matrix of the main filter, is the a posteriori state estimate of the main filter, is the posterior state estimate of the i-th sub-filter; Step 5.2: Update the main filter state. The formula is as follows: Step 5.3: Allocation feedback is performed based on the state update of the main filter. The formula is as follows: Among them, β i With β m is the information distribution coefficient, β i >0,β m >0, and satisfies the following equation: Step 5.4: Use the state output of the main filter as the position, velocity and attitude estimation of the UAV.

7. An optical flow-assisted nonlinear robust federated filtering navigation system for unmanned aerial vehicles, characterized in that: The system is used to implement the optical flow-assisted nonlinear robust federated filtering navigation method for unmanned aerial vehicles according to any one of claims 1 to 6. The system includes a first module to a fifth module, and the functions of each module are as follows: The first module constructs an anti-error weight matrix based on the observation residual, and constructs an anti-error robust filter based on the anti-error weight matrix; The second module constructs the SINS / GNSS sub-filter and the SINS / VIO sub-filter, wherein the SINS / GNSS sub-filter is combined and filtered by IMU, GNSS, magnetometer, and altimeter, and the SINS / VIO sub-filter is combined and filtered by IMU, GNSS, VIO, and altimeter; In the third module, the robust filter against errors is solved using an iterative method; The fourth module builds an adaptive federated filter and uses Mahalanobis distance as the information allocation factor of the adaptive federated filter; The fifth module estimates the UAV position based on the adaptive federated filter by fusing the SINS / GNSS sub-filter and the SINS / VIO sub-filter.

8. A mobile terminal comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the optical flow-assisted unmanned aerial vehicle nonlinear robust federated filtering navigation method as described in any one of claims 1 to 6 is implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps in the optical flow-assisted unmanned aerial vehicle nonlinear robust federated filtering navigation method as described in any one of claims 1 to 6 are implemented.

Images