Multi-sensor integrated underwater vehicle navigation and positioning method and system

Abstract

The application provides a kind of underwater vehicle navigation positioning method and system of multi-sensor combination, comprising: step S1: taking the mean of position and attitude angle as initial information;Step S2: time update of Kalman filter is carried out;Step S3: the speed measured with oneself as center of DVL eliminates proportional error and lever error, and the speed measurement value of SINS center is obtained;Step S4: according to whether the underwater vehicle is out of water, select GPS or USBL measurement value to eliminate lever error, and the position measurement value of SINS center is obtained;Step S5: according to the speed measurement value and the position measurement value, the measurement update of Kalman filter is carried out, and the position of underwater vehicle is obtained;Step S6: according to the position of underwater vehicle, the attitude angle is calculated, and the navigation position, speed and attitude angle are output.The application can realize the high-precision, long-time navigation positioning of underwater vehicle.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, and more specifically, to a navigation and positioning method and system for underwater vehicles using a combination of multiple sensors. Background Technology

[0002] In recent years, the exploration of marine resources and underwater unmanned military early warning and countermeasures have become research hotspots both domestically and internationally. The underwater environment is characterized by its enclosed nature and complexity and variability. Underwater vehicles generally need to complete a series of tasks underwater according to a predetermined trajectory. Therefore, the navigation system of underwater vehicles has become a key and challenging aspect of underwater vehicle design.

[0003] Patent document CN102980579A (application number: CN201210460353.0) discloses an autonomous navigation and positioning method for autonomous underwater vehicles (AUVs), particularly a method based on strong tracking capacitive Kalman filtering. This invention includes the following steps: selecting an AUV motion model; selecting an AUV measurement model; constructing a dynamic system using the AUV motion model and measurement model; initializing filter parameters; selecting a fading factor; updating the filter time; updating the filter measurement data; updating from the current time to the next time, repeating steps (5) to (7) until the termination time is reached, ending the calculation, and outputting the result.

[0004] The GPS global positioning system boasts all-weather operation, wide coverage, and high accuracy. However, electromagnetic waves attenuate significantly in water. Choosing GPS as a navigation system necessitates underwater vehicles constantly surfacing to receive signals, severely compromising their stealth capabilities. The SINS (Sinking Inertial Navigation System), based on dead reckoning, typically requires assistance from other navigation systems to compensate for accumulated errors. Doppler logs (DVL) exhibit good long-term stability, with measurement errors not increasing over time, making SINS / DVL integrated navigation systems widely used in underwater environments. However, SINS / DVL systems also inevitably suffer from the problem of navigation errors accumulating over time. Besides surfacing for repositioning, ultra-short baseline (USBL) systems can effectively eliminate accumulated position errors by measuring distances. By combining these four sensor systems, their advantages can be complemented, resulting in superior navigation performance. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the purpose of this invention is to provide a navigation and positioning method and system for underwater vehicles that combines multiple sensors.

[0006] The underwater vehicle navigation and positioning method based on a multi-sensor combination provided by the present invention includes:

[0007] Step S1: When the underwater vehicle is stationary, take the average value of its position and attitude angles over a period of time as the initial information;

[0008] Step S2: When the underwater vehicle is navigating, the attitude angle and acceleration information output by the inertial navigation system SINS are used to update the Kalman filter time.

[0009] Step S3: Eliminate proportional error and lever error from the velocity measured by the Doppler log (DVL) centered on itself to obtain the velocity measurement value of the SINS center;

[0010] Step S4: Depending on whether the underwater vehicle surfaces, select either the GPS navigation system or the Ultra-Short Baseline Positioning System (USBL) measurement value to eliminate the lever error and obtain the position measurement value of the SINS center.

[0011] Step S5: Use the velocity and position measurements from steps S3 and S4 to update the Kalman filter and obtain the position of the underwater vehicle;

[0012] Step S6: Calculate the attitude angles based on the position of the underwater vehicle obtained in step S5, and output the navigation position, velocity, and attitude angles.

[0013] Preferably, step S1 includes: establishing a navigation coordinate system with the initial position of SINS as the origin, the east direction as the x-axis, the north direction as the y-axis, and the sky direction as the z-axis, and establishing a carrier coordinate system with the carrier axis moving forward as the x-axis, the carrier moving to the left as the y-axis, and the carrier moving upward as the z-axis;

[0014] The underwater vehicle remains stationary on the water surface for a period of time. The average position measured by GPS during this period is taken as the initial navigation position of the underwater vehicle, and the average attitude angle measured by SINS during this period is taken as the initial heading and attitude information of the underwater vehicle. The average position measured by GPS under stationary conditions is recorded as _____. The average attitude angle measured by SINS is The initial position coordinates of the unmanned underwater vehicle are X0(x0,y0), the initial velocity is V0(0,0,0), and the initial attitude angle is...

[0015] Step S2 includes:

[0016] The state variables of the Kalman filter are selected as follows:

[0017] X = [xyv] x v y v z ] T

[0018] Where x and y are the position coordinates along the x-axis and y-axis in the underwater vehicle's navigation coordinate system; v x ,v y ,v z The three-axis directional velocities are defined in the coordinate system of the underwater vehicle carrier.

[0019] Based on the motion equations of the underwater vehicle, the state transition equations of the Kalman filter are obtained:

[0020]

[0021] in, F represents the state variables of the Kalman filter; F represents the state transition matrix of the Kalman filter.

[0022]

[0023] Where W is the process noise, which follows a normal distribution W ~ (0, Q); Δt represents the time interval of state transition of the Kalman filter; and Q represents the covariance matrix of the process noise.

[0024] Discretizing the state equations of the Kalman filter above yields:

[0025] X k =Φ k-1 X k-1 +W k-1

[0026] Where k represents the k-th time after the Kalman filter is discretized;

[0027]

[0028] Where I represents the identity matrix;

[0029] The process of using a Kalman filter to calculate the position and velocity of an underwater vehicle through time updates is as follows:

[0030] The expression for calculating the state variables backward is:

[0031] X k =Φ k-1 X k-1 +W k-1

[0032] The forward calculation of the error covariance matrix is ​​expressed as follows:

[0033]

