Multi-source navigation federated filtering method and system based on exponential decay adaptive information distribution

Abstract

The invention discloses a multi-source navigation federated filtering method and system based on exponential decay adaptive information distribution. The method specifically comprises the following steps: acquiring data of an SINS, a GNSS and an ODOM; sINS data are used as a reference system in a federated filter, the SINS data are solved in real time, SINS solving results are input into a main filter, a first sub-filter and a second sub-filter respectively, Kalman filtering is conducted on GNSS data and ODOM data through the first sub-filter and the second sub-filter respectively, and filtering results are output into the main filter to be fused; then constructing an information distribution strategy of an exponential decay formula based on a P matrix Frobenius norm to obtain an information distribution factor; and the main filter multiplies the fused common state quantity and the state estimation mean square error matrix of the common state by an information distribution factor, and feeds back to each sub-filter for feedback reset. The method has the advantages of high precision, high stability, high anti-interference capability and high adaptability.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-source navigation, and in particular to a multi-source navigation federated filtering method and system based on exponentially decaying adaptive information allocation. Background Art

[0002] In a multi-source navigation system with multiple sensors, the commonly used filtering method is centralized filtering, where a high-dimensional comprehensive filter containing all state variables is designed for Kalman filtering. However, this solution has many drawbacks, such as high state dimensions, high computational complexity, and the inability to ensure the real-time performance of the navigation system. To reduce the computational complexity, federated filtering can be used for decentralized reduction processing.

[0003] In the federated filtering with a feedback reset structure, the information allocation factor is usually set as a constant, and the classic allocation method is equal allocation. However, during the process of federated filtering, the filtering quality of each sub-filter will change. For example, when the multi-source navigation system is affected by the environment and the filtering quality of the SINS / GNSS sub-filter decreases, it is necessary to dynamically adjust the information allocation coefficient at this time to reduce the fusion weight of the SINS / GNSS sub-filter. Therefore, the fixed information allocation method cannot fully utilize the advantages of multi-sensor fusion, and dynamic information allocation is required at this time.

[0004] From the perspective of the system filtering accuracy, the higher the accuracy of the subsystem, the larger the information matrix and the information allocation factor; the lower the accuracy of the subsystem, the smaller the information allocation factor. According to the above rules, the existing information allocation methods include the following two: (1) The information allocation factor is directly calculated from the weight of the F-norm of the state estimation mean square error matrix; (2) First, an information coefficient is constructed using the F-norm of the state estimation mean square error matrix, and then the information allocation coefficient is calculated.

[0005] The above two information allocation methods are both adaptive information allocation methods based on the inverse proportional function. Although they conform to the information allocation principle, there are potential hazards. They do not consider the situation where the information allocation factor is close to 0 in extreme cases, resulting in the situation where the common state estimation mean square error matrix of a certain sub-filter is close to 0 after information allocation, which may cause the matrix to be close to singularity, and may lead to numerical instability during subsequent information fusion, and numerical calculations will encounter accuracy problems, thus affecting the navigation and positioning accuracy. Summary of the Invention

[0006] The purpose of the present invention is to provide a multi-source navigation federated filtering method and system based on exponentially decaying adaptive information allocation, so that the information allocation factor is adaptively adjusted according to the allocation principle, thereby achieving the effects of high filtering accuracy, high numerical calculation stability, strong adaptability, and strong fault tolerance.

[0007] The technical solution for implementing the present invention is as follows: A multi-source navigation federated filtering method based on exponential decay adaptive information allocation, comprising the following steps:

[0008] Step 1: Obtain the data of the strapdown inertial navigation system (SINS), the global navigation satellite system (GNSS), and the wheel odometer (ODOM).

[0009] Step 2: Take SINS as the reference system in the federated filter, perform real-time calculation on SINS data, and input the results of SINS calculation into the main filter, the first sub-filter, and the second sub-filter respectively.

[0010] Step 3: The first sub-filter performs Kalman filtering using GNSS data as the observation; the second sub-filter first performs dead reckoning using the data of ODOM and SINS data, and then performs Kalman filtering using the results of the reckoning as the observation.

[0011] Step 4: The filtering results of the first sub-filter and the second sub-filter are output to the main filter, and in the main filter, the common state quantities output by the two sub-filters, namely speed, position, and attitude, are fused.

