Underwater multi-source autonomous navigation system detectability quantitative model and filtering method

Abstract

The invention discloses a detectability quantitative model and a filtering method for an underwater multi-source autonomous navigation system, and belongs to an underwater robot integrated navigation and integrated positioning technology. The method comprises the following steps: firstly, respectively carrying out error modeling on SINS, DVL and USBL sensors based on an SINS / DVL / USBL integrated navigation model; establishing a multi-dimensional detectability quantitative model from four dimensions of coverage rate, accuracy rate, real-time rate and availability rate; and finally, based on the established detectability quantitative model, the detectability of the sensor is incorporated into an adaptive Kalman filtering algorithm so as to optimize the problem of computing resource allocation during data fusion. Compared with a traditional detectability method, the underwater environment interference is considered to establish a multi-dimensional detectability quantitative model, a corresponding algorithm is proposed to dynamically allocate computing resources, and the precision and robustness of the underwater vehicle multi-source autonomous navigation system are further improved.

Description

Technical Field

[0001] The present invention belongs to the technology of integrated navigation and positioning of underwater robots, and particularly relates to a quantifiable model and filtering method for the detectability of an underwater multi-source autonomous navigation system. Background Art

[0002] The multi-source autonomous navigation system is an important navigation tool for tasks such as marine resource exploration, environmental monitoring, and dam inspection. Its accuracy and robustness are directly related to the success rate of the tasks. To achieve high-precision autonomous navigation underwater, the multi-source autonomous navigation system mainly relies on sensors such as strapdown inertial navigation system (SINS), Doppler velocity log (DVL), and ultra-short baseline (USBL) for multi-source data fusion. Due to the complex and changeable underwater environment, these sensors are vulnerable to interference, which affects the navigation results and leads to the accumulation of navigation errors. Therefore, the detectability of multi-source sensors and the adaptive optimization of the fusion strategy after detection are of great significance.

[0003] Regarding the multi-source sensor data fusion method, significant progress has been made in related research. However, traditional data fusion methods mostly adopt fixed weight strategies. When the quality of sensor data deteriorates, they cannot dynamically adjust the fusion weights to maintain the navigation accuracy. To solve this problem, a quantifiable method for the detectability of multi-source sensors is introduced. The detectability of each sensor is quantified through the index of detectability to evaluate its health status, thereby providing a quantifiable index for the data fusion strategy. Summary of the Invention

[0004] Object of the Invention: The object of the present invention is to provide a quantifiable model and filtering method for the detectability of an underwater multi-source autonomous navigation system in view of the above problems. First, error models are established for SINS / DVL / USBL respectively and their root mean square errors are calculated. Secondly, considering the influence of underwater environmental factors, the present invention introduces an environmental interference factor to correct the sensor detectability to better conform to the actual underwater situation, and a multi-dimensional quantifiable model for detectability is established from four dimensions: coverage rate, accuracy rate, real-time rate, and availability rate to quantify the detectability of multi-source sensors. Finally, based on the results of the multi-dimensional quantifiable model for detectability, the detectability of multi-source sensors is incorporated into the adaptive Kalman filter optimization algorithm to dynamically adjust the data fusion strategy or reallocate computing resources, thereby improving the overall reliability and robustness of the navigation system.

[0005] The above object is achieved by the following technical solutions:

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

[0007] The parameters of the strapdown inertial navigation system SINS include the state vector : , where , , respectively represent the velocity errors in the east, north, and up directions in the navigation coordinate system; , , respectively represent the errors of the platform relative to the navigation coordinate system around , , axes; , , respectively represent the longitude, latitude, and altitude errors in the navigation coordinate system; , , respectively represent the zero bias errors of the gyroscope in the , , axis directions; the superscript T represents the transpose of the matrix; , , respectively represent the constant zero bias errors of the accelerometer in the , , axis directions; the superscript T represents the transpose of the matrix;

[0008] The parameters of the Doppler velocity log DVL include the velocity estimate in its own coordinate system , the direction cosine matrix of the installation error angle , the scale factor error , the lever arm error ;

[0009] The parameters of the ultra-short baseline USBL include the transponder coordinates , the lever arm , the conversion matrix from the acoustic array coordinate system to the vehicle coordinate system , the output coordinates , the calibration matrix ;

[0010] The environmental parameters include the environmental interference factor , the influence coefficient , the water flow velocity , the temperature , the salinity , the sound propagation delay , and its weight , , , ;

