Self-adaptive federated Kalman filtering method based on detectability and information entropy
By introducing adaptive adjustment of detectability and information entropy into the federated Kalman filtering method, the problem of navigation accuracy degradation caused by fixed sensor weights is solved, and high-precision autonomous navigation of underwater unmanned vehicles in complex environments is realized.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- BEIJING INST OF AEROSPACE CONTROL DEVICES
- Filing Date
- 2025-12-31
- Publication Date
- 2026-04-21
AI Technical Summary
Existing federated Kalman filtering methods cannot adaptively adjust weights based on the real-time availability of sensors and measurement uncertainties, resulting in decreased navigation accuracy and filter divergence for underwater unmanned vehicles in complex environments.
An adaptive federated Kalman filter method based on detectability and information entropy is adopted. By calculating the detectability quantification index and comprehensive information entropy of the sub-filters, the information allocation factor of each sub-filter is dynamically adjusted to achieve adaptive fusion of navigation information.
It improves the navigation accuracy and robustness of the multi-source autonomous navigation system in complex environments, and enhances the system's stability and fault tolerance under conditions of measurement degradation and sudden noise changes.
Smart Images
Figure CN121898413A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to an adaptive federated Kalman filtering method based on detectability and information entropy, belonging to the field of underwater navigation and positioning technology. Background Technology
[0002] Unmanned underwater vehicles (UUVs) have stringent requirements for navigation accuracy and reliability when performing long-endurance, autonomous operations. However, due to the high attenuation of electromagnetic waves in the underwater environment, conventional absolute positioning methods such as GNSS cannot be used directly. Underwater platforms must rely on onboard sensors for autonomous navigation. Single sensors are significantly affected by physical characteristics and the environment, making it difficult to maintain stable accuracy over long periods in complex sea conditions. Therefore, multi-source autonomous navigation technology has gradually become the mainstream solution for underwater positioning of UUVs.
[0003] In existing multi-source autonomous navigation systems, inertial navigation systems (INS) are typically used as the core, offering advantages such as high update rate, good short-term accuracy, and independence from external environments, providing continuous navigation information. However, due to integral drift, INS errors accumulate over time and eventually diverge, requiring external measurements for correction. Doppler logs (DVL) can provide high-precision velocity information under stable seabed or water body tracking conditions, effectively suppressing INS velocity drift. However, DVL is sensitive to changes in altitude, seabed scattering characteristics, and attitude, and is prone to measurement degradation or even interruption when suspended in water, experiencing seabed echo attenuation, or losing lock. Ultra-short baseline (USBL) systems can provide absolute position measurements and correct INS position drift, but their measurement frequency is low, noise is high, and they are limited by the underwater acoustic propagation channel and array geometry. When the carrier exceeds the effective coverage area or operating range of the array, the acoustic link is prone to interruption, resulting in intermittent or unusable position measurements.
[0004] To fully leverage the strengths of each sensor while mitigating its limitations, multi-source autonomous navigation methods such as INS / DVL / USBL are commonly employed in engineering. Federated Kalman filtering, as a typical distributed fusion structure, possesses good fault tolerance and engineering feasibility, making it widely used in multi-source fusion scenarios. However, most existing federated Kalman filters employ fixed information allocation factors, failing to adaptively adjust weights based on changes in sensor real-time availability, detectability, and measurement uncertainty. This can easily lead to a mismatch between information fusion and measurement quality.
[0005] In complex underwater environments, the availability and accuracy of measurements from various sensors fluctuate significantly over time due to factors such as underwater acoustic conditions, seabed topography, hydrodynamics, and maneuvering. If a fixed information allocation strategy is still used, problems such as excessive weighting of low-quality measurements or insufficient weighting of high-quality measurements may occur, leading to decreased navigation accuracy and, in severe cases, even filter divergence, making it difficult to meet the requirements of long-term, high-reliability autonomous navigation for underwater platforms. Therefore, there is an urgent need for a multi-source fusion method that can adaptively adjust weights in real time based on measurement detectability and estimation uncertainty to improve the accuracy and robustness of INS / DVL / USBL multi-source autonomous navigation systems in complex environments. Summary of the Invention
[0006] The technical problem solved by this invention is to overcome the shortcomings of existing technologies and provide an adaptive federated Kalman filter method based on detectability and information entropy. This method dynamically adjusts the influence of each sub-filter on the navigation information output by the navigation system by combining the detectability quantification index and the comprehensive information entropy, thus solving the problem that existing federated Kalman filters, which use fixed weights, cannot adapt to dynamic environments, resulting in low navigation accuracy.
[0007] The technical solution of this invention is: An adaptive federated Kalman filtering method based on detectability and information entropy, comprising the following steps: (1) Construct a multi-source navigation system including INS, DVL and USBL; the multi-source navigation system adopts a federated Kalman filter architecture and has a main filter inside; the INS serves as the global reference system of the multi-source navigation system; the DVL has multiple first sub-filters built in, and the USBL has multiple second sub-filters built in; all the first sub-filters and second sub-filters constitute the sub-filter group of the multi-source navigation system, thereby forming the sub-filter channel of the multi-source navigation system; (2) Establish the error state equation of INS; based on the error state equation of INS, establish the DVL error measurement equation of each first sub-filter; based on the error state equation of INS, establish the USBL error measurement equation of each second sub-filter. (3) Based on the DVL error measurement equation of each first sub-filter and the USBL error measurement equation of each second sub-filter, calculate the detectability quantification index of each sub-filter in the sub-filter bank. (4) Obtain the posterior covariance matrix of each sub-filter in the sub-filter bank, and calculate the comprehensive information entropy of each sub-filter in the sub-filter bank based on the posterior covariance matrix; (5) Calculate the information allocation factor for each sub-filter based on the detectability quantization index and comprehensive information entropy of each sub-filter in the sub-filter bank; (6) Based on the information allocation factor of each sub-filter in the sub-filter bank, the main filter of the multi-source navigation system performs weighted fusion of the state estimates of each sub-filter and weighted fusion of the covariance of each sub-filter; based on the weighted fusion of the state estimates and the weighted fusion of the covariance, the multi-source navigation system outputs navigation information.
[0008] Furthermore, in step (2), the specific process of establishing the error state equation of INS is as follows: (2.1) Obtain the state vector of the ISN based on the current position information of the multi-source navigation system. The expression is
[0009] in, These represent the attitude errors along the X, Y, and Z axes relative to the navigation coordinate system. These represent the velocity errors in the east, north, and sky directions under the navigation coordinate system, respectively. These represent the position errors in the longitude, latitude, and altitude directions, respectively, within the navigation coordinate system. These represent the zero-bias error of the gyroscope in the navigation coordinate system; These are the zero-bias error of the accelerometer in the navigation coordinate system; (2.2) Utilizing the state vector of ISN The error state equation of INS is constructed as follows:
[0010] in, for k The state vector of ISN at time +1, for k The state vector of ISN at time t; This is the error state transition matrix; This is the noise input matrix for the discrete process; This is the process noise vector; Furthermore, in step (2), the DVL error measurement equation for each first sub-filter is established, and the specific expression is as follows:
[0011] in, For the first j The error state vector of the first sub-filter; For the first j DVL measurement matrix of the first sub-filter; for k The state vector of ISN at time t is obtained by iterating the error state equation of INS; For the first jDVL measurement noise of the first sub-filter; In step (2), the USBL error measurement equation for each second sub-filter is established, and the specific expression is as follows:
[0012] in, For the first j The error state vector of the second sub-filter; For the first j USBL measurement matrix of the second sub-filter; for k The state vector of ISN at time t is obtained by iterating the error state equation of INS; For the first j USBL measurement noise of the second sub-filter.
[0013] Furthermore, the specific process for calculating the detectability quantification index of each sub-filter in the sub-filter bank in step (3) is as follows: (3.1) Calculate the detectability information matrix of each sub-filter in the sub-filter bank, using the following formula:
[0014] in, For the first sub-filter bank j The detectability information matrix of each sub-filter For the first j The measurement matrix of the sub-filter; when the... j When the sub-filter is the first sub-filter in DVL The DVL measurement matrix is obtained from the DVL error measurement equation corresponding to this sub-filter; when the... j When the sub-filter is the second sub-filter in USBL The USBL measurement matrix is obtained from the USBL error measurement equation corresponding to this sub-filter. For the first sub-filter bank j The noise covariance matrix of each sub-filter; for Transpose of; for The inverse matrix; (3.2) Based on the detectability information matrix of each sub-filter in the sub-filter bank, calculate the detectability quantization index corresponding to each sub-filter. The calculation formula is as follows:
[0015] in, Indicates the first sub-filter bank jThe quantitative index of detectability of individual sub-filters.
[0016] Furthermore, the specific process of calculating the comprehensive information entropy of each sub-filter in the sub-filter bank based on the posterior covariance matrix in step (4) is as follows: (4.1) Extract the position error sub-covariance matrix and velocity error sub-covariance matrix corresponding to each sub-filter from the posterior covariance matrix of each sub-filter in the sub-filter bank; (4.2) Based on the position error sub-covariance matrix of each sub-filter in the sub-filter bank, calculate the position information entropy of each sub-filter. The calculation formula is as follows:
[0017] in, For the first sub-filter bank j The position information entropy of each sub-filter; For the first sub-filter bank j The position error sub-covariance matrix of each sub-filter; for The dimension; det(.) is the operation to calculate the determinant of a matrix; (4.3) Based on the velocity error sub-covariance matrix of each sub-filter in the sub-filter bank, calculate the velocity information entropy of each sub-filter. The calculation formula is as follows:
[0018] in, For the first sub-filter bank j The velocity information entropy of each sub-filter; For the first sub-filter bank j The velocity error sub-covariance matrix of each sub-filter; for dimensionality; (4.4) Based on the position information entropy and velocity information entropy of each sub-filter in the sub-filter bank, calculate the comprehensive information entropy of each sub-filter. The calculation formula is as follows:
[0019] in, and These are the preset position entropy weighting coefficient and velocity entropy weighting coefficient, respectively.
[0020] Furthermore, the specific steps for calculating the information allocation factor of each sub-filter in step (5) are as follows: (5.1) Normalize the detectability quantization index of each sub-filter in the sub-filter bank to obtain the feedforward prior weight corresponding to each sub-filter. The calculation formula is as follows:
[0021] in, For the first sub-filter bank j Feedforward prior weights of each sub-filter; For the first sub-filter bank j Quantitative metrics for the detectability of individual sub-filters; N This represents the total number of sub-filters in the sub-filter bank; (5.2) Calculate the feedback posterior weight of each sub-filter based on the comprehensive information entropy of each sub-filter in the sub-filter bank; (5.3) Combining the feedforward prior weights and feedback posterior weights of each sub-filter in the sub-filter bank, calculate the information allocation factor for each sub-filter. The calculation formula is as follows:
[0022] in, For the first sub-filter bank j Information allocation factor of sub-filter; For the first sub-filter bank j Feedback posterior weights of sub-filters, For balance coefficient, .
[0023] Furthermore, the specific process for calculating the feedback posterior weights of each sub-filter in step (5.2) is as follows: (5.2.1) Normalize the combined information entropy of all sub-filters in the sub-filter bank to obtain the normalized combined information entropy of each sub-filter. The calculation process is as follows:
[0024] in, For the first sub-filter bank j The combined information entropy of each sub-filter; For the first sub-filter bank j Normalized integrated information entropy of each sub-filter; N This represents the number of all sub-filters in the sub-filter bank; (5.2.2) Using the normalized synthesized information entropy of each sub-filter in the sub-filter bank, calculate the reliability of the entropy inverse function form of each sub-filter, as shown in the formula:
[0025] in, For the first sub-filter bank j The credibility of the entropy inverse function form of each sub-filter It is a regular term; (5.2.3) The credibility of the entropy inverse function form of each sub-filter in the sub-filter bank is normalized to obtain the feedback posterior weight corresponding to each sub-filter. The calculation formula is as follows:
[0026] in, For the first sub-filter bank j The feedback posterior weights of each sub-filter.
[0027] Furthermore, the weighted fusion expression for the state estimation of each sub-filter by the main filter of the multi-source navigation system in step (6) is as follows:
[0028] in, The result is the weighted fusion of the state estimates of each sub-filter; For the first sub-filter bank j State estimation of each sub-filter; For the first sub-filter bank j Information allocation factor for each sub-filter; In step (6), the weighted fusion expression for the covariance of each sub-filter of the main filter of the multi-source navigation system is as follows:
[0029] in, The result is the weighted fusion of the covariance of each sub-filter; For the first sub-filter bank j The covariance of each sub-filter.
[0030] Furthermore, after step (3) is completed, the sub-filters with detectability quantization index less than the preset threshold are turned off. The sub-filters that are turned off will no longer be processed in steps (4) to (6) in order to reduce the impact of sub-filters with detectability quantization index less than the preset threshold on the navigation information output by the multi-source navigation system.
[0031] Secondly, the present invention also proposes a computer program product, characterized in that the computer program product includes a computer program, which, when executed by a processor, implements the steps of the aforementioned adaptive federated Kalman filtering method based on detectability and information entropy.
[0032] The beneficial effects of this invention compared to the prior art are: (1) By combining the detectability quantification index and the comprehensive information entropy, the present invention obtains the information allocation factor for the weighted fusion of each sub-filter, thereby dynamically adjusting the influence of each sub-filter on the navigation information output by the navigation system, and improving the autonomous navigation accuracy and fault tolerance of the multi-source autonomous navigation system in complex environments such as measurement degradation, geometric unobservables, and noise mutations.
[0033] (2) The present invention proposes a detectability quantification index, which adjusts the weights of sub-filters during fusion by quantizing the detectability level of the filter in real time, thereby improving navigation stability in complex environments such as measurement degradation.
[0034] (3) This invention proposes a comprehensive information entropy, which achieves a quantitative evaluation of the reliability of the filter by analyzing the uncertainty of the posterior covariance of the sub-filter, making the information allocation more consistent with the actual filtering effect.
[0035] (4) The present invention directly shuts down sub-filters with detectability quantization index below the threshold, thereby avoiding the influence of sub-filters with low detectability quantization index on the navigation information output by the subsequent system, and further improving the accuracy of the output navigation results. Attached Figure Description
[0036] Figure 1 This is a flowchart of an adaptive federated Kalman filtering method based on detectability and information entropy according to the present invention. Figure 2 This is a technical structure diagram of an adaptive federated Kalman filtering method based on detectability and information entropy according to the present invention. Detailed Implementation
[0037] The specific embodiments of the present invention will now be described in further detail with reference to the accompanying drawings.
[0038] like Figure 1 As shown, this invention provides an adaptive federated Kalman filtering method based on detectability and information entropy. (1) Construct a multi-source navigation system including INS, DVL and USBL; the multi-source navigation system adopts a federated Kalman filter architecture and has a main filter inside; the INS serves as the global reference system of the multi-source navigation system; the DVL has multiple first sub-filters built in, and the USBL has multiple second sub-filters built in; all the first sub-filters and second sub-filters constitute the sub-filter group of the multi-source navigation system, thereby forming the sub-filter channel of the multi-source navigation system; (2) Establish the error state equation of INS; based on the error state equation of INS, establish the DVL error measurement equation of each first sub-filter; based on the error state equation of INS, establish the USBL error measurement equation of each second sub-filter. In step (2), the specific process of establishing the error state equation of INS is as follows: (2.1) Obtain the state vector of ISN The expression is
[0039] in, These represent the attitude errors along the X, Y, and Z axes relative to the navigation coordinate system. These represent the velocity errors in the east, north, and sky directions under the navigation coordinate system, respectively. These represent the position errors in the longitude, latitude, and altitude directions, respectively, within the navigation coordinate system. These represent the zero-bias error of the gyroscope in the navigation coordinate system; These are the zero-bias error of the accelerometer in the navigation coordinate system; The All are considered as first-order Markov processes, with the specific expressions as follows:
[0040] in, , Let these represent the time constants of the first-order Markov process for the gyroscope and accelerometer, respectively. , This represents Gaussian white noise with zero mean in gyroscopes and accelerometers. (2.2) Utilizing the state vector of ISN The error state equation of INS is constructed as follows:
[0041] in, for k The state vector of ISN at time +1, for k The state vector of ISN at time t; This is the error state transition matrix; This is the noise input matrix for the discrete process; This is the process noise vector; Furthermore, in step (2), the DVL error measurement equation for each first sub-filter is established, and the specific expression is as follows:
[0042] in, For the first j The error state vector of the first sub-filter; For the first j DVL measurement matrix of the first sub-filter; fork The state vector of ISN at time t is obtained by iterating the error state equation of INS; For the first j DVL measurement noise of the first sub-filter; The specific expression is
[0043] in, This represents the projection of the velocity calculated by strapdown inertial navigation into the device coordinate system after transformation; This represents the direction cosine matrix from the carrier system to the DVL device system; This represents the direction cosine matrix from the navigation system to the carrier system; It can be represented as:
[0044] In the formula, Let the angle between the DVL beam and the horizontal plane be... =70°; In step (2), the USBL error measurement equation for each second sub-filter is established, and the specific expression is as follows:
[0045] in, For the first j The error state vector of the second sub-filter; For the first j USBL measurement matrix of the second sub-filter; for k The state vector of ISN at time t is obtained by iterating the error state equation of INS; For the first j USBL measurement noise of the second sub-filter; The specific expression is
[0046] In the federated Kalman filter architecture, each sub-filter performs prediction and update, and the specific process can be represented as follows: Prediction steps:
[0047] Update steps:
[0048] superscript Indicates the first Sub-filters, This is the corresponding measurement noise covariance matrix.
[0049] (3) Based on the DVL error measurement equation of each first sub-filter and the USBL error measurement equation of each second sub-filter, calculate the detectability quantification index of each sub-filter in the sub-filter bank. The specific process for calculating the detectability quantification index of each sub-filter in the sub-filter bank in step (3) is as follows: (3.1) Based on the Cramer-Rao inequality and the Fisher information matrix, calculate the detectability information matrix of each sub-filter in the sub-filter bank, as shown in the formula:
[0050] in, For the first sub-filter bank j The detectability information matrix of each sub-filter For the first j The measurement matrix of the sub-filter; when the... j When the sub-filter is the first sub-filter in DVL The DVL measurement matrix is obtained from the DVL error measurement equation corresponding to this sub-filter; when the... j When the sub-filter is the second sub-filter in USBL The USBL measurement matrix is obtained from the USBL error measurement equation corresponding to this sub-filter. For the first sub-filter bank j The noise covariance matrix of each sub-filter; for Transpose of; for The inverse matrix; (3.2) Based on the detectability information matrix of each sub-filter in the sub-filter bank, calculate the detectability quantization index corresponding to each sub-filter. The calculation formula is as follows:
[0051] in, Indicates the first sub-filter bank j The quantitative index of detectability of individual sub-filters.
[0052] Based on step (3), this invention proposes a detectability quantification index, which adjusts the weights of sub-filters during fusion by quantifying the detectability level of the filter in real time, thereby improving navigation stability in complex environments such as measurement degradation.
[0053] (4) Obtain the posterior covariance matrix of each sub-filter in the sub-filter bank, and calculate the comprehensive information entropy of each sub-filter in the sub-filter bank based on the posterior covariance matrix; The specific process of calculating the comprehensive information entropy of each sub-filter in the sub-filter bank based on the posterior covariance matrix in step (4) is as follows: (4.1) Extract the position error sub-covariance matrix and velocity error sub-covariance matrix corresponding to each sub-filter from the posterior covariance matrix of each sub-filter in the sub-filter bank; (4.2) Based on the position error sub-covariance matrix of each sub-filter in the sub-filter bank, calculate the position information entropy of each sub-filter. The calculation formula is as follows:
[0054] in, For the first sub-filter bank j The position information entropy of each sub-filter; For the first sub-filter bank j The position error sub-covariance matrix of each sub-filter; for The dimension; det(.) is the operation to calculate the determinant of a matrix; (4.3) Based on the velocity error sub-covariance matrix of each sub-filter in the sub-filter bank, calculate the velocity information entropy of each sub-filter. The calculation formula is as follows:
[0055] in, For the first sub-filter bank j The velocity information entropy of each sub-filter; For the first sub-filter bank j The velocity error sub-covariance matrix of each sub-filter; for dimensionality; (4.4) Based on the position information entropy and velocity information entropy of each sub-filter in the sub-filter bank, calculate the comprehensive information entropy of each sub-filter. The calculation formula is as follows:
[0056] in, and These are the preset position entropy weighting coefficient and velocity entropy weighting coefficient, respectively.
[0057] Based on step (4), this invention proposes a comprehensive information entropy, which achieves a quantitative evaluation of the reliability of the filter by analyzing the uncertainty of the posterior covariance of the sub-filter, making the information allocation more consistent with the actual filtering effect.
[0058] (5) Calculate the information allocation factor for each sub-filter based on the detectability quantization index and comprehensive information entropy of each sub-filter in the sub-filter bank; The specific steps for calculating the information allocation factor of each sub-filter in step (5) are as follows: (5.1) Normalize the detectability quantization index of each sub-filter in the sub-filter bank to obtain the feedforward prior weight corresponding to each sub-filter. The calculation formula is as follows:
[0059] in, For the first sub-filter bank j Feedforward prior weights of each sub-filter; For the first sub-filter bank j Quantitative metrics for the detectability of individual sub-filters; N This represents the total number of sub-filters in the sub-filter bank; (5.2) Based on the comprehensive information entropy of each sub-filter in the sub-filter bank, calculate the feedback posterior weight of each sub-filter. The specific process is as follows: (5.2.1) Normalize the combined information entropy of all sub-filters in the sub-filter bank to obtain the normalized combined information entropy of each sub-filter. The calculation process is as follows:
[0060] in, For the first sub-filter bank j The combined information entropy of each sub-filter; For the first sub-filter bank j Normalized integrated information entropy of each sub-filter; N This represents the number of all sub-filters in the sub-filter bank; (5.2.2) Using the normalized synthesized information entropy of each sub-filter in the sub-filter bank, calculate the reliability of the entropy inverse function form of each sub-filter, as shown in the formula:
[0061] in, For the first sub-filter bank j The credibility of the entropy inverse function form of each sub-filter It is a regular term; (5.2.3) The credibility of the entropy inverse function form of each sub-filter in the sub-filter bank is normalized to obtain the feedback posterior weight corresponding to each sub-filter. The calculation formula is as follows:
[0062] in, For the first sub-filter bank j The feedback posterior weights of each sub-filter.
[0063] (5.3) Combining the feedforward prior weights and feedback posterior weights of each sub-filter in the sub-filter bank, calculate the information allocation factor for each sub-filter. The calculation formula is as follows:
[0064] in, For the first sub-filter bank j Information allocation factor of sub-filter; For the first sub-filter bank j Feedback posterior weights of sub-filters, For balance coefficient, .
[0065] (6) Based on the information allocation factor of each sub-filter in the sub-filter bank, the main filter of the multi-source navigation system performs weighted fusion of the state estimates of each sub-filter and weighted fusion of the covariance of each sub-filter; based on the weighted fusion of the state estimates and the weighted fusion of the covariance, the multi-source navigation system outputs navigation information. The weighted fusion expression for the state estimation of each sub-filter by the main filter of the multi-source navigation system in step (6) is as follows:
[0066] in, The result is the weighted fusion of the state estimates of each sub-filter; For the first sub-filter bank j State estimation of each sub-filter; For the first sub-filter bank j Information allocation factor for each sub-filter; In step (6), the weighted fusion expression for the covariance of each sub-filter of the main filter of the multi-source navigation system is as follows:
[0067] in, The result is the weighted fusion of the covariance of each sub-filter; For the first sub-filter bank j The covariance of each sub-filter.
[0068] Furthermore, after step (3) is completed, the sub-filters with detectability quantization index less than the preset threshold are turned off. The sub-filters that are turned off will no longer be processed in steps (4) to (6) in order to reduce the impact of sub-filters with detectability quantization index less than the preset threshold on the navigation information output by the multi-source navigation system, thereby further improving the accuracy of the navigation results.
[0069] Based on the above process, the technical structure of the multi-source navigation system of the present invention is as follows: Figure 2As shown, this invention obtains the information allocation factor for weighted fusion of each sub-filter by combining the detectability quantification index and the comprehensive information entropy. This allows for dynamic adjustment of the influence of each sub-filter on the navigation information output by the navigation system, thereby improving the autonomous navigation accuracy and fault tolerance of the multi-source autonomous navigation system in complex environments such as measurement degradation, geometric unobservability, and sudden noise changes.
[0070] Secondly, the present invention also proposes a computer program product, characterized in that the computer program product includes a computer program, which, when executed by a processor, implements the steps of the aforementioned adaptive federated Kalman filtering method based on detectability and information entropy.
[0071] The parts of this invention not described in detail are common knowledge to those skilled in the art.
Claims
1. An adaptive federated Kalman filtering method based on detectability and information entropy, characterized in that... The steps are as follows: (1) Construct a multi-source navigation system including INS, DVL and USBL; the multi-source navigation system adopts a federated Kalman filter architecture and has a main filter inside; the INS serves as the global reference system of the multi-source navigation system; the DVL has multiple first sub-filters built in, and the USBL has multiple second sub-filters built in; all the first sub-filters and second sub-filters constitute the sub-filter group of the multi-source navigation system, thereby forming the sub-filter channel of the multi-source navigation system; (2) Establish the error state equation of INS; based on the error state equation of INS, establish the DVL error measurement equation of each first sub-filter; based on the error state equation of INS, establish the USBL error measurement equation of each second sub-filter. (3) Based on the DVL error measurement equation of each first sub-filter and the USBL error measurement equation of each second sub-filter, calculate the detectability quantification index of each sub-filter in the sub-filter bank. (4) Obtain the posterior covariance matrix of each sub-filter in the sub-filter bank, and calculate the comprehensive information entropy of each sub-filter in the sub-filter bank based on the posterior covariance matrix; (5) Calculate the information allocation factor for each sub-filter based on the detectability quantization index and comprehensive information entropy of each sub-filter in the sub-filter bank; (6) Based on the information allocation factor of each sub-filter in the sub-filter bank, the main filter of the multi-source navigation system performs weighted fusion of the state estimation of each sub-filter and weighted fusion of the covariance of each sub-filter; Based on the weighted fusion results of state estimation and covariance, the multi-source navigation system outputs navigation information.
2. The adaptive federated Kalman filtering method based on detectability and information entropy according to claim 1, characterized in that: In step (2), the specific process of establishing the error state equation of INS is as follows: (2.1) Obtain the state vector of the ISN based on the current position information of the multi-source navigation system. The expression is in, These represent the attitude errors along the X, Y, and Z axes relative to the navigation coordinate system. These represent the velocity errors in the east, north, and sky directions under the navigation coordinate system, respectively. These represent the position errors in the longitude, latitude, and altitude directions, respectively, within the navigation coordinate system. These represent the zero-bias error of the gyroscope in the navigation coordinate system; These are the zero-bias error of the accelerometer in the navigation coordinate system; (2.2) Utilizing the state vector of ISN The error state equation of INS is constructed as follows: in, for k The state vector of ISN at time +1, for k The state vector of ISN at time t; This is the error state transition matrix; The noise input matrix for the discrete process; This is the process noise vector.
3. The adaptive federated Kalman filtering method based on detectability and information entropy according to claim 1, characterized in that: In step (2), the DVL error measurement equation for each first sub-filter is established, and the specific expression is as follows: in, For the first j The error state vector of the first sub-filter; For the first j DVL measurement matrix of the first sub-filter; for k The state vector of ISN at time t is obtained by iterating the error state equation of INS; For the first j DVL measurement noise of the first sub-filter; In step (2), the USBL error measurement equation for each second sub-filter is established, and the specific expression is as follows: in, For the first j The error state vector of the second sub-filter; For the first j USBL measurement matrix of the second sub-filter; for k The state vector of ISN at time t is obtained by iterating the error state equation of INS; For the first j USBL measurement noise of the second sub-filter.
4. The adaptive federated Kalman filtering method based on detectability and information entropy according to claim 1, characterized in that: The specific process for calculating the detectability quantification index of each sub-filter in the sub-filter bank in step (3) is as follows: (3.1) Calculate the detectability information matrix of each sub-filter in the sub-filter bank, using the following formula: in, For the first sub-filter bank j The detectability information matrix of each sub-filter For the first j The measurement matrix of the sub-filter; when the... j When the sub-filter is the first sub-filter in DVL The DVL measurement matrix is obtained from the DVL error measurement equation corresponding to this sub-filter; when the... j When the sub-filter is the second sub-filter in USBL The USBL measurement matrix is obtained from the USBL error measurement equation corresponding to this sub-filter. For the first sub-filter bank j The noise covariance matrix of each sub-filter; for Transpose of; for The inverse matrix; (3.2) Based on the detectability information matrix of each sub-filter in the sub-filter bank, calculate the detectability quantization index corresponding to each sub-filter. The calculation formula is as follows: in, Indicates the first sub-filter bank j The quantitative index of the detectability of each sub-filter.
5. The adaptive federated Kalman filtering method based on detectability and information entropy according to claim 1, characterized in that: The specific process of calculating the comprehensive information entropy of each sub-filter in the sub-filter bank based on the posterior covariance matrix in step (4) is as follows: (4.1) Extract the position error sub-covariance matrix and velocity error sub-covariance matrix corresponding to each sub-filter from the posterior covariance matrix of each sub-filter in the sub-filter bank; (4.2) Based on the position error sub-covariance matrix of each sub-filter in the sub-filter bank, calculate the position information entropy of each sub-filter. The calculation formula is as follows: in, For the first sub-filter bank j The position information entropy of each sub-filter; For the first sub-filter bank j The position error sub-covariance matrix of each sub-filter; for The dimension; det(.) is the operation to calculate the determinant of a matrix; (4.3) Based on the velocity error sub-covariance matrix of each sub-filter in the sub-filter bank, calculate the velocity information entropy of each sub-filter. The calculation formula is as follows: in, For the first sub-filter bank j The velocity information entropy of each sub-filter; For the first sub-filter bank j The velocity error sub-covariance matrix of each sub-filter; for dimensionality; (4.4) Based on the position information entropy and velocity information entropy of each sub-filter in the sub-filter bank, calculate the comprehensive information entropy of each sub-filter. The calculation formula is as follows: in, and These are the preset position entropy weighting coefficient and velocity entropy weighting coefficient, respectively.
6. The adaptive federated Kalman filtering method based on detectability and information entropy according to claim 1, characterized in that: The specific steps for calculating the information allocation factor of each sub-filter in step (5) are as follows: (5.1) Normalize the detectability quantization index of each sub-filter in the sub-filter bank to obtain the feedforward prior weight corresponding to each sub-filter. The calculation formula is as follows: in, For the first sub-filter bank j Feedforward prior weights of each sub-filter; For the first sub-filter bank j Quantitative metrics for the detectability of individual sub-filters; N This represents the total number of sub-filters in the sub-filter bank; (5.2) Calculate the feedback posterior weight of each sub-filter based on the comprehensive information entropy of each sub-filter in the sub-filter bank; (5.3) Combining the feedforward prior weights and feedback posterior weights of each sub-filter in the sub-filter bank, calculate the information allocation factor for each sub-filter. The calculation formula is as follows: in, For the first sub-filter bank j Information allocation factor of sub-filter; For the first sub-filter bank j Feedback posterior weights of sub-filters, For balance coefficient, .
7. The adaptive federated Kalman filtering method based on detectability and information entropy according to claim 6, characterized in that: The specific process for calculating the feedback posterior weight of each sub-filter in step (5.2) is as follows: (5.2.1) Normalize the combined information entropy of all sub-filters in the sub-filter bank to obtain the normalized combined information entropy of each sub-filter. The calculation process is as follows: in, For the first sub-filter bank j The combined information entropy of each sub-filter; For the first sub-filter bank j Normalized integrated information entropy of each sub-filter; N This represents the number of all sub-filters in the sub-filter bank; (5.2.2) Using the normalized synthesized information entropy of each sub-filter in the sub-filter bank, calculate the reliability of the entropy inverse function form of each sub-filter, as shown in the formula: in, For the first sub-filter bank j The credibility of the entropy inverse function form of each sub-filter It is a regularization term; (5.2.3) The credibility of the entropy inverse function form of each sub-filter in the sub-filter bank is normalized to obtain the feedback posterior weight corresponding to each sub-filter. The calculation formula is as follows: in, For the first sub-filter bank j The feedback posterior weights of each sub-filter.
8. The adaptive federated Kalman filtering method based on detectability and information entropy according to claim 1, characterized in that: The weighted fusion expression for the state estimation of each sub-filter by the main filter of the multi-source navigation system in step (6) is as follows: in, The result is the weighted fusion of the state estimates of each sub-filter; For the first sub-filter bank j State estimation of each sub-filter; For the first sub-filter bank j Information allocation factor for each sub-filter; In step (6), the weighted fusion expression for the covariance of each sub-filter of the main filter of the multi-source navigation system is as follows: in, The result is the weighted fusion of the covariance of each sub-filter; For the first sub-filter bank j The covariance of each sub-filter.
9. An adaptive federated Kalman filtering method based on detectability and information entropy according to any one of claims 1 to 8, characterized in that: After step (3) is completed, the sub-filters whose detectability quantization index is less than the preset threshold are turned off. The sub-filters that are turned off will no longer be processed in steps (4) to (6) in order to reduce the impact of the sub-filters whose detectability quantization index is less than the preset threshold on the navigation information output by the multi-source navigation system.
10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the steps of the adaptive federated Kalman filtering method based on detectability and information entropy as described in any one of claims 1 to 9.