[0012] Step 5: Construct an information allocation strategy based on the exponential decay formula of the Frobenius norm of the P matrix, complete information allocation using the exponential decay adaptive information allocation method, obtain the expression of the information allocation factor, and calculate the information allocation factor.

[0013] Step 6: The main filter feeds back the fused common state quantity and the state estimation mean square error matrix of the common state multiplied by the information allocation factor to each sub-filter to complete the task of feedback reset.

[0014] A multi-source navigation federated filtering system based on exponential decay adaptive information allocation, which is used to implement the multi-source navigation federated filtering method based on exponential decay adaptive information allocation. The system includes a first module to a fourth module, and the functions of each module are as follows:

[0015] The first module obtains the data of the strapdown inertial navigation system (SINS), the global navigation satellite system (GNSS), and the wheel odometer (ODOM).

[0016] The second module takes SINS as the reference system in the federated filter, performs real-time calculation on SINS data, and inputs the results of SINS calculation into the main filter, the first sub-filter, and the second sub-filter respectively.

[0017] The first sub-filter performs Kalman filtering using GNSS data as observations; the second sub-filter first performs dead reckoning using ODOM data and SINS data, and then performs Kalman filtering using the results of the reckoning as observations; the filtering results of the first sub-filter and the second sub-filter are output to the main filter, and in the main filter, the common state variables output by the two sub-filters, namely speed, position, and attitude, are fused;

[0018] The third module constructs an information distribution strategy based on the exponential decay formula of the Frobenius norm of the P matrix, uses the exponential decay adaptive information distribution method to complete information distribution, obtains the expression of the information distribution factor, and calculates the information distribution factor;

[0019] The fourth module feeds back the fused common state variables and the state estimation mean square error matrix of the common state multiplied by the information distribution factor to each sub-filter to complete the task of feedback reset.

[0020] 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 multi-source navigation federated filtering method based on exponential decay adaptive information distribution.

[0021] A computer-readable storage medium stores a computer program thereon. When the program is executed by a processor, it implements the steps in the multi-source navigation federated filtering method based on exponential decay adaptive information distribution.

[0022] Compared with the prior art, the significant advantages of the present invention are: (1) The information distribution factor can be adaptively adjusted according to the filtering quality of each sub-filter, giving full play to the advantages of multi-sensors and improving the filtering accuracy; (2) Using the exponential function to perform decay processing on the norm improves the stability of the filtering process after information distribution. Description of the Drawings

[0023] Figure 1 It is a schematic flowchart of a multi-source navigation federated filtering method based on exponential decay adaptive information distribution.

[0024] Figure 2 It is a comparison diagram of the planar trajectories of the federated filtering outputs of the original data using three information distribution methods and the reference trajectory in the embodiment of the present invention.

[0025] Figure 3 It is a comparison diagram of the three-axis position errors of the federated filtering of the original data using three information distribution methods in the embodiment of the present invention.

[0026] Figure 4This is a comparison chart of the overall spatial position errors of the federated filter for the original data in the embodiments of the present invention using three information allocation methods.

[0027] Figure 5 This is a comparison chart of the triaxial position errors of the federated filter using three information allocation methods after adding noise to the GNSS data in the embodiments of the present invention.

[0028] Figure 6 This is a comparison chart of the overall spatial position errors of the federated filter using three information allocation methods after adding noise to the GNSS data in the embodiments of the present invention.

[0029] Figure 7 This is a comparison chart of the triaxial velocity errors of the federated filter using three information allocation methods after adding noise to the GNSS data in the embodiments of the present invention.

[0030] Figure 8 This is a comparison chart of the heading angle errors of the federated filter using three information allocation methods after adding noise to the GNSS data in the embodiments of the present invention.

[0031] Figure 9 This is the value range of the information allocation factor β in the information allocation method based on the inverse proportional function in the embodiments of the present invention.

[0032] Figure 10 This is the value range of the information allocation factor β in the information allocation method based on exponential decay in the embodiments of the present invention. Detailed implementation manners

[0033] The following further describes the present invention in detail with reference to the accompanying drawings and specific embodiments.

[0034] As Figure 1 shown, a multi-source navigation federated filtering method based on exponential decay adaptive information allocation of the present invention includes the following steps:

[0035] Step 1: Obtain data of the strapdown inertial navigation system SINS, the global navigation satellite system GNSS, and the wheel odometer ODOM;