[0011] The th weight of the accuracy index for quantifying the detectability of the sensor , the weight of the real-time rate index , and the weights of the availability index ;

[0012] The parameters of the adaptive Kalman filtering algorithm include the state transition matrix , the process noise covariance matrix , and the dynamic measurement noise covariance under the initial conditions of multi-source sensors ;

[0013] The initial contribution weight of sensor data fusion ;

[0014] The method specifically includes the following steps:

[0015] Step 1, error modeling is performed on the SINS, DVL, and USBL sensors respectively based on the SINS / DVL / USBL integrated navigation model;

[0016] Step 2, a multi-dimensional detectability quantization model is established from four dimensions of coverage rate, accuracy rate, real-time rate, and availability rate;

[0017] Step 3, based on the detectability quantization model established in Step 2, the sensor detectability is incorporated into the adaptive Kalman filtering algorithm to optimize the calculation resource allocation problem during data fusion.

[0018] Furthermore, the specific method of Step 1 is as follows:

[0019] The state equation of SINS is: , where represents the state vector, represents the system matrix, represents the system noise matrix, represents the state vector derivative;

[0020] The state vector is 15-dimensional and is expressed as follows: ,

[0021] Then, the root mean square error of SINS is calculated as the SINS error index for use in Step 2;

[0022] The velocity information of DVL in the body coordinate system is expressed as: V DVL b = C d b [ V ^ DVL d / (1 + δK)] - ω ^ nb b × L DVL b , wherein, represents the direction cosine matrix of the DVL installation error angle; represents the velocity estimation of the DVL in its own coordinate system; represents the DVL scale factor error; represents the DVL lever arm error; represents the angular velocity estimation of the carrier in the carrier coordinate system;

[0023] Similarly, calculate the root mean square error of the DVL for use in step two;

[0024] The position information in the navigation system observed by USBL is expressed as: , wherein, represents the transponder coordinates; represents the lever arm; represents the transformation matrix from the acoustic array coordinate system to the carrier coordinate system; represents the output coordinates of the USBL; represents the transformation matrix from the carrier coordinate system to the navigation system; calibration matrix is further expressed as: , wherein, represents the radius of curvature of the meridian; represents the depth; represents the secant of the latitude; represents the radius of curvature of the prime vertical;

[0025] At this time, calculate the root mean square error of the USBL for use in step two.

[0026] Furthermore, the specific method of step two is as follows:

[0027] Use the detectability as the detectability quantification index, and the detectability formula for the SINS / DVL / USBL multi-source sensors is as follows: D i = k i [ α i a i + β i b i + γ i c i ] , wherein, represents the The coverage rate index of a sensor, specifically represented as the proportion of the data output duration within the working cycle; represents the real-time rate index of the -th sensor, specifically represented as the data update frequency; represents the availability rate index of the -th sensor, specifically represented as the effective data acquisition rate; , , are respectively the weights of the accuracy rate index, real-time rate index, and availability rate index of the -th sensor, determined according to the working experience of the sensor, and satisfy ; is a fixed value, taking 1, 2, 3 corresponding to SINS, DVL, and USBL sensors respectively; is the -th sensor's accuracy rate, expressed as follows: , wherein, represents the -th sensor's root mean square error;

[0028] The coverage rate index is specifically represented as: , wherein, represents the duration of the sensor's working cycle, represents the data output duration within the working cycle;

[0029] The real-time rate index is specifically represented as: , wherein, represents the nominal frequency of the sensor; represents the actual data update frequency;

[0030] The availability rate index is specifically represented as: , wherein, represents the total amount of data collected within the working cycle; represents the total amount of effective data therein;

[0031] Furthermore, considering the interference in the underwater environment, the environmental interference factor and its influence coefficient are introduced. The environmental interference factor considers factors such as water flow velocity, temperature, salinity, and sound propagation delay. After measuring its value, it is normalized to be maintained at the same order of magnitude and is expressed as: , wherein, represents the water flow velocity; represents the temperature; represents the salinity; represents the sound propagation delay; , , , respectively represent their weights, which are obtained by fitting sensor data and satisfy ; the influence coefficient is obtained through sensitivity analysis and is used to amplify or reduce the environmental interference factor ;

[0032] Thus, the detectable degree after environmental correction is obtained : ,

[0033] For the SINS / DVL / USBL combined autonomous navigation system, its detectability is quantified as: , where is the initial contribution weight of the fusion of each sensor data, and there is , represents the detectability of the navigation system.

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

