Quantitative model and filtering method for detectability of underwater multi-source autonomous navigation system

Abstract

The application discloses an underwater multi-source autonomous navigation system detectability quantification model and a filtering method, and belongs to the underwater robot integrated navigation and integrated positioning technology. The application firstly respectively models errors of SINS, DVL and USBL sensors based on an SINS / DVL / USBL integrated navigation model; then establishes a multi-dimensional detectability quantification model from four dimensions of coverage, accuracy, real-time rate and availability; and finally, based on the established detectability quantification model, the sensor detectability is introduced into an adaptive Kalman filtering algorithm to optimize the calculation resource allocation problem during data fusion. Compared with the traditional detectability method, the application considers the establishment of the multi-dimensional detectability quantification model under the underwater environmental interference, and proposes a corresponding algorithm to dynamically allocate the calculation resource, thereby further improving the precision and robustness of the underwater vehicle multi-source autonomous navigation system.

Description

Technical Field

[0001] This invention pertains to underwater robot integrated navigation and positioning technology, specifically involving a detectability quantification model and filtering method for underwater multi-source autonomous navigation systems. Background Technology

[0002] Multi-source autonomous navigation systems (MARS) are crucial navigation tools for tasks such as marine resource exploration, environmental monitoring, and reservoir / dam inspection. Their accuracy and robustness directly impact mission success rates. To achieve high-precision underwater autonomous navigation, MARS primarily rely on strapdown inertial navigation systems (SINS), Doppler velocimeters (DVL), and ultra-short baseline sensors (USBL) for multi-source data fusion. Due to the complex and variable underwater environment, these sensors are susceptible to interference, affecting navigation results and leading to accumulated navigation errors. Therefore, the detectability of multi-source sensors and the adaptive optimization of post-detection fusion strategies are of great significance.

[0003] Significant progress has been made in research on multi-source sensor data fusion methods. However, traditional data fusion methods often employ fixed-weight strategies, which cannot dynamically adjust the fusion weights to maintain navigation accuracy when sensor data quality deteriorates. To address this issue, a multi-source sensor detectability quantification method has been introduced. This method quantifies the detectability of each sensor using the detectability index, thereby assessing its health status and providing a quantitative indicator for the data fusion strategy. Summary of the Invention

[0004] Objective of this invention: To address the aforementioned problems, this invention provides a quantification model and filtering method for the detectability of underwater multi-source autonomous navigation systems. First, error models are established for SINS / DVL / USBL respectively, and their root mean square errors are calculated. Second, considering the influence of underwater environmental factors, this invention introduces an environmental interference factor to correct sensor detectability, thus better reflecting actual underwater conditions. A multi-dimensional detectability quantification model is established from four dimensions: coverage, accuracy, real-time rate, and availability, to quantify the detectability of multi-source sensors. Finally, based on the results of the multi-dimensional detectability quantification model, the detectability of multi-source sensors is incorporated into an adaptive Kalman filter optimization algorithm to dynamically adjust the data fusion strategy or reallocate computational resources, thereby improving the overall reliability and robustness of the navigation system.

[0005] The above objectives are achieved through the following technical solutions:

[0006] A quantification model and filtering method for the detectability of underwater multi-source autonomous navigation systems, based on the following known quantities:

[0007] Strapdown Inertial Navigation System (SINS) parameters include state vectors. :

[0008] ,in , , These represent the velocity errors in the east, north, and sky directions in the navigation coordinate system, respectively. , , These represent the platform's orbit relative to the navigation coordinate system. , , Shaft error; , , These represent the longitude, latitude, and altitude errors in the navigation coordinate system, respectively. , , These represent the gyroscope's position in... , , Zero offset error in the axial direction; , , These represent the accelerometer readings at... , , The constant zero bias error in the axial direction; the superscript T indicates the transpose of the matrix;

[0009] Doppler velocimeter DVL parameters include velocity estimates in its own coordinate system. Installation error angular direction cosine matrix Scale factor error Lever arm error ;