[0036] Step 2: Take SINS as the reference system in the federated filter, perform real-time calculation on SINS data, and input the results of SINS calculation into the main filter, the first sub-filter, and the second sub-filter respectively;

[0037] Step 3: The first sub-filter performs Kalman filtering using GNSS data as the observation; the second sub-filter first performs dead reckoning using the data of ODOM and SINS data, and then performs Kalman filtering using the results of the reckoning as the observation;

[0038] Step 4: The filtering results of the first sub-filter and the second sub-filter are output to the main filter, and the common state variables output by the two sub-filters, namely speed, position, and attitude, are fused in the main filter;

[0039] Step 5: Construct an information distribution strategy based on the exponential decay formula of the Frobenius norm of the P matrix, adopt the exponential decay adaptive information distribution method to complete information distribution, obtain the expression of the information distribution factor, and calculate the information distribution factor;

[0040] Step 6: The main filter feeds back the fused common state variables and the state estimation mean square error matrix of the common state multiplied by the information distribution factor to each sub-filter to complete the task of feedback reset.

[0041] As a specific example, in Step 3, sub-filter 1 performs Kalman filtering using GNSS data as observations; sub-filter 2 first performs dead reckoning using ODOM data and SINS data, and then performs Kalman filtering using the reckoning result as observations, specifically as follows:

[0042] Step 3.1: The state space model and observation model of the sub-filter are:

[0043]

[0044] where, is the state estimation value of sub-filter i at time k; is the one-step state transition matrix of sub-filter i from time k-1 to time k; is the noise matrix of sub-filter i; are the observation value, observation matrix, and observation noise matrix of sub-filter i at time k, respectively;

[0045] Step 3.2: The state variables of sub-filter 1 are selected as:

[0046]

[0047] where (φ) T , (δv n ) T , (δp) T , (ε b ) T , represent attitude error, velocity error, position error, gyroscope zero bias, and accelerometer zero bias, respectively;

[0048] The observation vector and observation matrix of sub-filter 1 are respectively:

[0049]

[0050] Among them, and respectively represent the velocity and position information calculated by the strapdown inertial navigation system, and respectively represent the velocity and position information obtained from GNSS observations; 0 3×3 is a 3×3 zero matrix, and I 3×3 is a 3×3 identity matrix;

[0051] Step 3.3, the state variables of sub-filter 2 are selected as:

[0052]

[0053] where (φ) T , (δv n ) T , (δp) T , (ε b ) T , respectively represent attitude error, velocity error, position error, gyroscope zero bias, and accelerometer zero bias, and (δp D ) T represents the displacement error obtained by dead reckoning of the wheel odometer;

[0054] The observation vector and the observation matrix of sub-filter 2 are respectively:

[0055]

[0056] Among them, represents the position information calculated by the strapdown inertial navigation system, represents the position information obtained by dead reckoning of the wheel odometer; 0 3×3 is a 3×3 zero matrix, and I 3×3 is a 3×3 identity matrix;

[0057] Step 3.4, according to the Kalman filter, the sub-filter has:

[0058]

[0059] In the formula, is the prior estimate at time k, is the posterior estimate at time k-1;

[0060] Step 3.5, calculate the state one-step prediction mean square error matrix as:

[0061]

[0062] Wherein, P k-1 is the mean square error matrix of the state estimate at time k-1, and Q k-1 is the system process noise matrix;

[0063] Step 3.6, calculate the filtering gain as:

[0064]

[0065] Wherein, K k is the filtering gain at time k, and R k is the observation noise matrix at time k;

[0066] Step 3.7, update the state estimate as:

[0067]

[0068] Wherein is the posterior estimate at time k;

[0069] Step 3.8, update the state estimate mean square error matrix as:

[0070] P k =(I-K k H k )P k / k-1 (12)

[0071] Wherein P k is the state estimate mean square error matrix at time k.

[0072] As a specific example, the filtering results of sub-filter 1 and sub-filter 2 in step 4 are output to the main filter, and the common state quantities output by the two sub-filters, namely speed, position, and attitude, are fused in the main filter, specifically as follows:

[0073] First, take the common state of the sub-filter, and let represent the common state of sub-filter i, and take that is, attitude error, speed error, position error, gyro zero bias, and accelerometer zero bias as the common state; let represent the proprietary state of sub-filter i; only the common states shared by all sub-filters can be fused and reset in the main filter, and non-common states cannot be fused; set the state estimate and its mean square error matrix of the i-th sub-filter at time k to be:

[0074]

[0075] After information fusion of each sub-filter:

[0076]

[0077] After information fusion, a globally optimal state estimate is obtained and the mean square error matrix of the state estimate

[0078] As a specific example, in step 5, an information distribution strategy for constructing an exponentially decaying formula based on the Frobenius norm of the P matrix is adopted, and the information distribution is completed by using an exponentially decaying adaptive information distribution method to obtain an expression for the information distribution factor, and the information distribution factor β is calculated as follows:

[0079] Step 5.1: The present invention adopts a federated filter with a feedback reset structure, and multiplies the mean square error matrix of the state estimate of the common state by the information distribution factor β and feeds it back to each sub-filter:

[0080]

[0081] where β i is the information distribution factor and satisfies the following relationship:

[0082]

[0083] where β m is the information distribution factor of the main filter, β i is the information distribution factor of each sub-filter. According to the different structures of the federated filter and different selection strategies for information distribution, β has different value-taking methods, and thus the federated filter has different structures;

[0084] Step 5.2: In order to make full use of the information of each sub-filter in the main filter, improve the filtering accuracy and maintain a certain fault tolerance, an adaptive information distribution method is adopted; in the sub-filter, describes the filtering quality of the sub-filter i for the common state at time k; taking as the information matrix, when is smaller, the state estimation quality of the system is better, and the information matrix will be larger;

[0085] It can be seen from Equation (16) that in information distribution, the mean square error matrix of the estimated common state after information distribution of the sub-filter is inversely proportional to the information distribution factor β. The larger β is, the smaller the error covariance matrix after information distribution. It can be seen from Equation (15) that the fusion of state quantities is essentially weighted by the inverse matrix of the error covariance matrix of each sub-filter, that is the smaller it is, the larger the fusion weight; based on the above analysis, the following conclusion can be obtained:

[0086] The information distribution factor β affects the utilization weight of the system for the navigation information of the sub - filters. The smaller β is, the lower the utilization weight. In order to make the fusion result more affected by the sub - filter with higher accuracy, the information distribution factor of the sub - filter with higher accuracy should be larger. Based on the above conclusions, the following information distribution strategy is obtained:

[0087]

[0088] Step 6: The main filter multiplies the fused common state quantity and the state estimation mean - square error matrix of the common state by the information distribution factor β, and feeds them back to each sub - filter to complete the task of feedback reset.

[0089] The present invention also provides a multi - source navigation federated filtering system based on exponentially - decaying adaptive information distribution, which is used to implement the multi - source navigation federated filtering method based on exponentially - decaying adaptive information distribution. The system includes a first module to a fourth module, and the functions of each module are as follows:

[0090] The first module acquires data from the strap - down inertial navigation system (SINS), the global navigation satellite system (GNSS), and the wheel odometer (ODOM).

[0091] The second module takes SINS as the reference system in the federated filter, resolves the SINS data in real - time, and inputs the results of SINS resolution into the main filter, the first sub - filter, and the second sub - filter respectively.

[0092] The first sub - filter performs Kalman filtering with GNSS data as the observation; the second sub - filter first performs dead reckoning using the data of ODOM and SINS, and then performs Kalman filtering with the results of dead reckoning as the observation. The filtering results of the first sub - filter and the second sub - filter are output to the main filter, and in the main filter, the common state quantities output by the two sub - filters, namely speed, position, and attitude, are fused.

[0093] The third module constructs an information distribution strategy based on the exponentially - decaying formula of the Frobenius norm of the P matrix, completes information distribution using the exponentially - decaying adaptive information distribution method, obtains the expression of the information distribution factor, and calculates the information distribution factor.

[0094] The fourth module: The main filter multiplies the fused common state quantity and the state estimation mean - square error matrix of the common state by the information distribution factor, and feeds them back to each sub - filter to complete the task of feedback reset.

[0095] 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, the multi-source navigation federated filtering method based on exponential decay adaptive information allocation is implemented.

[0096] 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, the steps in the multi-source navigation federated filtering method based on exponential decay adaptive information allocation are implemented.

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

[0098] Embodiment

[0099] In this embodiment, a self-driving vehicle equipped with a ROS system is used to collect data, and a hardware-in-the-loop simulation experiment is carried out on the collected data in MATLAB to verify the effectiveness and accuracy of a multi-source navigation federated filtering method based on exponential decay adaptive information allocation of the present invention.