[0035] At time According to the system state transition model, the state and covariance at time can be predicted as follows: , , wherein, represents the state estimate at time represents the predicted system state at time ; represents the state transition matrix; represents the predicted error covariance; represents the estimation error covariance; represents the process noise covariance matrix;

[0036] For each sensor, first, the measurement noise covariance is dynamically adjusted with the detectable degree after environmental correction, and the specific calculation is as follows: , wherein, Indicates the moment The dynamic measurement noise covariance of the sensor, Indicates the dynamic measurement noise covariance of the sensor under initial conditions; Indicates the detectability of the sensor after environmental interference correction;

[0037] Then, calculate the Kalman gain of each sensor using the dynamic measurement noise covariance: , where Indicates the Kalman gain of the sensor at the moment ; Indicates the measurement matrix;

[0038] Finally, calculate the estimated state of each sensor through the Kalman gain: , where Indicates the state estimate calculated through the Kalman gain; Indicates the measurement data of the sensor at the moment ;

[0039] Then, assign its fusion weight using the detectability and perform normalization processing: ,

[0040] where Indicates the weight of the sensor in data fusion, and satisfies ;

[0041] According to fuse the data of each sensor to obtain the final state estimate and the fused error covariance : , , where Indicates the fused error covariance; Indicates the identity matrix.

[0042] Beneficial effects:

[0043] 1. The present invention aims at an underwater multi-source autonomous navigation system, and establishes a quantifiable model for the detectability of the multi-source autonomous navigation system from four dimensions of coverage rate, accuracy rate, real-time rate and availability rate according to the detectability theory, and quantifies the detectability of multi-source sensors.

[0044] 2. Regarding the problem of quantifying detectability, according to the actual underwater situation, the present invention considers the influence of factors such as water flow velocity, temperature, salinity, and sound propagation delay on multi-source sensors, introduces an environmental interference factor to correct the detectability, and further improves the accuracy of the underwater multi-dimensional detectability quantification model.

[0045] 3. Based on the quantifiable results of the detectability of the underwater multi-source autonomous navigation system, the present invention incorporates the detectability of the multi-source sensors corrected by the environment into the adaptive Kalman filtering algorithm, and improves the accuracy and robustness of the navigation results by optimizing the dynamic allocation of computing resources during data fusion. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0047] Figure 2 is a simulation dead reckoning trajectory diagram of the method of the present invention;

[0048] Figure 3 are the eastward position error, northward position error and vertical position error curves of the simulation dead reckoning of the method of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0049] Refer to Figures 1 - 3 As shown, the quantifiable model for the detectability of the underwater multi-source autonomous navigation system and the filtering method of the present invention are based on the known quantities:

[0050] The parameters of the strapdown inertial navigation system SINS include the state vector : where , , respectively represent the velocity errors in the east, north and vertical directions in the navigation coordinate system; , , respectively represent the errors of the platform relative to the navigation coordinate system around , , axes; , , respectively represent the longitude, latitude and altitude errors in the navigation coordinate system; , , respectively represent the gyroscopes at , , Zero bias error in the axis direction; , , respectively represent the constant zero bias errors of the accelerometer in the , , axis directions; the superscript T represents the transpose of the matrix;

[0051] The parameters of the Doppler velocity log (DVL) include the velocity estimation in its own coordinate system , the direction cosine matrix of the installation error angle , the scale factor error , the lever arm error ;

[0052] The parameters of the ultra-short baseline (USBL) include the transponder coordinates , the lever arm , the transformation matrix from the acoustic array coordinate system to the carrier coordinate system , the output coordinates , the calibration matrix ;

[0053] The environmental parameters include the environmental interference factor , the influence coefficient , the water flow velocity , the temperature , the salinity , the acoustic propagation delay , and its weight , , , ;

[0054] The weight of the accuracy index for quantifying the detectability of the th sensor, the weight of the real-time rate index , and the weight of the availability rate index ; ;

[0055] The parameters of the adaptive Kalman filtering algorithm include the state transition matrix , the process noise covariance matrix , the dynamic measurement noise covariance under the initial conditions of multi-source sensors ;

[0056] The initial contribution weight of sensor data fusion . The method includes the following steps:

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

[0058] Error models for SINS, DVL, and USBL sensors are established respectively based on the SINS / DVL / USBL integrated navigation model.

[0059] The state equation of SINS is: , where represents the state vector, represents the derivative of the state vector , represents the system matrix, represents the system noise matrix. The state vector is 15-dimensional and can be expressed as follows: ,