[0034] Among them, X k P represents the state variables of the Kalman filter at time k; k|k-1This represents the prior covariance estimate.

[0035] Preferably, step S3 includes:

[0036] If the speed measured at the DVL center Multiplying it by a scaling factor δk and correcting it, we obtain the measurement velocity at the DVL center. Let the lever arm between the SINS center and the DVL center be l. DVL If the angular acceleration output by SINS is ω, then the lever arm error is ω×l. DVL The measured velocity at the center of the SINS is obtained by subtracting the lever arm error from the velocity measured by the DVL with itself as the center. The expression is:

[0037]

[0038] Among them, l DVL This refers to the lever arm from the center of the SINS to the center of the DVL.

[0039] Preferably, step S4 includes:

[0040] The depth gauge determines whether the underwater vehicle has surfaced. If the underwater vehicle has surfaced, the position measured by GPS is used as the position measurement value of the SINS center after eliminating the lever arm error. If the underwater vehicle has not surfaced, the position measured by USBL is used as the position measurement value of the SINS center after eliminating the lever arm error.

[0041] Let the lever arm connecting the SINS center and the GPS center be l. GPS The lever arm between the SINS center and the USBL center is l USBL The attitude angle output by SINS is The rotation matrix is ​​calculated based on the attitude angles, and the expression is as follows:

[0042]

[0043] If the underwater vehicle surfaces, let the GPS-measured position be... The measured value of the center of SINS is:

[0044]

[0045] in, This indicates the location of the underwater vehicle as determined by GPS.

[0046] If the underwater vehicle does not surface, let the location of the USBL base station be... The USBL mobile station measures a distance of ΔX = [Δx, Δy] from the USBL base station. Therefore, the measured position of the SINS center is:

[0047]

[0048] Preferably, step S5 includes:

[0049] Using position and velocity as measurements, the measurement equations for the Kalman filter are obtained as follows:

[0050]

[0051] Where H represents the measurement matrix of the Kalman filter; v x The velocity along the x-axis in the coordinate system of the underwater vehicle carrier; v y This represents the velocity along the y-axis in the coordinate system of the underwater vehicle; v z U represents the velocity along the z-axis in the coordinate system of the underwater vehicle carrier; U is the measurement noise, which follows a normal distribution U ~ (0, R); R represents the covariance matrix of the measurement noise.

[0052] The measurement equations of the Kalman filter are decomposed into:

[0053]

[0054] This represents the first set of measurement vectors after decomposition. This represents the second set of measurement vectors after decomposition. This represents the first set of measurement matrices after decomposition. This represents the second set of measurement matrices after decomposition; This represents the first set of measurement noise matrices after decomposition. This represents the second set of measurement noise matrices after decomposition;

[0055] in, make x k The x-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; y k This represents the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; v xk v represents the velocity along the x-axis in the coordinate system of the underwater vehicle at time k; yk v represents the y-axis velocity of the underwater vehicle in the coordinate system at time k; zk This represents the velocity along the z-axis in the coordinate system of the underwater vehicle at time k. This represents the first set of measurement noise covariance matrices after decomposition. This represents the second set of measurement noise covariance matrices after decomposition; X represents the initial value of the state variable in the sequential filter. k|k-1 This represents the estimation of prior state variables; P represents the initial value of the prior covariance estimate for sequential filtering;k|k-1 This represents the initial value of the prior covariance estimate;

[0056] Based on the principle of sequential filtering, the computational cost of measurement updates is reduced. If a position measurement value is observed, the first sub-measurement update is performed:

[0057]

[0058] in, This represents the Kalman gain of the first sequential filter. This represents the first posterior state variable estimate in the sequential filtering process. This represents the first posterior covariance estimate of the sequential filtering; This represents the first set of measurement matrices after decomposition. This represents the first set of measurement vectors after decomposition.

[0059] If a velocity measurement is observed, a second sub-measurement update is performed:

[0060]

[0061] Finally, the optimal estimate of the state variables is obtained.

[0062] Among them, P k This represents the final posterior covariance estimate of the sequential filtering observations;

[0063] Step S6 includes:

[0064] Using the pitch angle θ and roll angle φ output by SINS as true values, the yaw angle ψ is calculated using the underwater vehicle's positions at both ends. The expression is:

[0065]

[0066] The underwater vehicle navigation and positioning system based on a multi-sensor combination according to the present invention includes:

[0067] Module M1: When the underwater vehicle is stationary, take the average value of its position and attitude angles over a period of time as initial information;

[0068] Module M2: When the underwater vehicle is navigating, the attitude angle and acceleration information output by the inertial navigation system SINS is used to update the Kalman filter time.

[0069] Module M3: Eliminates proportional error and lever error from the velocity measured by the Doppler log (DVL) centered on itself, and obtains the velocity measurement value of the SINS center;

[0070] Module M4: Selects either GPS navigation system or Ultra-Short Baseline Positioning System (USBL) measurement value to eliminate lever error and obtain the position measurement value of SINS center, depending on whether the underwater vehicle surfaces.

[0071] Module M5: The position of the underwater vehicle is obtained by updating the Kalman filter using the velocity and position measurements from modules M3 and M4.

[0072] Module M6: Calculates attitude angles based on the underwater vehicle's position obtained from Module M5, and outputs navigation position, velocity, and attitude angles.

[0073] Preferably, the module M1 includes: establishing a navigation coordinate system with the initial position of SINS as the origin, the east direction as the x-axis, the north direction as the y-axis, and the sky direction as the z-axis; and establishing a carrier coordinate system with the carrier axis moving forward as the x-axis, the carrier moving to the left as the y-axis, and the carrier moving upward as the z-axis.

[0074] The underwater vehicle remains stationary on the water surface for a period of time. The average position measured by GPS during this period is taken as the initial navigation position of the underwater vehicle, and the average attitude angle measured by SINS during this period is taken as the initial heading and attitude information of the underwater vehicle. The average position measured by GPS under stationary conditions is recorded as _____. The average attitude angle measured by SINS is The initial position coordinates of the unmanned underwater vehicle are X0(x0,y0), the initial velocity is V0(0,0,0), and the initial attitude angle is...

[0075] The module M2 includes:

[0076] The state variables of the Kalman filter are selected as follows:

[0077] X = [xyv] x v y v z ] T

[0078] Where x and y are the position coordinates along the x-axis and y-axis in the underwater vehicle's navigation coordinate system; v x ,v y ,v z The three-axis directional velocities are defined in the coordinate system of the underwater vehicle carrier.

[0079] Based on the motion equations of the underwater vehicle, the state transition equations of the Kalman filter are obtained:

[0080]

[0081] in, F represents the state variables of the Kalman filter; F represents the state transition matrix of the Kalman filter.

[0082]

[0083] Where W is the process noise, which follows a normal distribution W ~ (0, Q); Δt represents the time interval of state transition of the Kalman filter; and Q represents the covariance matrix of the process noise.

[0084] Discretizing the state equations of the Kalman filter above yields:

[0085] X k =Φ k-1 X k-1 +W k-1

[0086] Where k represents the k-th time after the Kalman filter is discretized;

[0087]

[0088] Where I represents the identity matrix;

[0089] The process of using a Kalman filter to calculate the position and velocity of an underwater vehicle through time updates is as follows:

[0090] The expression for calculating the state variables backward is:

[0091] X k =Φ k-1 X k-1 +W k-1

[0092] The forward calculation of the error covariance matrix is ​​expressed as follows:

[0093]

[0094] Among them, X k P represents the state variables of the Kalman filter at time k; k|k-1 This represents the prior covariance estimate.

[0095] Preferably, the module M3 includes:

[0096] If the speed measured at the DVL center Multiplying it by a scaling factor δk and correcting it, we obtain the measurement velocity at the DVL center. Let the lever arm between the SINS center and the DVL center be l. DVL If the angular acceleration output by SINS is ω, then the lever arm error is ω×l. DVL The measured velocity at the center of the SINS is obtained by subtracting the lever arm error from the velocity measured by the DVL with itself as the center. The expression is:

[0097]

[0098] Among them, l DVL This refers to the lever arm from the center of the SINS to the center of the DVL.

[0099] Preferably, the module M4 includes:

[0100] The depth gauge determines whether the underwater vehicle has surfaced. If the underwater vehicle has surfaced, the position measured by GPS is used as the position measurement value of the SINS center after eliminating the lever arm error. If the underwater vehicle has not surfaced, the position measured by USBL is used as the position measurement value of the SINS center after eliminating the lever arm error.

[0101] Let the lever arm connecting the SINS center and the GPS center be l. GPS The lever arm between the SINS center and the USBL center is l USBL The attitude angle output by SINS is The rotation matrix is ​​calculated based on the attitude angles, and the expression is as follows:

[0102]

[0103] If the underwater vehicle surfaces, let the GPS-measured position be... The measured value of the center of SINS is:

[0104]

[0105] in, This indicates the location of the underwater vehicle as determined by GPS.

[0106] If the underwater vehicle does not surface, let the location of the USBL base station be... The USBL mobile station measures a distance of ΔX = [Δx, Δy] from the USBL base station. Therefore, the measured position of the SINS center is:

[0107]

[0108] Preferably, the module M5 includes:

[0109] Using position and velocity as measurements, the measurement equations for the Kalman filter are obtained as follows:

[0110]

[0111] Where H represents the measurement matrix of the Kalman filter; v x The velocity along the x-axis in the coordinate system of the underwater vehicle carrier; v yThis represents the velocity along the y-axis in the coordinate system of the underwater vehicle; v z U represents the velocity along the z-axis in the coordinate system of the underwater vehicle carrier; U is the measurement noise, which follows a normal distribution U ~ (0, R); R represents the covariance matrix of the measurement noise.

[0112] The measurement equations of the Kalman filter are decomposed into:

[0113]

[0114] This represents the first set of measurement vectors after decomposition. This represents the second set of measurement vectors after decomposition. This represents the first set of measurement matrices after decomposition. This represents the second set of measurement matrices after decomposition; This represents the first set of measurement noise matrices after decomposition. This represents the second set of measurement noise matrices after decomposition;

[0115] in, make x k The x-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; y k This represents the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; v xk v represents the velocity along the x-axis in the coordinate system of the underwater vehicle at time k; yk v represents the y-axis velocity of the underwater vehicle in the coordinate system at time k; zk This represents the velocity along the z-axis in the coordinate system of the underwater vehicle at time k. This represents the first set of measurement noise covariance matrices after decomposition. This represents the second set of measurement noise covariance matrices after decomposition; X represents the initial value of the state variable in the sequential filter. k|k-1 This represents the estimation of prior state variables; P represents the initial value of the prior covariance estimate for sequential filtering; k|k-1 This represents the initial value of the prior covariance estimate;

[0116] Based on the principle of sequential filtering, the computational cost of measurement updates is reduced. If a position measurement value is observed, the first sub-measurement update is performed:

[0117]

[0118] in, This represents the Kalman gain of the first sequential filter. This represents the first posterior state variable estimate in the sequential filtering process. This represents the first posterior covariance estimate of the sequential filtering; This represents the first set of measurement matrices after decomposition. This represents the first set of measurement vectors after decomposition.

[0119] If a velocity measurement is observed, a second sub-measurement update is performed:

[0120]

[0121] Finally, the optimal estimate of the state variables is obtained.

[0122] Among them, P k This represents the final posterior covariance estimate of the sequential filtering observations;

[0123] The module M6 includes:

[0124] Using the pitch angle θ and roll angle φ output by SINS as true values, the yaw angle ψ is calculated using the underwater vehicle's positions at both ends. The expression is:

[0125]

[0126] Compared with the prior art, the present invention has the following beneficial effects:

[0127] (1) This invention uses attitude angle and acceleration information directly from SINS to construct a 5D Kalman filter, which effectively solves the shortcomings of conventional underwater navigation and positioning, such as large computational load and low computational efficiency.

[0128] (2) This invention eliminates the proportional error of DVL speed measurement and the lever arm error between DVL / GPS / USBL and SINS, and can achieve high-precision, long-endurance navigation and positioning for underwater vehicles. Attached Figure Description

[0129] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

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

[0131] Figure 2 This is the true location and the estimated trajectory plot. Detailed Implementation

[0132] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0133] Example 1:

[0134] The technical problem this invention aims to solve is to design a high-precision, long-endurance underwater navigation and positioning system using four sensors: SINS, DVL, USBL, and GPS. To address this problem, this invention proposes a corresponding technical solution. It utilizes the attitude angle and acceleration information output by SINS, combined with the motion model of the underwater vehicle, to construct a 5D Kalman filter state equation. The three-dimensional velocity information of the underwater vehicle is measured using DVL, and the three-dimensional position information is measured using USBL / GPS. The measured position and velocity information are then used as observations for sequential filtering in the Kalman filter to obtain accurate position, velocity, and attitude information of the underwater vehicle, thus achieving high-precision, long-endurance navigation and positioning for the underwater vehicle.

[0135] like Figure 1 The main steps of the technical solution of the present invention include:

[0136] Step S1: The underwater vehicle remains stationary for a period of time, and the average value of its position and attitude angles is taken as the initial information.

[0137] A navigation coordinate system is established with the initial position of the SINS as the origin, east as the x-axis, north as the y-axis, and the sky as the z-axis. A carrier coordinate system is established with the carrier axis pointing forward as the x-axis, leftward as the y-axis, and upward as the z-axis. The underwater vehicle remains stationary on the water surface for a period of time. The average position measured by GPS during this period is taken as the initial navigation position of the underwater vehicle, and the average attitude angle measured by SINS during this period is taken as the initial heading and attitude information of the underwater vehicle. Let the average position measured by GPS under stationary conditions be denoted as... The average attitude angle measured by SINS is The initial position coordinates of the unmanned underwater vehicle are X0(x0,y0), the initial velocity is V0(0,0,0), and the initial attitude angle is...

[0138] Step S2: The underwater vehicle navigates, and the time update is performed using the attitude angle and acceleration information output by SINS through Kalman filtering.

[0139] Since the depth information of underwater vehicles is generally obtained directly from depth gauges, the position coordinates of the underwater vehicle along the z-axis are not considered, and the state variables of the Kalman filter are selected as follows:

[0140] X = [xyv] x v y v z ] T

[0141] Where x and y are the position coordinates along the x-axis and y-axis in the underwater vehicle's navigation coordinate system; v x ,v y ,v z Let be the velocities along the three axes in the coordinate system of the underwater vehicle. Based on the motion equations of the underwater vehicle, the state transition equations of the Kalman filter can be obtained:

[0142]

[0143] F represents the state variables of the Kalman filter; F represents the state transition matrix of the Kalman filter.

[0144]

[0145] W is the process noise, which follows a normal distribution W ~ (0, Q); Δt represents the time interval of state transition in the Kalman filter; Q represents the covariance matrix of the process noise;

[0146] Discretizing the state equations of the Kalman filter above yields:

[0147] X k =Φ k-1 X k-1 +W k-1

[0148] k represents the k-th time after the Kalman filter is discretized;

[0149] in:

[0150]

[0151] I represents the identity matrix;

[0152] The steps for using a Kalman filter to calculate the position and velocity of an underwater vehicle using time updates are as follows:

[0153] Forward calculation of state variables:

[0154] X k =Φ k-1 X k-1 +W k-1

[0155] Forward calculation of the error covariance matrix:

[0156]

[0157] X k P represents the state variables of the Kalman filter at time k; k|k-1 This represents the prior covariance estimate.

[0158] Step S3: Eliminate proportional error and lever arm error from the velocity measured by DVL centered on itself to obtain the velocity measurement value of the SINS center.

[0159] If the speed measured at the DVL center Multiplying it by the scaling factor δk and correcting it gives the measurement velocity of the DVL center. Let the lever arm between the SINS center and the DVL center be l. DVL If the angular acceleration output by SINS is ω, then the lever arm error is ω×l. DVL Subtracting the lever arm error from the velocity measured by DVL with itself as the center yields the measured velocity at the center of SINS.

[0160]

[0161] l DVL This refers to the lever arm from the center of the SINS to the center of the DVL.

[0162] Step S4: Select GPS or USBL measurement values ​​to eliminate lever error based on whether the underwater vehicle surfaces, and obtain the position measurement value of the SINS center.

[0163] The depth gauge determines whether the underwater vehicle has surfaced. If it has, the GPS-measured position, after adjusting for lever arm error, is used as the SINS center position. If the underwater vehicle has not surfaced, the USBL-measured position, after adjusting for lever arm error, is used as the SINS center position. Let the lever arm between the SINS center and the GPS center be l. GPS The lever arm between the SINS center and the USBL center is l USBL The attitude angle output by SINS is The rotation matrix can be calculated based on the attitude angle:

[0164]

[0165] If the underwater vehicle surfaces, let the GPS-measured position be... The measured value of the center of SINS is:

[0166]

[0167] This indicates the location of the underwater vehicle as determined by GPS.

[0168] If the underwater vehicle does not surface, let the location of the USBL base station be... The USBL mobile station measures a distance of ΔX = [Δx, Δy] from the USBL base station. Therefore, the measured position of the SINS center is:

[0169]

[0170] Step S5: Select GPS or USBL measurement values ​​to eliminate stick arm error based on whether the underwater vehicle surfaces, and obtain the position measurement value of the SINS center.

[0171] The measurement equations for the Kalman filter, obtained by measuring position and velocity, are as follows:

[0172]

[0173] in

[0174]

[0175] H represents the measurement matrix of the Kalman filter; v x The velocity along the x-axis in the coordinate system of the underwater vehicle carrier; v y This represents the velocity along the y-axis in the coordinate system of the underwater vehicle; v z U represents the velocity along the z-axis in the coordinate system of the underwater vehicle carrier; U is the measurement noise, which follows a normal distribution U ~ (0, R); R represents the covariance matrix of the measurement noise.

[0176] The measurement equations for the Kalman filter described above can be decomposed as follows:

[0177]

[0178] This represents the first set of measurement vectors after decomposition. This represents the second set of measurement vectors after decomposition. This represents the first set of measurement matrices after decomposition. This represents the second set of measurement matrices after decomposition; This represents the first set of measurement noise matrices after decomposition. This represents the second set of measurement noise matrices after decomposition;

[0179] in make