[0100] In order to compare the federated Kalman filtering of the multi-source navigation federated Kalman filtering method based on exponential decay adaptive information allocation and the traditional information allocation method, first, a road test is completed by the self-driving vehicle to collect data, and then the federated Kalman filtering results of several information allocation methods are compared with the actual road test data collected to verify the advantages of the present invention.

[0101] The specific working process of this embodiment is as follows: First, a self-driving vehicle experimental platform with a strapdown inertial navigation system SINS, a global navigation satellite system GNSS, a wheel odometer ODOM, an integrated navigation module IPMV, and an in-vehicle navigation computer is built; then, it runs laps on an open square and records the data of various sensors. Among them, the high-precision integrated navigation module is the reference, and the speed, position, and attitude output by it are regarded as the true values. The original data of the sensors are recorded through a rosbag package; finally, the data is extracted from the rosbag package and saved as a txt file, and MATLAB is used to read the txt file to import the data of each sensor and process it.

[0102] In the hardware-in-the-loop simulation experiment, the collected data is filtered and estimated respectively by the federated Kalman filtering of the traditional information allocation method and the multi-source navigation federated filtering method based on exponential decay adaptive information allocation of the present invention. Then, the difference between the filtered result and the data of the IPMV is calculated to obtain the estimation error, and the root mean square error RMES of the experimental result is calculated to measure the size of the error, and the method of the present invention is compared with the multi-source navigation federated filtering method based on the traditional information allocation method. In order to prove the effect of the method of the present invention, zero-mean noise is added to the collected GNSS data in MATLAB, and the filtered comparison is carried out again with this GNSS data with added noise.

[0103] Comparison of Root Mean Square Errors of Simulation Results of Original Data in Table 1

[0104] Method Spatial position error (m) Spatial velocity error (m / s) Course angle error (°) average 0.8570 0.0940 0.7581 Dync1 0.9001 0.0956 0.8137 Dync2 0.8563 0.0945 0.7594

[0105] Table 1 is a comparison chart of the results of simulating the original data collected by the unmanned vehicle. In the table, "average" represents the vehicle-mounted federated filtering method that evenly distributes information in the traditional method, "Dync1" represents the vehicle-mounted federated filtering method based on the inverse proportional function information adaptive distribution method, and "Dync2" represents the vehicle-mounted federated filtering method based on the exponential decay adaptive information distribution method of the present invention. The spatial position error in the table is the overall position error after the superposition of the three-axis position error vectors, and the spatial velocity error is the overall velocity error after the superposition of the three-axis velocity error vectors. It can be seen from the table that without artificial interference, the results output by the method of the present invention are almost the same as those of the federated filtering method with evenly distributed information, and the result error of the vehicle-mounted federated filtering method based on the inverse proportional function information distribution is larger.

[0106] Figure 2 Figure for comparing the planar trajectories obtained by the three methods of "average", "Dync1", and "Dync2" with the reference trajectory Figures 3 to 4 Figure for the three-axis position errors and spatial position errors output by the three methods in this simulation experiment

[0107] Comparison of Root Mean Square Errors of Data Position Simulation Results after Adding Noise to GNSS Data in Table 2

[0108] Method Spatial position error (m) Spatial velocity error (m / s) Course angle error (°) average 4.3857 0.5040 3.5548 Dync1 4.1507 0.5405 2.2931 Dync2 2.1795 0.2642 1.8221

[0109] Table 2 shows the comparison of the root mean square errors of the federated filtering results of the three information distribution methods after adding noise to the position and velocity of GNSS respectively. It can be seen that the result error of using the method of the present invention is the smallest. Compared with the vehicle-mounted federated filtering method with evenly distributed information, the root mean square error of the spatial position is reduced by about 50%, the root mean square error of the overall spatial velocity is reduced by 47%, and the root mean square error of the heading angle is reduced by about 44%. Compared with the vehicle-mounted federated filtering method based on the inverse proportional function information distribution, the root mean square error of the spatial position is reduced by about 47%, the root mean square error of the overall spatial velocity is reduced by 51%, and the root mean square error of the heading angle is reduced by about 20.53%.

[0110] Figures 5 to 8 Figure for the three-axis position errors, overall spatial position errors, three-axis velocity errors, and heading angle errors output by the federated filtering of the three information distribution methods after adding GNSS noise. It can be seen from the figure that the method of the present invention has significant advantages compared with the other two methods, and the error reduction is more obvious.