[0060] Then, the root mean square error of SINS is calculated as the SINS error index for use in the detectability quantification part of step two.

[0061] The velocity information of DVL in the body frame can be expressed as: V DVL b = C d b [ V ^ DVL d / (1 + δK)] - ω ^ nb b × L DVL b , where, represents the direction cosine matrix of DVL installation error angle; represents the velocity estimate of DVL in its own coordinate system; represents the DVL scale factor error; represents the DVL lever arm error.

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

[0063] The position information of USBL observed in the navigation frame can be expressed as: , where, represents the transponder coordinates; represents the lever arm; represents the transformation matrix from the hydrophone array coordinate system to the body coordinate system; represents the output coordinates of USBL; represents the transformation matrix from the body frame to the navigation frame; calibration matrix can also be expressed as: , where, Represents the radius of curvature of the meridian; Represents the depth; Represents the secant of the latitude; Represents the radius of curvature of the prime vertical;

[0064] At this time, the root mean square error of the USBL is calculated for use in Step 2.

[0065] The above are the error models of SINS, DVL, and USBL.

[0066] Step 2: Establish a multi-dimensional detectability quantification model:

[0067] Next, according to the detectability theory, a multi-dimensional detectability quantification model is established from four dimensions: coverage rate, accuracy rate, real-time rate, and availability rate. The detectability is used as the detectability quantification index. The detectability formula for the SINS / DVL / USBL multi-source sensors is as follows: D i = k i [ α i a i + β i b i + γ i c i ] , Among them, Represents the coverage rate index of the th sensor, specifically manifested as the proportion of the data output duration within the working cycle; Represents the real-time rate index of the th sensor, specifically manifested as the data update frequency; Represents the availability rate index of the th sensor, specifically manifested as the effective data acquisition rate; , , Are respectively the weights of the accuracy rate index, real-time rate index, and availability rate index of the th sensor, which can be determined according to the sensor working experience and satisfy ; Is a fixed value, taking 1, 2, 3 corresponding to the SINS, DVL, and USBL sensors respectively; Is the accuracy rate of the th sensor, which can be expressed as follows: , Among them, Represents the root mean square error of the th sensor.

[0068] Coverage rate index Specifically, it can be expressed as: , wherein, represents the working cycle duration of the sensor; represents the data output duration within the working cycle.

[0069] Real-time rate index Specifically, it can be expressed as: , wherein, represents the nominal frequency of the sensor; represents the actual data update frequency.

[0070] Availability rate index Specifically, it can be expressed as: , wherein, represents the total amount of data collected within the working cycle; represents the total amount of valid data therein.

[0071] Furthermore, considering the interference of the underwater environment, an environmental interference factor and its influence coefficient are introduced. The environmental interference factor mainly considers factors such as water flow velocity, temperature, salinity, and sound propagation delay. After measuring their values, they are kept at the same order of magnitude through normalization processing and can be expressed as: , wherein, represents the water flow velocity; represents the temperature; represents the salinity; represents the sound propagation delay; , , , respectively represent their weights, which can be obtained by fitting sensor data and satisfy ; The influence coefficient is obtained through sensitivity analysis and is used to amplify or reduce the environmental interference factor .

[0072] Thus, the detectable degree after environmental correction is obtained: ,

[0073] For the SINS / DVL / USBL combined autonomous navigation system, its detectability is quantified as: , wherein is the weight assigned to each sensor according to its contribution, and there is , representing the detectability of the navigation system.

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

[0075] To improve the overall reliability and robustness of the navigation system, based on the above navigation system detectability quantification model, the present invention incorporates the sensor detectability into the adaptive Kalman filter algorithm to optimize the calculation resource allocation problem during data fusion. At time According to the system state transition model, the state and covariance at time can be predicted as follows: , , wherein represents the state estimate at time represents the predicted system state at time ; represents the state transition matrix; represents the predicted error covariance; represents the estimation error covariance; represents the process noise covariance matrix.

[0076] For each sensor, first dynamically adjust its measurement noise covariance with the detectability corrected by the environment, and the specific calculation is as follows: , wherein represents the dynamic measurement noise covariance of the th sensor at time represents the dynamic measurement noise covariance of the th sensor under the initial conditions; represents the detectability of the th sensor corrected by environmental interference.

[0077] Then calculate the Kalman gain of each sensor with the dynamic measurement noise covariance: , wherein represents the Kalman gain of the th sensor at time ; represents the measurement matrix.