[0010] Ultra-short baseline (USBL) parameters include transponder coordinates. lever arm Transformation matrix from acoustic array coordinate system to carrier coordinate system Output coordinates Correction matrix ;

[0011] Environmental parameters include environmental disturbance factors. Influence coefficient Water flow velocity ,temperature ,salinity Sound propagation delay , and their weights , , , ;

[0012] No. Weights of accuracy metrics for sensor detectability quantification Real-time rate indicator weight Weights of availability metrics ;

[0013] The parameters of the adaptive Kalman filter algorithm include the state transition matrix. Process noise covariance matrix Dynamic measurement noise covariance under initial conditions of multi-source sensors ;

[0014] Initial contribution weights for sensor data fusion ;

[0015] The method specifically includes the following steps:

[0016] Step 1: Based on the SINS / DVL / USBL integrated navigation model, perform error modeling for the SINS, DVL, and USBL sensors respectively;

[0017] Step 2: Establish a multidimensional quantification model of detectability from four dimensions: coverage, accuracy, real-time rate, and availability.

[0018] Step 3: Based on the detectability quantification model established in Step 2, sensor detectability is incorporated into the adaptive Kalman filter algorithm to optimize the allocation of computational resources during data fusion.

[0019] Furthermore, the specific method for step one is as follows:

[0020] The state equation of SINS is:

[0021] ,

[0022] in Represents the state vector. Represents the system matrix. Represents the system noise matrix. State vector The derivative;

[0023] State vector For 15 dimensions, it is represented as follows:

[0024] ,

[0025] Then the root mean square error of SINS is calculated as the SINS error index for use in step two.

[0026] DVL speed information in the load system Represented as:

[0027] V DVL b = C d b [ V ̂ DVL d / (1+δK)]- ω ̂ nb b × L DVL b ,

[0028] in, This represents the direction cosine matrix of the DVL installation error angle; This represents the velocity estimate of DVL in its own coordinate system; Indicates the DVL scale factor error; Indicates DVL lever arm error; This represents the estimate of the angular velocity of the carrier under the load system;

[0029] Similarly, the root mean square error of DVL is calculated for use in step two;

[0030] The position information observed by USBL in the navigation frame is represented as follows:

[0031] ,

[0032] in, Indicates the coordinates of the transponder; Indicates the lever arm; The transformation matrix represents the transformation from the acoustic matrix coordinate system to the carrier coordinate system; Indicates the output coordinates of USBL; The transformation matrix from the carrier system to the navigation system; the correction matrix. It can also be expressed as:

[0033] ,

[0034] in, Indicates the radius of curvature of the meridian; Indicates depth; A secant representing latitude; Indicates the radius of curvature of the zonal parallel;

[0035] At this point, the root mean square error of USBL is calculated for use in step two.

[0036] Furthermore, the specific method for step two is as follows:

[0037] Detectability As a quantitative indicator of detectability, the following formula is proposed for the detectability of SINS / DVL / USBL multi-source sensors:

[0038] D i = k i [ α i a i + β i b i + γ i c i ] ,

[0039] in, Indicates the first The coverage rate of each sensor is specifically reflected in the percentage of data output time within the working cycle; Indicates the first The real-time rate metric for each sensor is specifically reflected in the data update frequency; Indicates the first The availability index of each sensor is specifically reflected in the effective data acquisition rate; , , The first The weights of the sensor accuracy, real-time performance, and availability metrics are determined based on sensor operating experience and must meet the following requirements. ; These are fixed values; values ​​of 1, 2, and 3 correspond to SINS, DVL, and USBL sensors, respectively. For the first The accuracy of each sensor is expressed as follows:

[0040] ,

[0041] in, Indicates the first The root mean square error of each sensor;

[0042] Coverage metrics Specifically, it is expressed as follows:

[0043] ,

[0044] in, Indicates the duration of the sensor's duty cycle. Indicates the duration of data output within a work cycle;

[0045] Real-time rate metric Specifically, it is expressed as follows:

[0046] ,