[0111] It can be seen that the multi-source navigation federated filtering method based on exponentially decaying adaptive information allocation proposed by the present invention, compared with the federated filtering method based on traditional information allocation, can not only adaptively adjust the information allocation factor, give full play to the advantages of multi-sensors, and improve the filtering accuracy, but also has better adaptive fault tolerance performance when there is large noise interference in the observation sensor, reducing the error caused by additional noise. Moreover, in the formula of information allocation, the exponential function is used to decay the norm, so that the information allocation factor is limited within a reasonable range, avoiding extreme situations, and the filtering process after information allocation is more stable.

[0112] Figures 9 to 10 For the value ranges of β for exponentially decaying information allocation and information allocation based on the inverse proportional function respectively, it can be seen from the figure that in the information allocation method based on exponential decay, the value of the information allocation factor β is smoother and is limited within a reasonable range, without extreme values that may lead to instability in numerical calculations.

[0113] In summary, a multi-source navigation federated filtering method based on exponentially decaying adaptive information allocation proposed by the present invention improves the calculation formula of the information allocation factor, enhances the adaptive performance under large noise interference, improves the filtering accuracy and the stability of numerical calculations, and improves the fault tolerance performance.

[0114] The above is only the preferred embodiment 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. A multi-source navigation federated filtering method based on exponential decay adaptive information allocation, characterized in that: The following steps are involved: Step 1: Obtain data from the strapdown inertial navigation system SINS, the global navigation satellite system GNSS, and the wheel odometer ODOM; Step 2: Using SINS as a reference system in the federated filter, solving the SINS data in real time, and inputting the results of the SINS solution into the main filter, the first sub-filter, and the second sub-filter respectively; Step 3: The first sub-filter uses GNSS data as observations to perform Kalman filtering; the second sub-filter first uses ODOM data and SINS data to perform dead reckoning, and then uses the dead reckoning results as observations to perform Kalman filtering; Step 4: The filtering results of the first sub-filter and the second sub-filter are output to the main filter, and the common state quantities output by the two sub-filters, namely, speed, position, and posture, are fused in the main filter; Step 5: construct an information allocation strategy based on the exponential decay formula of the Frobenius norm of the P matrix, use an exponential decay-based adaptive information allocation method to complete information allocation, obtain an expression for the information allocation factor, and calculate the information allocation factor; Step 6: The main filter multiplies the fused common state quantity and the state estimation mean square error matrix of the common state by the information allocation factor, and feeds them back to each sub-filter to complete the task of feedback reset.

2. The multi-source navigation federated filtering method based on exponential decay adaptive information allocation according to claim 1, characterized in that: In step 3, the first sub-filter uses GNSS data as observations for Kalman filtering; the second sub-filter first uses ODOM data and SINS data to perform dead reckoning, and then uses the dead reckoning results as observations for Kalman filtering, as follows: Step 3.1, the state space model and observation model of the sub-filter are: in, is the estimated state value of sub-filter i at time k; is the state one-step transfer matrix of sub-filter i from time k-1 to time k; is the noise matrix of sub-filter i; are the observation value, observation matrix and observation noise matrix of sub-filter i at time k respectively; Step 3.2: State variables of the first sub-filter Select as: Where (φ) T 、(δv n ) T 、(δp) T 、(ε b ) T , They represent attitude error, velocity error, position error, gyroscope bias and accelerometer bias respectively; The observation vector of the first subfilter and the observation matrix They are: in, and They represent the speed and position information calculated by the strapdown inertial navigation system respectively, and Respectively represent the speed and position information obtained by GNSS observation; 0 3×3 is a 3×3 zero matrix, I 3×3 is a 3×3 unit matrix; Step 3.3: State variables of the second sub-filter Select as: Where (φ) T 、(δv n ) T 、(δp) T 、(ε b ) T , They represent attitude error, velocity error, position error, gyroscope bias and accelerometer bias respectively, (δp D ) T It represents the displacement error obtained by dead reckoning of the wheel odometer; The observation vector of the second sub-filter and the observation matrix They are: in, It represents the position information solved by the strapdown inertial navigation system. Indicates the position information obtained by dead reckoning using a wheel odometer; 0 3×3 is a 3×3 zero matrix, I 3×3 is a 3×3 unit matrix; Step 3.4, the sub-filter is based on Kalman filtering: In the formula, is the prior estimate at time k, is the posterior estimate at time k-1; Step 3.5, calculate the state one-step prediction mean square error matrix: Where P k-1 is the mean square error matrix of the state estimation at time k-1, Q k-1 is the system process noise matrix; Step 3.6, calculate the filter gain: In the formula, K k is the filter gain at time k, R k is the observation noise matrix at time k; Step 3.7, update the state estimate to: In the formula is the posterior estimate at time k; Step 3.8, update the state estimation mean square error matrix: P k =(I-K k H k )P k / k-1 (12) Where P k is the mean square error matrix of the state estimation at time k.