[0078] Finally, the estimated states of each sensor are calculated through the Kalman gain: , where, represents the state estimate calculated through the Kalman gain; represents the th sensor's measurement data at time .

[0079] In order to dynamically adjust the computing resources during data fusion according to the health degree of each sensor, the detectability is used to allocate its fusion weight and perform normalization processing: , where, represents the weight of the th sensor during data fusion, and satisfies .

[0080] Fusing the data of each sensor according to the above weights, the final state estimate and the fused error covariance can be obtained: , , where, represents the fused error covariance; represents the identity matrix.

[0081] Simulation experiment:

[0082] The proposed detectability quantization model and filtering algorithm of the present invention are simulated on Matlab. The sensor parameters in the experiment are set as follows:

[0083] In terms of inertial navigation, the zero-bias stability of the three-axis fiber optic gyroscope is better than 0.02° / h, and the zero-bias stability of the three-axis quartz accelerometer is better than 100 μg; in terms of DVL, the frequency of the acoustic Doppler velocimeter is 600 KHz, and the bottom-tracking velocity measurement accuracy is ; in terms of USBL, the transponder is deployed about 20 m underwater, and the positioning accuracy is better than . The entire test process lasts for 600 s, and the carrier moves in a circular motion around the transponder.

[0084] To prove the superior performance of the proposed filtering algorithm, a traditional Kalman filtering algorithm is introduced for comparison. The errors of the two in the north, east, and depth directions are respectively statistically analyzed and curves are plotted. The error curves are as shown in Appendix Figure 3 .

[0085] From Appendix Figure 3It can be seen that the error of the traditional Kalman filter algorithm diverges significantly in the northward direction from 0 to 130 s, in the eastward direction from 0 to 270 s, and in the depth direction from 0 to 80 s. However, the errors of the proposed method in the northward, eastward, and depth directions are relatively concentrated, and the error magnitudes are much smaller than those of the traditional Kalman filter. This proves that the proposed method improves the positioning accuracy and robustness compared with the traditional Kalman filter algorithm.

Claims

1. A quantifiable model for detectability and filtering method of an underwater multi-source autonomous navigation system, which is based on the following known quantities: The parameters of the strapdown inertial navigation system SINS include the state vector :[[]] , where , , respectively represent the velocity errors in the east, north, and up directions in the navigation coordinate system; , , respectively represent the errors of the platform relative to the navigation coordinate system about , , axes; , , respectively represent the longitude, latitude, and altitude errors in the navigation coordinate system; , , respectively represent the zero bias errors of the gyroscopes in the , , axis directions; the superscript T represents the transpose of the matrix; , , respectively represent the constant zero bias errors of the accelerometers in the , , axis directions; the superscript T represents the transpose of the matrix; The parameters of the Doppler Velocity Log (DVL) include the velocity estimation in its own coordinate system , the direction cosine matrix of the installation error angle , the scale factor error , the lever arm error ; Ultra-short baseline USBL parameters include transponder coordinates , lever arm , transformation matrix from the acoustic array coordinate system to the vehicle coordinate system , output coordinates , calibration matrix ; The environmental parameters include environmental interference factors , influence coefficients , water flow velocity , temperature , salinity , sound propagation delay , and their weights , , , ; The weight of the accuracy rate index for quantifying the detectability of the first sensor, the weight of the real-time rate index and the weight of the availability rate index ; The parameters of the adaptive Kalman filtering algorithm include the state transition matrix , the process noise covariance matrix , and the dynamic measurement noise covariance under the initial conditions of multi-source sensors ; Initial contribution weight of sensor data fusion ; It is characterized in that The method specifically includes the following steps: Step 1: Error modeling is performed on the SINS, DVL, and USBL sensors respectively based on the SINS / DVL / USBL integrated navigation model; Step 2: A multi-dimensional quantifiable model for detectability is established from four dimensions: coverage rate, accuracy rate, real-time rate, and availability rate; Step 3: Based on the quantifiable model for detectability established in Step 2, the sensor detectability is incorporated into the adaptive Kalman filtering algorithm to optimize the calculation resource allocation problem during data fusion.