[0180] x k The x-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; y k This represents the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; v xkv represents the velocity along the x-axis in the coordinate system of the underwater vehicle at time k; yk v represents the y-axis velocity of the underwater vehicle in the coordinate system at time k; zk This represents the velocity along the z-axis in the coordinate system of the underwater vehicle at time k. This represents the first set of measurement noise covariance matrices after decomposition. This represents the second set of measurement noise covariance matrices after decomposition; X represents the initial value of the state variable in the sequential filter. k|k-1 This represents the estimation of prior state variables; P represents the initial value of the prior covariance estimate for sequential filtering; k|k-1 This represents the initial value of the prior covariance estimate.

[0181] Based on the principle of sequential filtering, the computational load of measurement updates can be reduced. If a position measurement value is observed, the first sub-measurement update is performed:

[0182]

[0183] This represents the Kalman gain of the first sequential filter. This represents the first posterior state variable estimate in the sequential filtering process. This represents the first posterior covariance estimate of the sequential filtering; This represents the first set of measurement matrices after decomposition. This represents the first set of measurement vectors after decomposition.

[0184] If a velocity measurement is observed, a second sub-measurement update is performed:

[0185]

[0186] Finally, the optimal estimate of the state variables is obtained.

[0187] P k This represents the final posterior covariance estimate of the sequential filtering observations.

[0188] Step S6: Calculate the attitude angles using the position interpolation of the underwater vehicle obtained in step S5, and output the navigation position, velocity, and attitude angles.

[0189] Considering that the pitch angle θ and roll angle φ change relatively little during the underwater vehicle's navigation, and the error in the SINS output is also small, the pitch angle θ and roll angle φ output by the SINS are directly used as the true values. The yaw angle ψ, which has a larger variation error, can be calculated using the underwater vehicle's positions at the beginning and end of its journey.

[0190]

[0191] At this point, we can obtain the underwater vehicle's position (x, y), attitude (θ, φ, ψ), and velocity (v). x ,v y ,v z The optimal estimate.

[0192] The performance of the multi-sensor underwater vehicle navigation and positioning method described above is illustrated through the following experimental example. AUV navigation data was tested on the surface of a reservoir in Zhejiang Province to verify the performance of the integrated navigation algorithm. The total navigation distance was 5 kilometers, taking 60 minutes. Data was simultaneously collected from SINS, DVL, USBL, single-precision GPS, and differential GPS sensors. The differential GPS measurements were used as a benchmark, and the results of the multi-sensor combination were compared with the differential GPS trajectory. Figure 2 As shown in the comparison results of the implementation examples, the maximum positioning error using a multi-sensor combination is 5.63 meters, which is less than one-thousandth of the total path length. This indicates that the proposed method can achieve high-precision, long-endurance navigation and positioning for underwater vehicles.

[0193] Example 2:

[0194] The present invention also provides a multi-sensor underwater vehicle navigation and positioning system, which can be implemented by executing the process steps of the multi-sensor underwater vehicle navigation and positioning method. That is, those skilled in the art can understand the multi-sensor underwater vehicle navigation and positioning method as a preferred embodiment of the multi-sensor underwater vehicle navigation and positioning system.

[0195] The underwater vehicle navigation and positioning system with multi-sensor combination provided by the present invention includes: Module M1: when the underwater vehicle is stationary, taking the average value of position and attitude angles over a period of time as initial information; Module M2: when the underwater vehicle is navigating, using the attitude angle and acceleration information output by the inertial navigation system (SINS) to perform time updates of the Kalman filter; Module M3: eliminating proportional error and lever error from the velocity centered on itself measured by the Doppler log (DVL) to obtain the velocity measurement value of the SINS center; Module M4: selecting the measurement value of the GPS navigation system or the ultra-short baseline positioning system (USBL) to eliminate lever error based on whether the underwater vehicle is surfacing to obtain the position measurement value of the SINS center; Module M5: using the velocity measurement value and position measurement value of modules M3 and M4 to perform measurement updates of the Kalman filter to obtain the position of the underwater vehicle; Module M6: calculating the attitude angles based on the position of the underwater vehicle obtained by module M5, and outputting the navigation position, velocity, and attitude angles.

[0196] The module M1 includes: establishing a navigation coordinate system with the initial position of SINS as the origin, east as the x-axis, north as the y-axis, and the sky as the z-axis; establishing a carrier coordinate system with the carrier axis forward as the x-axis, the carrier to the left as the y-axis, and the carrier upward as the z-axis; the underwater vehicle remains stationary on the water surface for a period of time, and the average position measured by GPS during this period is taken as the initial navigation position of the underwater vehicle; the average attitude angle measured by SINS during this period is taken as the initial heading and attitude information of the underwater vehicle; and the average position measured by GPS under stationary conditions is recorded as... The average attitude angle measured by SINS is The initial position coordinates of the unmanned underwater vehicle are X0(x0,y0), the initial velocity is V0(0,0,0), and the initial attitude angle is...

[0197] The module M2 includes: selecting the state variables of the Kalman filter as:

[0198] X = [xyv] x v y v z ] T

[0199] Where x and y are the position coordinates along the x-axis and y-axis in the underwater vehicle's navigation coordinate system; v x ,v y ,v z The three-axis directional velocities are defined in the coordinate system of the underwater vehicle carrier.

[0200] Based on the motion equations of the underwater vehicle, the state transition equations of the Kalman filter are obtained:

[0201]

[0202] in, F represents the state variables of the Kalman filter; F represents the state transition matrix of the Kalman filter.

[0203]

[0204] Where W is the process noise, which follows a normal distribution W ~ (0, Q); Δt represents the time interval of state transition of the Kalman filter; and Q represents the covariance matrix of the process noise.

[0205] Discretizing the state equations of the Kalman filter above yields:

[0206] X k =Φ k-1 X k-1 +W k-1

[0207] Where k represents the k-th time after the Kalman filter is discretized;

[0208]

[0209] Where I represents the identity matrix;

[0210] The process of using a Kalman filter to calculate the position and velocity of an underwater vehicle through time updates is as follows:

[0211] The expression for calculating the state variables backward is:

[0212] X k =Φ k-1 X k-1 +W k-1

[0213] The forward calculation of the error covariance matrix is ​​expressed as follows:

[0214]

[0215] Among them, X k P represents the state variables of the Kalman filter at time k; k|k-1 This represents the prior covariance estimate.

[0216] The module M3 includes: if the speed measured at the DVL center Multiplying it by a scaling factor δk and correcting it, we obtain the measurement velocity at the DVL center. Let the lever arm between the SINS center and the DVL center be l. DVL If the angular acceleration output by SINS is ω, then the lever arm error is ω×l. DVL The measured velocity at the center of the SINS is obtained by subtracting the lever arm error from the velocity measured by the DVL with itself as the center. The expression is:

[0217]

[0218] Among them, l DVL This refers to the lever arm from the center of the SINS to the center of the DVL.

[0219] The module M4 includes: determining whether the underwater vehicle has surfaced based on the depth gauge; if the underwater vehicle has surfaced, the position measured by GPS, after adjusting for lever arm error, is used as the position measurement value of the SINS center; if the underwater vehicle has not surfaced, the position measured by USBL, after adjusting for lever arm error, is used as the position measurement value of the SINS center; let the lever arm between the SINS center and the GPS center be l. GPS The lever arm between the SINS center and the USBL center is l USBL The attitude angle output by SINS is The rotation matrix is ​​calculated based on the attitude angles, and the expression is as follows:

[0220]

[0221] If the underwater vehicle surfaces, let the GPS-measured position be... The measured value of the center of SINS is:

[0222]

[0223] in, This indicates the location of the underwater vehicle as determined by GPS.

[0224] If the underwater vehicle does not surface, let the location of the USBL base station be... The USBL mobile station measures a distance of ΔX = [Δx, Δy] from the USBL base station. Therefore, the measured position of the SINS center is:

[0225]

[0226] The module M5 includes: using position and velocity as measurements, the measurement equation of the Kalman filter is obtained as follows:

[0227]

[0228] Where H represents the measurement matrix of the Kalman filter; v x The velocity along the x-axis in the coordinate system of the underwater vehicle carrier; v y This represents the velocity along the y-axis in the coordinate system of the underwater vehicle; v z U represents the velocity along the z-axis in the coordinate system of the underwater vehicle carrier; U is the measurement noise, which follows a normal distribution U ~ (0, R); R represents the covariance matrix of the measurement noise.

[0229] The measurement equations of the Kalman filter are decomposed into:

[0230]

[0231] This represents the first set of measurement vectors after decomposition. This represents the second set of measurement vectors after decomposition. This represents the first set of measurement matrices after decomposition. This represents the second set of measurement matrices after decomposition; This represents the first set of measurement noise matrices after decomposition. This represents the second set of measurement noise matrices after decomposition;

[0232] in, make x kThe x-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; y k This represents the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; v xk v represents the velocity along the x-axis in the coordinate system of the underwater vehicle at time k; yk v represents the y-axis velocity of the underwater vehicle in the coordinate system at time k; zk This represents the velocity along the z-axis in the coordinate system of the underwater vehicle at time k. This represents the first set of measurement noise covariance matrices after decomposition. This represents the second set of measurement noise covariance matrices after decomposition; X represents the initial value of the state variable in the sequential filter. k|k-1 This represents the estimation of prior state variables; P represents the initial value of the prior covariance estimate for sequential filtering; k|k-1 This represents the initial value of the prior covariance estimate;

[0233] Based on the principle of sequential filtering, the computational cost of measurement updates is reduced. If a position measurement value is observed, the first sub-measurement update is performed:

[0234]

[0235] in, This represents the Kalman gain of the first sequential filter. This represents the first posterior state variable estimate in the sequential filtering process. This represents the first posterior covariance estimate of the sequential filtering; This represents the first set of measurement matrices after decomposition. This represents the first set of measurement vectors after decomposition.

[0236] If a velocity measurement is observed, a second sub-measurement update is performed:

[0237]

[0238] Finally, the optimal estimate of the state variables is obtained.

[0239] Among them, P k This represents the final posterior covariance estimate of the sequential filtering observations;

[0240] The module M6 includes: using the pitch angle θ and roll angle φ output by SINS as true values, and calculating the yaw angle ψ using the underwater vehicle's positions at the forward and backward points, expressed as:

[0241]

[0242] Those skilled in the art will understand that, in addition to implementing the system, apparatus, and their modules provided by this invention in purely computer-readable program code, the same program can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system, apparatus, and their modules provided by this invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; alternatively, modules for implementing various functions can be considered both software programs implementing the method and structures within the hardware component.

[0243] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A method of navigation and positioning of an underwater vehicle using a multi-sensor combination, characterized in that, Comprise: Step S1: when the underwater vehicle is stationary, take the average of position and attitude angle in a period of time as initial information; Step S2: when the underwater vehicle is sailing, the attitude angle and acceleration information output by the inertial navigation system SINS are used for time updating of the Kalman filter; Step S3: the velocity measured by the Doppler velocity log DVL is used to eliminate the proportional error and the boom error of the SINS center, and the velocity measurement value of the SINS center is obtained; Step S4: according to whether the underwater vehicle floats out of the water, the GPS navigation system or the ultra-short baseline positioning system USBL is used to eliminate the boom error of the measured value, and the position measurement value of the SINS center is obtained; Step S5: the velocity measurement value and the position measurement value of step S3 and step S4 are used for measurement updating of the Kalman filter, and the position of the underwater vehicle is obtained; Step S6: the position of the underwater vehicle obtained in step S5 is used to calculate the attitude angle, and the navigation position, velocity and attitude angle are output; The step S1 comprises: establishing a navigation coordinate system with the initial position of the SINS as the coordinate origin, east as the x axis, north as the y axis and sky as the z axis, and establishing a carrier coordinate system with the carrier axis forward as the x axis, the carrier left as the y axis and the carrier upward as the z axis; The underwater vehicle is static on the water surface for a period of time, the mean value of the position measured by the GPS during the period of time is taken as the initial navigation position of the underwater vehicle, the mean value of the attitude angle measured by the SINS during the period of time is taken as the initial heading attitude information of the underwater vehicle, and the mean value of the position measured by the GPS under the static condition is denoted as the mean value of the attitude angle measured by the SINS is denoted as then the initial position coordinates X0(x0, y0) of the unmanned underwater vehicle, the initial velocity V0(0, 0, 0), and the initial attitude angle are The step S2 comprises: The state variable of the Kalman filter is selected as: X = [x y v x v y v z ] T wherein x, y are the position coordinates in the x-axis and y-axis directions of the underwater vehicle navigation coordinate system; v x y z is the three-axis direction velocity in the underwater vehicle carrier coordinate system;​​ According to the motion equation of the underwater vehicle, the state transition equation of the Kalman filter is obtained: wherein, x represents the state variable of the Kalman filter; F represents the state transition matrix of the Kalman filter; Wherein, W is the process noise, which obeys the normal distribution W~(0, Q); Δt represents the time interval of the state transition of the Kalman filter; Q represents the covariance matrix of the process noise; The above Kalman filter state equation is discretized to obtain: X k = Φ k-1 X k-1 + W k-1 Wherein, k represents the kth time after the Kalman filter is discretized; Wherein, I represents the unit matrix; The process of time updating of the Kalman filter to calculate the position and velocity of the underwater vehicle is as follows: The state variable is calculated forward, and the expression is: X k = Φ k-1 X k-1 + W k-1 The error covariance matrix is calculated forward, and the expression is: where X k represents the state variable of the Kalman filter at the kth moment; P k|k-1 represents the prior covariance estimate.