3. The multi-source navigation federated filtering method based on exponential decay adaptive information allocation according to claim 1, characterized in that: In step 4, the filtering results of the first sub-filter and the second sub-filter are output to the main filter, and the common state quantities output by the two sub-filters, namely, speed, position, and posture, are fused in the main filter, as follows: First, take the common state of the sub-filters, and let Represents the common state of sub-filter i, taking That is, attitude error, velocity error, position error, gyroscope bias, and accelerometer bias are taken as common states; Represents the exclusive state of sub-filter i; in the main filter, only the common state shared by all sub-filters can be fused and reset, and non-public states cannot be fused; the state estimation and mean square error matrix of the i-th sub-filter at time k are set to be: Each sub-filter undergoes information fusion: After information fusion, the global optimal state estimation is obtained and the state estimation mean square error matrix 4. The multi-source navigation federated filtering method based on exponential decay adaptive information allocation according to claim 1, characterized in that: In step 5, an information allocation strategy based on the exponential decay formula of the Frobenius norm of the P matrix is ​​constructed, and the information allocation is completed by using the exponential decay-based adaptive information allocation method to obtain the expression of the information allocation factor and calculate the information allocation factor, as follows: Step 5.1: Using the federated filter with feedback reset structure, multiply the state estimation mean square error matrix of the public state by the information allocation factor and feed it back to each sub-filter: Among them, β i Assign factors to information, satisfying the following relationship: Among them, β m is the information allocation factor of the main filter, β i is the information allocation factor of each sub-filter. According to the different structures of the federated filter and the different selection strategies of information allocation, the information allocation factor β m , β i There are different ways of taking values, so the federated filter has different structures; Step 5.2, the information allocation factor affects the system's utilization weight of the sub-filter navigation information. The smaller the information allocation factor, the lower the utilization weight. In order to make the fusion result more affected by the sub-filter with high precision, the information allocation factor of the sub-filter with higher precision should be larger. Therefore, the information allocation strategy based on the exponential decay formula of the Frobenius norm of the P matrix is ​​constructed as follows:

5. A multi-source navigation federated filtering system based on exponential decay adaptive information allocation, characterized in that: The system is used to implement the multi-source navigation federated filtering method based on exponential decay adaptive information allocation according to any one of claims 1 to 4, and the system includes a first module to a fourth module, wherein the functions of each module are specifically as follows: The first module acquires data from the strapdown inertial navigation system SINS, the global navigation satellite system GNSS, and the wheel odometer ODOM; The second module uses SINS as the reference system in the federated filter, solves the SINS data in real time, and inputs the results of the SINS solution into the main filter, the first sub-filter, and the second sub-filter respectively; The first sub-filter uses GNSS data as observations for Kalman filtering; the second sub-filter first uses ODOM data and SINS data to perform dead reckoning, and then uses the reckoning results as observations for Kalman filtering; the filtering results of the first and second sub-filters are output to the main filter, and the common state quantities output by the two sub-filters, namely speed, position, and attitude, are fused in the main filter; The third module constructs an information allocation strategy based on the exponential decay formula of the Frobenius norm of the P matrix, uses an exponential decay-based adaptive information allocation method to complete information allocation, obtains the expression of the information allocation factor, and calculates the information allocation factor; In the fourth module, the main filter multiplies the fused common state quantity and the state estimation mean square error matrix of the common state by the information distribution factor, and feeds them back to each sub-filter to complete the task of feedback reset.

6. 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 multi-source navigation federated filtering method based on exponential decay adaptive information allocation as described in any one of claims 1 to 4 is implemented.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the multi-source navigation federated filtering method based on exponential decay adaptive information allocation as described in any one of claims 1 to 4 are implemented.

Images