2. The quantifiable model and filtering method for the detectability of the underwater multi-source autonomous navigation system according to claim 1, wherein The specific method of Step 1 is as follows: The state equation of SINS is: ， wherein represents the state vector, represents the system matrix, represents the system noise matrix, represents the state vector derivative; State vector It is 15-dimensional and is expressed as follows: ， Then calculate the root mean square error of SINS as the SINS error index for use in Step 2; Velocity information of DVL under the carrier system Expressed as: ， Among them, represents the direction cosine matrix of the DVL installation error angle; represents the velocity estimation of the DVL in its own coordinate system; represents the DVL scale factor error; represents the DVL lever arm error; represents the angular velocity estimation of the carrier in the carrier coordinate system; Similarly, calculate the root mean square error of DVL for use in Step 2; The position information in the navigation system observed by USBL is expressed as: ， Among them, represents the transponder coordinates; represents the lever arm; represents the transformation matrix from the acoustic array coordinate system to the vehicle coordinate system; represents the output coordinates of the USBL; represents the transformation matrix from the vehicle coordinate system to the navigation coordinate system; calibration matrix is also expressed as: ， Among them, represents the radius of curvature of the meridian; represents the depth; represents the secant of the latitude; represents the radius of curvature of the prime vertical; At this time, calculate the root mean square error of USBL for use in Step 2.

3. The underwater multi-source autonomous navigation system detectability quantification model and filtering method according to claim 2, characterized in that, The specific method of Step 2 is as follows: Detectability As a quantifying index for detectability, the detectability formula proposed for SINS / DVL / USBL multi-source sensors is as follows: ， Among them, represents the coverage rate index of the th sensor, which is specifically manifested as the proportion of the data output duration in the working cycle; represents the real-time rate index of the th sensor, which is specifically manifested as the data update frequency; represents the availability rate index of the th sensor, which is specifically manifested as the effective data acquisition rate; , , are respectively the weights of the accuracy rate index, real-time rate index and availability rate index of the th sensor, which are determined according to the working experience of the sensor and satisfy ; is a fixed value, and taking 1, 2, 3 corresponds to SINS, DVL, and USBL sensors respectively; is the accuracy rate of the th sensor, which is expressed as follows: ， Among them, represents the root mean square error of the th sensor; Coverage metric Specifically expressed as: ， Among them, represents the duration of the sensor working cycle, represents the data output duration within the working cycle; Real-time rate indicator Specifically expressed as: ， Among them, represents the nominal frequency of the sensor; represents the actual data update frequency; Availability metric Specifically expressed as: ， Among them, represents the total amount of data collected during the working cycle; represents the total amount of valid data among them; Furthermore, considering the interference of the underwater environment, an environmental interference factor is introduced and its influence coefficient , the environmental interference factor considers factors such as water flow velocity, temperature, salinity, and sound propagation delay. After measuring their values, they are normalized to be maintained at the same order of magnitude, expressed as: ， Among them, represents the water flow velocity; represents the temperature; represents the salinity; represents the sound propagation delay; 、 、 、 respectively represent their weights, which are obtained by fitting sensor data and satisfy ; The influence coefficient is obtained through sensitivity analysis and is used to amplify or reduce the environmental interference factor ; Thus, the detectability after environmental correction is obtained. : ， For the autonomous navigation system combining SINS / DVL / USBL, its detectability is quantified as: ， wherein is the initial contribution weight for the fusion of each sensor data, and there is , represents the detectability of the navigation system.

4. The underwater multi-source autonomous navigation system detectability quantization model and filtering method according to claim 3, characterized in that, Step 3 specifically includes the following sub-steps: At time According to the system state transition model, the state and covariance at time are as follows: ， ， Among them, represents the state estimate at a moment; represents the moment predicted system state; represents the state transition matrix; represents the prediction error covariance; represents the estimation error covariance; represents the process noise covariance matrix; For each sensor, first dynamically adjust its measurement noise covariance with the detectability corrected by the environment, and the specific calculation is as follows: ， Among them, represents the dynamic measurement noise covariance of the th sensor, and represents the dynamic measurement noise covariance of the th sensor under the initial conditions; represents the detectability of the th sensor after being corrected by environmental interference. Then calculate the Kalman gain of each sensor with the dynamic measurement noise covariance: ， Among them, represents the th sensor's Kalman gain at time ; represents the measurement matrix; Finally, calculate the estimated state of each sensor through the Kalman gain: ， Among them, represents the state estimate after Kalman gain calculation; represents the -th sensor's measurement data at time . Then allocate its fusion weight with the detectability and perform normalization processing: ， Among them, represents the weight of the th sensor in data fusion, and satisfies ; According to Fuse the data from each sensor to obtain the final state estimate and the fused error covariance : ， ， Among them, represents the error covariance after fusion; represents the identity matrix.

Images