[0047] in, Indicates the sensor's nominal frequency; Indicates the actual data update frequency;

[0048] Availability metrics Specifically, it is expressed as follows:

[0049] ,

[0050] in, This indicates the total amount of data collected within the work cycle; This indicates the total amount of valid data.

[0051] Furthermore, considering the interference from the underwater environment, environmental interference factors are introduced. and its influence coefficient Environmental interference factors Considering factors such as water flow velocity, temperature, salinity, and sound propagation delay, the measured values ​​are normalized to maintain the same order of magnitude, and are expressed as follows:

[0052] ,

[0053] in, Indicates the speed of water flow; Indicates temperature; Indicates salinity; Indicates the delay in sound propagation; , , , These represent their respective weights, which are obtained through fitting sensor data and satisfy the following conditions: Influence coefficient This is obtained through sensitivity analysis and is used to amplify or reduce environmental disturbance factors. ;

[0054] Thus, the detectability after environmental correction is obtained. :

[0055] ,

[0056] For autonomous navigation systems combining SINS / DVL / USBL, their detectability is quantified as follows:

[0057] ,

[0058] in The initial contribution weights for data fusion from each sensor are given, and there are , This indicates the detectability of the navigation system.

[0059] Furthermore, step three specifically includes the following sub-steps:

[0060] At any moment The timing can be predicted based on the system state transition model. The states and covariances are as follows:

[0061] ,

[0062] ,

[0063] in, express State estimation at time; Indicates time Predicted system state; Represents the state transition matrix; This represents the prediction error covariance; This represents the covariance of the estimation error; Represents the process noise covariance matrix;

[0064] For each sensor, its measurement noise covariance is first dynamically adjusted using the environment-corrected detectability, as calculated below:

[0065] ,

[0066] in, Indicates time No. The dynamic measurement noise covariance of each sensor Indicates the first The dynamic measurement noise covariance of each sensor under initial conditions; Indicates the first Detectability of a sensor after environmental interference correction;

[0067] Then, the Kalman gain of each sensor is calculated using the dynamic measurement noise covariance:

[0068] ,

[0069] in, Indicates the first Each sensor at time Kalman gain; Represents the measurement matrix;

[0070] Finally, the estimated state of each sensor is calculated using Kalman gain:

[0071] ,

[0072] in, This represents the state estimate calculated using the Kalman gain. Indicates the first Each sensor at time Measurement data;

[0073] Then, the fusion weights are assigned using detectability and normalized simultaneously:

[0074] ,

[0075] in, Indicates the first The weight of each sensor in data fusion, and satisfying ;

[0076] according to By fusing data from various sensors, the final state estimate and the fused error covariance are obtained. :

[0077] ,

[0078] ,

[0079] in, This represents the error covariance after fusion; Represents the identity matrix.

[0080] Beneficial effects:

[0081] 1. This invention targets underwater multi-source autonomous navigation systems. Based on the detectability theory, it establishes a quantitative model of the detectability of multi-source autonomous navigation systems from four dimensions: coverage, accuracy, real-time rate, and availability, thereby quantifying the detectability of multi-source sensors.

[0082] 2. This invention addresses the problem of detectability quantification. Based on actual underwater conditions, it considers the influence of factors such as water flow velocity, temperature, salinity, and sound propagation delay on multi-source sensors, and introduces an environmental interference factor to correct detectability, thereby further improving the accuracy of the underwater multidimensional detectability quantification model.

[0083] 3. Based on the detectability quantification results of underwater multi-source autonomous navigation systems, this invention incorporates the environmentally corrected multi-source sensor detectability into an adaptive Kalman filter algorithm, thereby improving the accuracy and robustness of navigation results by optimizing the dynamic allocation of computing resources during data fusion. Attached Figure Description

[0084] Figure 1 This is a flowchart of the method of the present invention;

[0085] Figure 2 This is a simulated dead reckoning trajectory diagram of the method of the present invention;

[0086] Figure 3 The curves show the eastward, northward, and celestial position errors calculated using the simulated dead reckoning method of this invention. Detailed Implementation