2. The method of claim 1, wherein, The step S3 comprises: The velocity measured by the DVL center The velocity measured by the DVL center is corrected by multiplying a proportional factor δk Let the arm of the SINS center and the DVL center be l DVL , and the angular acceleration output by the SINS be ω, then the arm error is ωxl DVL The velocity measured by the DVL with itself as the center is subtracted by the arm error to obtain the velocity measured by the SINS center, and the expression is: where l DVL represents the lever arm from the SINS center to the DVL center.

3. The method of claim 2, wherein, The step S4 comprises: According to the depth gauge, it is judged whether the underwater vehicle floats out of the water, if the underwater vehicle floats out of the water, the position measured by the GPS is used as the position measurement value of the SINS center after eliminating the boom error; if the underwater vehicle does not float out of the water, the position measured by the USBL is used as the position measurement value of the SINS center after eliminating the boom error; Let the lever arm between SINS center and GPS center be l GPS , the lever arm between SINS center and USBL center be l USBL , the attitude angle output by SINS be According to the attitude angle, the rotation matrix is calculated, and the expression is: If the underwater vehicle floats to the surface, let the position measured by GPS be The position measurement of the SINS center is wherein, represents the position of the underwater vehicle measured by GPS; If the underwater vehicle does not float to the surface, set the USBL base station position as The USBL mobile station measures the distance ΔX = [Δx, Δy] from the USBL base station, and the position measurement of the SINS center is:

4. The method of claim 3, wherein, The step S5 comprises: The position and velocity are used as the measurement, and the measurement equation of the Kalman filter is obtained as: where H represents the measurement matrix of the Kalman filter; v x represents the x-axis direction velocity in the underwater vehicle carrier coordinate system; v y represents the y-axis direction velocity in the underwater vehicle carrier coordinate system; v z represents the z-axis direction velocity in the underwater vehicle carrier coordinate system; U is the measurement noise, which is subject to a normal distribution U~(0, R); R represents the covariance matrix of the measurement noise; The measurement equation of the Kalman filter is decomposed as: represents the first set of decomposed measurement vectors; represents the second set of decomposed measurement vectors; represents the first set of decomposed measurement matrices; represents the second set of decomposed measurement matrices; represents the first set of decomposed measurement noise matrices; represents the second set of decomposed measurement noise matrices; wherein, Let x k denotes the x-axis position coordinate of the underwater vehicle in the navigation coordinate system at the kth time instant; y k denotes the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at the kth time instant; v xk denotes the x-axis direction velocity of the underwater vehicle in the body coordinate system at the kth time instant; v yk denotes the y-axis direction velocity of the underwater vehicle in the body coordinate system at the kth time instant; v zk denotes the z-axis direction velocity of the underwater vehicle in the body coordinate system at the kth time instant; denotes the decomposed first group of measurement noise covariance matrices; denotes the decomposed second group of measurement noise covariance matrices; denotes the initial value of the state variable of the sequential filter; X k|k-1 denotes the prior state variable estimate; denotes the initial value of the prior covariance estimate of the sequential filter; P k|k-1 denotes the initial value of the prior covariance estimate; According to the principle of sequential filtering, the calculation amount of measurement updating is reduced, if the position measurement value is observed, the first sub measurement updating is carried out: wherein, denotes the first Kalman gain of the sequential filter; denotes the first posterior state variable estimate of the sequential filter; denotes the first posterior covariance estimate of the sequential filter; denotes the first set of decomposed measurement matrices; denotes the first set of decomposed measurement vectors; If the velocity measurement value is observed, the second sub measurement updating is carried out: final optimal estimate of the state quantity where P k denotes the final posterior covariance estimate of the current sequential filtering observation; The step S6 comprises: The pitch angle θ and the roll angle φ output by the SINS are used as the true value, the yaw angle ψ is calculated by using the position of the underwater vehicle before and after, and the expression is:

5. A multi-sensor integrated underwater vehicle navigation and positioning system, characterized by, Comprise: Module M1: when the underwater vehicle is stationary, take the average of position and attitude angle in a period of time as initial information; Module M2: when the underwater vehicle is navigating, the attitude angle and acceleration information output by the inertial navigation system SINS are used to perform time updating of the Kalman filter; Module M3: the velocity measured by the Doppler velocity log DVL with the underwater vehicle as the center is used to eliminate proportional error and lever error, so as to obtain the velocity measurement value of the center of the SINS; Module M4: according to whether the underwater vehicle is out of water, the measurement value of the GPS navigation system or the ultra-short baseline positioning system USBL is used to eliminate lever error, so as to obtain the position measurement value of the center of the SINS; Module M5: the velocity measurement value and the position measurement value of the modules M3 and M4 are used to perform measurement updating of the Kalman filter, so as to obtain the position of the underwater vehicle; Module M6: the attitude angle is calculated according to the position of the underwater vehicle obtained by the module M5, and the navigation position, velocity and attitude angle are output; The module M1 comprises: establishing a navigation coordinate system with the initial position of the SINS as a coordinate origin, with an east direction as an x axis, a north direction as a y axis and a sky direction as a z axis, and establishing a carrier coordinate system with the forward direction of the carrier axis as an x axis, the left direction of the carrier as a y axis and the upward direction of the carrier as a z axis; The underwater vehicle is stationary on the water surface for a period of time, the mean position measured by the GPS during the period of time is taken as the initial navigation position of the underwater vehicle, the mean attitude angle measured by the SINS during the period of time is taken as the initial heading attitude information of the underwater vehicle, and the mean position measured by the GPS under the stationary condition is denoted as the mean attitude angle measured by the SINS is denoted as then the initial position coordinates X0(x0, y0) of the unmanned underwater vehicle, the initial velocity V0(0, 0, 0), and the initial attitude angle are The module M2 comprises: The state variable of the Kalman filter is selected as: X = [x y v x v y v z ] T wherein x, y are the position coordinates in the x-axis and y-axis directions of the underwater vehicle navigation coordinate system; v x ,v y ,v z are the three-axis direction velocities in the underwater vehicle carrier coordinate system; According to the motion equation of the underwater vehicle, the state transition equation of the Kalman filter is obtained: wherein, x represents the state variable of the Kalman filter; F represents the state transition matrix of the Kalman filter; Wherein, W is process noise, which is subject to normal distribution W~(0, Q); Δt represents the time interval of the state transition of the Kalman filter; Q represents the covariance matrix of the process noise; The above Kalman filter state equation is discretized as: X k = Φ k-1 X k-1 + W k-1 Wherein, k represents the kth time after the Kalman filter is discretized; Wherein, I represents a unit matrix; The process of time updating of the Kalman filter to calculate the position and velocity of the underwater vehicle is as follows: The state variable is calculated forward, and the expression is: X k = Φ k-1 X k-1 + W k-1 The error covariance matrix is calculated forward, and the expression is: where X k represents the state variable of the Kalman filter at the kth moment; P k|k-1 represents the prior covariance estimate.

6. The multi-sensor integrated underwater vehicle navigation and positioning system of claim 5, wherein, The module M3 comprises: If the speed measured by the DVL center Corrected by multiplying the proportional factor δk to get the measured speed of the DVL center Let the rod arm between the SINS center and the DVL center be l DVL , the angular acceleration output by the SINS is ω, and the rod arm error is ωxl DVL Subtract the rod arm error from the speed measured by the DVL with itself as the center to get the measured speed of the SINS center, and the expression is: where l DVL represents the lever arm from the SINS center to the DVL center.

7. The multi-sensor integrated underwater vehicle navigation and positioning system of claim 6, wherein, The module M4 comprises: According to the depth gauge, it is judged whether the underwater vehicle is out of water, if the underwater vehicle is out of water, the position measured by the GPS is used as the position measurement value of the center of the SINS after lever error elimination; if the underwater vehicle is not out of water, the position measured by the USBL is used as the position measurement value of the center of the SINS after lever error elimination; Let the lever arm between SINS center and GPS center be l GPS , the lever arm between SINS center and USBL center be l USBL , the attitude angle output by SINS be According to the attitude angle, the rotation matrix is calculated, and the expression is as follows: If the underwater vehicle floats to the surface, let the position measured by GPS be The position measurement of the SINS center is wherein, represents the position of the underwater vehicle measured by GPS; If the underwater vehicle does not float to the surface, set the USBL base station position as The USBL mobile station measures the distance ΔX = [Δx, Δy] from the USBL base station, and the position measurement of the SINS center is:

8. The multi-sensor integrated underwater vehicle navigation and positioning system of claim 7, wherein, The module M5 comprises: The measurement equation of the Kalman filter is obtained by taking the position and velocity as measurement, and the measurement equation of the Kalman filter is obtained as: where H represents the measurement matrix of the Kalman filter; v x represents the x-axis direction velocity in the underwater vehicle carrier coordinate system; v y represents the y-axis direction velocity in the underwater vehicle carrier coordinate system; v z represents the z-axis direction velocity in the underwater vehicle carrier coordinate system; U is the measurement noise, which is subject to a normal distribution U~(0, R); R represents the covariance matrix of the measurement noise; The measurement equation of the Kalman filter is decomposed as: represents the first set of decomposed measurement vectors; represents the second set of decomposed measurement vectors; represents the first set of decomposed measurement matrices; represents the second set of decomposed measurement matrices; represents the first set of decomposed measurement noise matrices; represents the second set of decomposed measurement noise matrices; wherein, Let x k denotes the x-axis position coordinate of the underwater vehicle in the navigation coordinate system at the kth time instant; y k denotes the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at the kth time instant; v xk denotes the x-axis direction velocity of the underwater vehicle in the body coordinate system at the kth time instant; v yk denotes the y-axis direction velocity of the underwater vehicle in the body coordinate system at the kth time instant; v zk denotes the z-axis direction velocity of the underwater vehicle in the body coordinate system at the kth time instant; denotes the decomposed first group of measurement noise covariance matrices; denotes the decomposed second group of measurement noise covariance matrices; denotes the initial value of the state variable of the sequential filter; X k|k-1 denotes the prior state variable estimate; denotes the initial value of the prior covariance estimate of the sequential filter; P k|k-1 denotes the initial value of the prior covariance estimate; According to the principle of sequential filtering, the calculation amount of measurement updating is reduced, if the position measurement value is observed, the first sub-measurement updating is performed: wherein, denotes the first Kalman gain of the sequential filter; denotes the first posterior state variable estimate of the sequential filter; denotes the first posterior covariance estimate of the sequential filter; denotes the first set of decomposed measurement matrices; denotes the first set of decomposed measurement vectors; If the velocity measurement value is observed, the second sub-measurement updating is performed: final optimal estimate of the state quantity where P k denotes the final posterior covariance estimate of the current sequential filtering observation; The module M6 comprises: The pitch angle θ and the roll angle φ output by the SINS are taken as true values, the yaw angle ψ is calculated by using the positions of the underwater vehicle before and after, and the expression is:

Images