[0087] See Figures 1-3As shown, the underwater multi-source autonomous navigation system detectability quantification model and filtering method of the present invention are based on known quantities:

[0088] Strapdown Inertial Navigation System (SINS) parameters include state vectors. :

[0089] ,in , , These represent the velocity errors in the east, north, and sky directions in the navigation coordinate system, respectively. , , These represent the platform's orbit relative to the navigation coordinate system. , , Shaft error; , , These represent the longitude, latitude, and altitude errors in the navigation coordinate system, respectively. , , These represent the gyroscope's position in... , , Zero offset error in the axial direction; , , These represent the accelerometer readings at... , , The constant zero bias error in the axial direction; the superscript T indicates the transpose of the matrix;

[0090] Doppler velocimeter (DVL) parameters include velocity estimates in its own coordinate system. Installation error angular direction cosine matrix Scale factor error Lever arm error ;

[0091] Ultra-short baseline (USBL) parameters include transponder coordinates. lever arm Transformation matrix from acoustic array coordinate system to carrier coordinate system Output coordinates Correction matrix ;

[0092] Environmental parameters include environmental disturbance factors. Influence coefficient Water flow velocity ,temperature ,salinity Sound propagation delay , and their weights , , , ;

[0093] No. Weights of accuracy metrics for sensor detectability quantification Real-time rate indicator weight Weights of availability metrics ;

[0094] The parameters of the adaptive Kalman filter algorithm include the state transition matrix. Process noise covariance matrix Dynamic measurement noise covariance under initial conditions of multi-source sensors ;

[0095] Initial contribution weights for sensor data fusion The method includes the following steps:

[0096] Step 1: Establish the SINS / DVL / USBL error model:

[0097] Error modeling is performed on the SINS, DVL, and USBL sensors based on the SINS / DVL / USBL integrated navigation model.

[0098] The state equation of SINS is:

[0099] ,

[0100] in Represents the state vector. State vector The derivative of Represents the system matrix. This represents the system noise matrix. The state vector is 15-dimensional and can be represented as follows:

[0101] ,

[0102] Then, the root mean square error of SINS is calculated as the SINS error index, which is used in the detectability quantification part of step two.

[0103] DVL speed information in the load system It can be represented as:

[0104] V DVL b = C d b [ V ̂ DVL d / (1+δK)]- ω ̂ nb b × L DVL b ,

[0105] in, This represents the direction cosine matrix of the DVL installation error angle; This represents the velocity estimate of DVL in its own coordinate system; Indicates the DVL scale factor error; This indicates the DVL lever arm error.

[0106] Similarly, the root mean square error of DVL is calculated for use in step two.

[0107] The position information observed by USBL in the navigation frame can be represented as:

[0108] ,

[0109] in, Indicates the coordinates of the transponder; Indicates the lever arm; The transformation matrix represents the transformation from the acoustic matrix coordinate system to the carrier coordinate system; Indicates the output coordinates of USBL; The transformation matrix from the carrier system to the navigation system; the correction matrix. It can also be expressed as:

[0110] ,

[0111] in, Indicates the radius of curvature of the meridian; Indicates depth; A secant representing latitude; Indicates the radius of curvature of the zonal parallel;

[0112] At this point, the root mean square error of USBL is calculated for use in step two.

[0113] The above are the error models for SINS, DVL, and USBL.

[0114] Step 2: Establish a multidimensional detectability quantification model:

[0115] Next, based on detectability theory, a multi-dimensional detectability quantification model is established from four dimensions: coverage, accuracy, real-time rate, and availability. Detectability is used as the quantification index of detectability. The detectability formula proposed for SINS / DVL / USBL multi-source sensors is as follows:

[0116] D i = k i [ α i a i + β i b i + γ i c i ] ,

[0117] in, Indicates the first The coverage rate of each sensor is specifically reflected in the percentage of data output time within the working cycle; Indicates the first The real-time rate metric for each sensor is specifically reflected in the data update frequency; Indicates the first The availability index of each sensor is specifically reflected in the effective data acquisition rate; , , The first The weights of the sensor accuracy, real-time performance, and availability metrics can be determined based on sensor operating experience, and must meet the following requirements: ; These are fixed values; values ​​of 1, 2, and 3 correspond to SINS, DVL, and USBL sensors, respectively. For the first The accuracy of a sensor can be expressed as follows:

[0118] ,

[0119] in, Indicates the first The root mean square error of each sensor.

[0120] Coverage metrics Specifically, it can be expressed as:

[0121] ,

[0122] in, Indicates the duration of the sensor's duty cycle. This indicates the duration of data output within a work cycle.

[0123] Real-time rate metric Specifically, it can be expressed as:

[0124] ,

[0125] in, Indicates the sensor's nominal frequency; This indicates the actual data update frequency.

[0126] Availability metrics Specifically, it can be expressed as:

[0127] ,

[0128] in, This indicates the total amount of data collected within the work cycle; This indicates the total amount of valid data.

[0129] Furthermore, considering the interference from the underwater environment, environmental interference factors are introduced. and its influence coefficient Environmental interference factors The main factors considered are water flow velocity, temperature, salinity, and sound propagation delay. After measuring these values, they are normalized to maintain the same order of magnitude, and can be expressed as:

[0130] ,

[0131] in, Indicates the speed of water flow; Indicates temperature; Indicates salinity; Indicates the delay in sound propagation; , , , Their respective weights can be obtained by fitting sensor data and satisfy the following conditions: Influence coefficient This is obtained through sensitivity analysis and is used to amplify or reduce environmental disturbance factors. .

[0132] Thus, the detectability after environmental correction is obtained:

[0133] ,

[0134] For autonomous navigation systems combining SINS / DVL / USBL, their detectability is quantified as follows:

[0135] ,

[0136] in The weights assigned to each sensor according to its contribution are as follows: , This indicates the detectability of the navigation system.

[0137] Step 3: Design an adaptive Kalman filter optimization algorithm based on detectability:

[0138] To improve the overall reliability and robustness of the navigation system, this invention incorporates sensor detectability into the adaptive Kalman filter algorithm based on the aforementioned navigation system detectability quantification model, thereby optimizing the allocation of computational resources during data fusion. At time... The timing can be predicted based on the system state transition model. The states and covariances are as follows:

[0139] ,

[0140] ,

[0141] in, express State estimation at time; Indicates time Predicted system state; Represents the state transition matrix; This represents the prediction error covariance; This represents the covariance of the estimation error; This represents the process noise covariance matrix.

[0142] For each sensor, its measurement noise covariance is first dynamically adjusted using the environment-corrected detectability, as calculated below:

[0143] ,

[0144] in, Indicates time No. Dynamic measurement noise covariance of each sensor. Indicates the first The dynamic measurement noise covariance of each sensor under initial conditions; Indicates the first The detectability of a sensor after correction for environmental interference.

[0145] Then, the Kalman gain of each sensor is calculated using the dynamic measurement noise covariance:

[0146] ,

[0147] in, Indicates the first Each sensor at time Kalman gain; This represents the measurement matrix.

[0148] Finally, the estimated state of each sensor is calculated using Kalman gain:

[0149] ,

[0150] in, This represents the state estimate calculated using the Kalman gain. Indicates the first Each sensor at time Measurement data.

[0151] To dynamically adjust computational resources for data fusion based on the health status of each sensor in real time, fusion weights are assigned using detectability and then normalized.

[0152] ,

[0153] in, Indicates the first The weight of each sensor in data fusion, and satisfying .

[0154] By fusing the data from each sensor according to the above weights, the final state estimate and the fused error covariance can be obtained:

[0155] ,

[0156] ,

[0157] in, This represents the error covariance after fusion; Represents the identity matrix.

[0158] Simulation experiment:

[0159] The detectability quantization model and filtering algorithm proposed in this invention were simulated in MATLAB. The sensor parameters were set as follows:

[0160] In terms of inertial navigation, the three-axis fiber optic gyroscope exhibits zero-bias stability better than 0.02° / h, and the three-axis quartz accelerometer demonstrates zero-bias stability better than 100ug. Regarding digital velocity measurement (DVL), the acoustic Doppler velocimeter operates at a frequency of 600kHz, achieving a bottom-tracking velocity measurement accuracy of [missing information]. Regarding USBL, the transponder is deployed at a depth of approximately 20 meters underwater, and its positioning accuracy is better than... The entire test lasted 600 seconds, during which the carrier moved in a circular motion around the transponder.

[0161] To demonstrate the superior performance of the proposed filtering algorithm, a comparison with the traditional Kalman filter algorithm is introduced. The errors of both algorithms in the north, east, and depth directions are statistically analyzed and plotted as follows: Figure 3 As shown.

[0162] From the appendix Figure 3 It can be seen that the traditional Kalman filter algorithm exhibits significant error divergence in the northward direction (0-130s), the eastward direction (0-270s), and the depth direction (0-80s). In contrast, the errors of this method are more concentrated in the northward, eastward, and depth directions, and the error magnitude is much smaller than that of the traditional Kalman filter. This demonstrates that the proposed method improves positioning accuracy and robustness compared to the traditional Kalman filter algorithm.

Claims

1. A detectionability quantification model and filtering method for an underwater multi-source autonomous navigation system, the method being based on the following known quantities: Strapdown Inertial Navigation System (SINS) parameters include state vectors. : ,in , , These represent the velocity errors in the east, north, and sky directions in the navigation coordinate system, respectively. , , These represent the platform's orbit relative to the navigation coordinate system. , , Shaft error; , , These represent the longitude, latitude, and altitude errors in the navigation coordinate system, respectively. , , These represent the gyroscope's position in... , , Zero offset error in the axial direction; , , These represent the accelerometer readings at... , , The constant zero bias error in the axial direction; the superscript T indicates the transpose of the matrix; Doppler velocimeter DVL parameters include velocity estimates in its own coordinate system. Installation error angular direction cosine matrix Scale factor error Lever arm error ; Ultra-short baseline (USBL) parameters include transponder coordinates. lever arm Transformation matrix from acoustic array coordinate system to carrier coordinate system Output coordinates Correction matrix ; Environmental parameters include environmental disturbance factors. Influence coefficient Water flow velocity ,temperature ,salinity Sound propagation delay , and their weights , , , ; No. Weights of accuracy metrics for sensor detectability quantification Real-time rate indicator weight Weights of availability metrics ; The parameters of the adaptive Kalman filter algorithm include the state transition matrix. Process noise covariance matrix Dynamic measurement noise covariance under initial conditions of multi-source sensors ; Initial contribution weights for sensor data fusion ; Its features are, The method specifically includes the following steps: Step 1: Based on the SINS / DVL / USBL integrated navigation model, perform error modeling for the SINS, DVL, and USBL sensors respectively; Step 2: Establish a multidimensional quantification model of detectability from four dimensions: coverage, accuracy, real-time rate, and availability. Step 3: Based on the detectability quantification model established in Step 2, sensor detectability is incorporated into the adaptive Kalman filter algorithm to optimize the allocation of computational resources during data fusion. The specific method for step two is as follows: Detectability As a quantitative indicator of detectability, the following formula is proposed for the detectability of SINS / DVL / USBL multi-source sensors: in, Indicates the first The coverage rate of each sensor is specifically reflected in the percentage of data output time within the working cycle; Indicates the first The real-time rate metric for each sensor is specifically reflected in the data update frequency; Indicates the first The availability index of each sensor is specifically reflected in the effective data acquisition rate; , , The first The weights of the sensor accuracy, real-time performance, and availability metrics are determined based on sensor operating experience and must meet the following requirements. ; These are fixed values; values ​​of 1, 2, and 3 correspond to SINS, DVL, and USBL sensors, respectively. For the first The accuracy of each sensor is expressed as follows: in, Indicates the first The root mean square error of each sensor; Coverage metrics Specifically, it is expressed as follows: in, Indicates the duration of the sensor's duty cycle. Indicates the duration of data output within a work cycle; Real-time rate metric Specifically, it is expressed as follows: in, Indicates the sensor's nominal frequency; Indicates the actual data update frequency; Availability metrics Specifically, it is expressed as follows: in, This indicates the total amount of data collected within the work cycle; This indicates the total amount of valid data. Furthermore, considering the interference from the underwater environment, environmental interference factors are introduced. and its influence coefficient Environmental interference factors Considering factors such as water flow velocity, temperature, salinity, and sound propagation delay, the measured values ​​are normalized to maintain the same order of magnitude, and are expressed as follows: in, Indicates the speed of water flow; Indicates temperature; Indicates salinity; Indicates the delay in sound propagation; , , , These represent their respective weights, which are obtained through fitting sensor data and satisfy the following conditions: Influence coefficient This is obtained through sensitivity analysis and is used to amplify or reduce environmental disturbance factors. ; Thus, the detectability after environmental correction is obtained. : For autonomous navigation systems combining SINS / DVL / USBL, their detectability is quantified as follows: in The initial contribution weights for data fusion from each sensor are given, and there are , This indicates the detectability of the navigation system.

2. The underwater multi-source autonomous navigation system detectability quantification model and filtering method according to claim 1, characterized in that, The specific method for step one is as follows: The state equation of SINS is: in Represents the state vector. Represents the system matrix. Represents the system noise matrix. State vector The derivative; State vector For 15 dimensions, it is represented as follows: Then the root mean square error of SINS is calculated as the SINS error index for use in step two. DVL speed information in the load system Represented as: in, This represents the direction cosine matrix of the DVL installation error angle; This represents the velocity estimate of DVL in its own coordinate system; Indicates the DVL scale factor error; Indicates DVL lever arm error; This represents the estimate of the angular velocity of the carrier under the load system; Similarly, the root mean square error of DVL is calculated for use in step two; The position information observed by USBL in the navigation frame is represented as follows: in, Indicates the coordinates of the transponder; Indicates the lever arm; The transformation matrix represents the transformation from the acoustic matrix coordinate system to the carrier coordinate system; Indicates the output coordinates of USBL; The transformation matrix from the carrier system to the navigation system; the correction matrix. It can also be expressed as: in, Indicates the radius of curvature of the meridian; Indicates depth; A secant representing latitude; Indicates the radius of curvature of the zonal parallel; At this point, the root mean square error of USBL is calculated for use in step two.

3. The underwater multi-source autonomous navigation system detectability quantification model and filtering method according to claim 1 or 2, characterized in that, Step three specifically includes the following sub-steps: At any moment The timing can be predicted based on the system state transition model. The states and covariances are as follows: in, express State estimation at time; Indicates time Predicted system state; Represents the state transition matrix; This represents the prediction error covariance; This represents the covariance of the estimation error; Represents the process noise covariance matrix; For each sensor, its measurement noise covariance is first dynamically adjusted using the environment-corrected detectability, as calculated below: in, Indicates time No. The dynamic measurement noise covariance of each sensor Indicates the first The dynamic measurement noise covariance of each sensor under initial conditions; Indicates the first Detectability of a sensor after environmental interference correction; Then, the Kalman gain of each sensor is calculated using the dynamic measurement noise covariance: in, Indicates the first Each sensor at time Kalman gain; Represents the measurement matrix; Finally, the estimated state of each sensor is calculated using Kalman gain: in, This represents the state estimate calculated using the Kalman gain. Indicates the first Each sensor at time Measurement data; Then, the fusion weights are assigned using detectability and normalized simultaneously: in, Indicates the first The weight of each sensor in data fusion, and satisfying ; according to By fusing data from various sensors, the final state estimate and the fused error covariance are obtained. : in, This represents the error covariance after fusion; Represents the identity matrix.

Images