Improved Kalman filtering ocean current estimation method based on inertial information and DVL speed

By constructing an augmented state model and using adaptive Kalman filtering, the problem of insufficient accuracy of traditional ocean current estimation methods in complex marine environments is solved, achieving high-precision and robust ocean current estimation and navigation correction, which is suitable for deep-sea environments.

CN121230709APending Publication Date: 2025-12-30TIANJIN YINGNAISHI TECH CO LTD

Patent Information

Application Number
CN202511501039.6
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2025-10-21
Publication Date
2025-12-30

AI Technical Summary

Technical Problem

In existing technologies, traditional ocean current estimation methods are difficult to achieve real-time, high-precision ocean current velocity estimation in complex marine environments. In particular, the filtering algorithms in nonlinear systems lack robustness and cannot adapt to dynamic changes in the ocean current environment, resulting in a decrease in the navigation accuracy of underwater vehicles.

Method used

An improved Kalman filter ocean current estimation method based on inertial information and DVL velocity is constructed. The ocean current velocity is treated as an independent state variable by using an augmented state model. Combined with an adaptive Kalman filter algorithm, the ocean current is inverted by the velocity difference between INS and DVL. The noise covariance matrix is ​​adjusted online by the Sage-Husa adaptive algorithm to achieve closed-loop correction.

Benefits of technology

It improves the accuracy and robustness of ocean current estimation, enhances the navigation performance of underwater vehicles, is applicable to complex ocean current scenarios in deep seas, and enables real-time and reliable output of ocean current environment data.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN121230709A_ABST
    Figure CN121230709A_ABST
Patent Text Reader

Abstract

The invention discloses an improved Kalman filtering ocean current estimation method based on inertial information and DVL speed. The method comprises the following steps: S1, system initialization and data acquisition; s2, performing inertial navigation system (INS) resolving, namely performing mechanical arrangement resolving based on IMU data; s3, constructing a fusion filtering state model and an observation model; s4, adaptive Kalman filtering iteration estimation: after discretizing the state equation and the observation equation, executing filtering iteration according to a period; and S5, state feedback correction and ocean current output. According to the improved Kalman filtering ocean current estimation method based on inertial information and DVL speed, a fusion framework of an augmented state model and adaptive Kalman filtering is constructed, and high-precision ocean current estimation and navigation correction are realized through five key processes.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the field of instrumentation technology, and in particular to an improved Kalman filter ocean current estimation method based on inertial information and DVL velocity. Background Technology

[0002] In fields such as marine engineering, underwater exploration, and autonomous underwater vehicle (AUV) operations, ocean currents are a key disturbance factor in the underwater environment. Accurate estimation of their speed and direction directly determines the positioning accuracy, trajectory tracking capability, and operational reliability of underwater equipment. As marine development activities expand into the deep sea, underwater vehicles need to achieve long-duration, high-precision operations in complex ocean current environments (such as upwellings, eddies, and stratified currents). The limitations of traditional ocean current estimation methods are becoming increasingly apparent, making it difficult to meet practical application requirements.

[0003] Current mainstream ocean current estimation techniques can be divided into three categories: First, indirect estimation methods based on sea surface observation data. These methods acquire ocean current field information through satellite remote sensing, buoy monitoring, etc., and then combine this information with ocean dynamics models to infer underwater current distribution. However, this method suffers from low spatiotemporal resolution and strong lag, failing to reflect instantaneous ocean current changes in local underwater areas in real time, and is particularly unsuitable for the real-time navigation needs of dynamic equipment such as AUVs. Second, direct measurement methods based on a single sensor. For example, this involves using a Doppler velocimeter (DVL) to measure the speed of an underwater vehicle relative to the surrounding water, and then combining this with the vehicle's own propulsion speed to invert the ocean current velocity. However, DVL is prone to signal loss and increased measurement noise in shallow waters, high-turbidity waters, or complex terrain areas, leading to frequent jumps or decreased accuracy in the ocean current data retrieved by a single DVL, making it difficult to consistently output reliable results. Thirdly, the fusion estimation method based on inertial measurement unit (IMU) and DVL (Dual Volume Level) uses classic data fusion algorithms such as Kalman filtering (KF) to combine the inertial motion information (acceleration, angular velocity) of the IMU with the relative velocity information of the DVL, reducing the measurement error of a single sensor. However, traditional Kalman filtering algorithms are prone to introducing errors due to the linearization assumption of the model when dealing with nonlinear systems (such as the time-varying characteristics of ocean currents and vehicle attitude coupling interference). At the same time, when the DVL experiences short-term failure or sudden noise changes, the robustness of the filtering system is insufficient, which can lead to divergence in the ocean current estimation results, thereby affecting the positioning and control accuracy of underwater vehicles.

[0004] Furthermore, existing fusion algorithms often employ fixed process noise and measurement noise covariance matrices, failing to adaptively adjust filtering parameters based on dynamic changes in the ocean current environment (such as switching from a steady flow field to a vortex flow field). This results in a significant decrease in estimation accuracy under complex ocean current scenarios. Therefore, overcoming the limitations of single-sensor measurements, improving the robustness and adaptability of filtering algorithms under nonlinear systems, and achieving real-time, high-precision estimation of ocean current velocity in complex marine environments have become critical technical challenges urgently needing to be addressed in the fields of underwater navigation and marine engineering. Summary of the Invention

[0005] This invention provides an improved Kalman filter ocean current estimation method based on inertial information and DVL velocity to address the shortcomings of existing technologies in the field of underwater vehicle integrated navigation. Specifically, it solves the problem that the standard Kalman filter model in the prior art does not accurately estimate the time-varying and dynamically complex ocean current as an independent state variable. This results in the inability to effectively utilize and compensate for the difference between the ground velocity output by the inertial navigation system (INS) and the water velocity measured by the Doppler velocity meter (DVL), leading to insufficient accuracy in ocean current estimation and ultimately reducing the overall navigation and positioning accuracy of the underwater vehicle.

[0006] To achieve the above objectives, the present invention is implemented through the following technical solution: The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity of the present invention includes the following steps: S1. System initialization and data acquisition, specifically including the following steps: S110. Define the navigation coordinate system (northeast-sky coordinate system, n system) and the vehicle coordinate system (right-front-upper coordinate system, b system). S120. Initialize the initial position, velocity, and attitude parameters of the underwater vehicle; S130: Real-time acquisition of triaxial angular rate and triaxial force data from the inertial measurement unit (IMU), and simultaneous acquisition of the three-dimensional velocity vector of the Doppler velocimeter (DVL) in the carrier coordinate system; S2. Inertial Navigation System (INS) Solution: This involves mechanically arranging and solving data based on IMU data, specifically including the following three aspects: S210, Attitude Update: Obtain the attitude transformation matrix from the vehicle coordinate system to the navigation coordinate system through the three-axis angular rate of the integral inertial measurement unit (IMU); S220, Speed ​​Update: Project the three-axis specific force onto the navigation coordinate system, compensate for the Earth's rotation Coriolis acceleration and gravitational acceleration, and then integrate to obtain the vehicle's speed relative to the ground; S230, Position Update: Integrate the updated ground velocity and output the vehicle's longitude, latitude, and altitude information; S3. Construction of the fusion filter state model and observation model, specifically including the following steps: S310. Establish an 18-dimensional augmented system state-space model, state vector... X(t) = [δφ E , δφ N, δφ U , δv E , δv N , δv U , δL, δλ, δh, ε bx , ε by , ε bz , ∇ bx ,∇ by , ∇ bz , Vc E, Vc N , Vc U ] T The system's state equation covers platform misalignment angle error, velocity error, position error, gyroscope constant drift, accelerometer constant zero bias, and the ocean current velocity component in the northeast direction. Ẋ(t) = F (t) X (t) + G (t) w (t) Where F(t) is the dynamic matrix, w(t) is zero-mean Gaussian white noise, and the system dynamic matrix F(t) is derived based on the theory of inertial navigation error propagation; S320. Establish the observation model: Transform the Doppler velocity (DVL) relative to the water into the navigation coordinate system using an attitude transformation matrix, and calculate the difference between this and the INS velocity relative to the ground as the observation value Z(t). The observation equation is: Z(t) = H(t)X(t) + v(t) Where H(t) is the observation matrix and v(t) is zero-mean Gaussian white noise. The observation matrix H(t) of the system is obtained by superimposing the observations as the difference between the velocity relative to the ground and the velocity relative to the water, i.e., the ocean current velocity and the velocity error of the inertial navigation system (INS). S4. Adaptive Kalman filter iterative estimation: After discretizing the state equation and the observation equation, the filtering iteration is performed periodically; S5. Status feedback correction and ocean current output, specifically including the following steps: S510, INS Feedback Correction: Feeds back the INS error term from the filtered output to the INS calculation module to perform closed-loop correction of attitude, velocity, and position; S520, Ocean Current Output: Extract the ocean current velocity component from the final state estimate and output it as the ocean current estimate result at the current moment.

[0007] As a further improvement to the above technical solution, the following steps are included after step S130: S140. Perform zero bias compensation (based on factory calibration parameters) and outlier removal on the IMU data (using the 3σ criterion). S150. Validate the DVL data. If the signal quality is below the threshold, mark it as invalid data. The extrapolated value of the valid DVL data from the previous cycle is used for this filtering.

[0008] As a further improvement to the above technical solution, step S210 specifically includes the following steps: S211. Within a certain integration period, the IMU angular rate sampling points are divided into segments, and the quaternion increment is calculated for each segment using the Picard integration method. S212. All increments of the lightning strike result in the current cycle quaternion. Through the transformation relationship between the quaternion and the attitude matrix, the attitude transformation matrix from the carrier coordinate system to the navigation coordinate system is calculated.

[0009] As a further improvement to the above technical solution, in step S4, the first-order Runge-Kutta method is used to discretize the continuous state equation.

[0010] As a further improvement to the above technical solution, the adaptive Kalman filter iteration in step S4 includes the following aspects: S410, Time Update: Based on the state estimate and covariance matrix of the previous cycle, calculate the one-step prediction value and one-step prediction covariance matrix of the current cycle state; S420, Measurement Update: Calculate the innovation sequence and Kalman gain, and update the current state estimate and covariance matrix; S430, Noise Adaptive Adjustment: The Sage-Husa adaptive filtering algorithm is adopted, which improves the filtering robustness based on the online update process noise covariance matrix and measurement noise covariance matrix of the innovation sequence. As a further improvement to the above technical solution, the INS feedback correction in step S510 specifically includes the following steps: S511. Extract the inertial navigation system (INS) error term from the state estimate: S512, The platform misalignment angle error is corrected by adjusting the attitude transformation matrix using the attitude matrix correction amount; S513, Velocity error correction for the ground velocity of the inertial navigation system (INS); S514, Position error correction for inertial navigation system (INS) position; S515. The corrected attitude transformation matrix, ground velocity, and position are used as the initial values ​​for the next INS cycle solution to form a closed-loop correction.

[0011] As a further improvement to the above technical solution, the following steps are included after step S520: S530, Data storage: Ocean current data and navigation data are synchronously stored to the SD card every unit of time for post-analysis and model optimization.

[0012] As can be seen from the above technical solution, the present invention has the following beneficial effects: The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity of this invention constructs a fusion framework of "augmented state model + adaptive Kalman filter", and achieves high-precision ocean current estimation and navigation correction through a five-step key process, so as to achieve the following technical effects: 1. Improved estimation accuracy: By incorporating ocean current velocity as an independent state variable into the augmented model, the ocean current is directly inverted using the velocity difference between INS and DVL, thus solving the estimation bias problem caused by the traditional model ignoring the dynamic characteristics of ocean currents; 2. Enhanced robustness: The Sage-Husa adaptive algorithm can adjust the noise covariance matrix online, effectively suppressing the risk of filter divergence caused by DVL signal packet loss, noise abrupt changes, and time-varying ocean current characteristics; 3. Navigation performance optimization: Closed-loop correction of INS error and ocean current estimation are realized simultaneously, which not only improves the autonomous navigation accuracy of the vehicle, but also provides real-time and reliable ocean current environment data for underwater operations, making it suitable for complex ocean current scenarios in deep sea. Attached Figure Description

[0013] Figure 1 This is a schematic diagram of the improved Kalman filter ocean current estimation method based on inertial information and DVL velocity of the present invention. Detailed Implementation

[0014] In the description of this invention, it should be understood that the terms "center", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0015] The terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more.

[0016] The following describes the implementation process of this invention in detail, using a specific application scenario: deep-sea exploration operations conducted by an autonomous underwater vehicle (AUV). The AUV's onboard IMU sampling frequency is 200Hz, the DVL sampling frequency is 10Hz, and the filtering period is consistent with the DVL sampling period (0.1s). The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity described in this embodiment includes the following steps: (a) Implementation preparation Hardware configuration: A tactical-grade IMU (including a 70 fiber optic gyroscope system and a quartz accelerometer) is selected, outputting three-axis angular rate (range ±400° / s) and three-axis specific force (range ±10g); a narrow-beam DVL (Teledyne RDI Workhorse) is used, supporting water depth measurement up to 3000m, outputting three-dimensional water velocity in the b-series (accuracy ±0.01m / s). DVL data is sent to the IMU via serial port for processing. The IMU uses a DSP6748 microprocessor system to perform real-time information fusion processing.

[0017] Coordinate system and parameter initialization: Set the n-system as the northeast-sky coordinate system (origin at the initial position of the AUV, E-axis pointing east, N-axis pointing north, U-axis perpendicular to the sea level and upward), and the b-system as the starboard-forward-upper coordinate system (origin at the center of mass of the AUV, x-axis pointing to starboard, y-axis pointing bow, z-axis pointing above the deck); Initialize the AUV's initial state: initial position (latitude and longitude L0=30°N, λ0=120°E, altitude h0=0m), initial velocity (v... E0 =0m / s, v N0 =0m / s, v U0 =0m / s), initial attitude (roll angle φ0=0°, pitch angle θ0=0°, heading angle ψ0=0°); initial filter parameters: the initial state estimate X̂0 is a zero vector, and the diagonal elements of the initial state covariance matrix P0 are set as follows: platform misalignment angle error 0.1°, velocity error 0.1m / s, position error 1m, gyroscope drift 0.01° / h, accelerometer zero bias 100μg, ocean current velocity error 0.5m / s.

[0018] (II) Step 1: System Initialization and Data Acquisition After the navigation computer is powered on, it reads IMU data via serial port, acquiring the triaxial angular rate (ω) every 0.005s (200Hz). bx , ω by, ω bz ) and triaxial specific force (f bx , f by , f bzThe data is stored in a circular buffer; DVL data is read via an RS485 interface, and the water velocity (v) under system b is sampled every 0.1s (10Hz). DVLx , v DVLy , v DVLz Simultaneously trigger subsequent INS calculation and filtering processes; Data preprocessing: perform zero bias compensation (based on factory calibration parameters) and outlier removal (using the 3σ criterion) on IMU data; perform validity judgment on DVL data, if the signal quality is lower than the threshold (correlation coefficient < 0.7), it is marked as invalid data, and the extrapolated value of the valid DVL data of the previous cycle is used for this filtering.

[0019] (III) Step Two: Inertial Navigation System (INS) Solution 1. Attitude update calculation Attitude updates are achieved using quaternion differential equations based on IMU angular rate data: quaternion initial value q0 = [1, 0, 0, 0] T (Corresponding to the initial attitude); Within the integration period (0.1s), the IMU angular rate is divided into segments of 200Hz sampling points, and the quaternion increment for each segment is calculated using the Picard integration method: Δq i = (1 / 2)Ω(ω bi ) q i-1 Δt (Ω(·) is the antisymmetric matrix of angular velocity, Δt=0.005s); accumulating all increments yields the current period quaternion q. k By utilizing the transformation relationship between quaternions and attitude matrices, the attitude transformation matrix C from the b-system to the n-system is calculated. bn : 2. Speed ​​update calculation Compare the IMU force f b Projected onto the n-system: f n = C bn f b ; Compensation for Earth's rotation and Coriolis acceleration: f ncomp = f n - 2Ω ien v n - Ω enn v n (Ω) ien Ω is the projection of the Earth's rotational angular velocity onto the n-frame. enn (where ω is the angular velocity of the n-frame relative to the Earth coordinate system); and the compensating gravitational acceleration g. n (n series g) n= [0, 0, 9.81] T m / s 2 ):f nnav = f ncomp -g n ; for f nnav Integrating, we obtain the velocity increment Δv relative to the ground in the n-frame. n Accumulated speed v from the previous cycle nk-1 The current period's INS velocity relative to the ground, v, is obtained. nk = [v Ek , v Nk ,v Uk ] T Position update calculation is based on the current velocity v. nk The position increment is calculated using the trapezoidal integral method: ΔL = (v Nk + v Nk-1 )Δt / (2RM + h)(R M (where is the radius of curvature of the meridional circle), Δλ = (v Ek + v Ek-1 )Δt / (2 (R N + h) cosL)(R N (where the radius of curvature is the radius of curvature of the zonal loop), Δh = (v Uk + v Uk-1 )Δt / 2; the cumulative increment yields the current position L k = L k-1 + ΔL, λ k = λ k-1 + Δλ,h k = h k-1 + Δh.

[0020] (iv) Step 3: Constructing the fused filtering state model and the observation model system state equations System state equation construction: Based on the theory of inertial navigation error propagation, the system dynamic matrix F(t) is derived. The core elements include the coupling term between the platform misalignment angle error and the velocity error: F(1,4) = -f N / (v U + g), F (1,5) = f E / (v U + g)(f E , f N (For the specific force component of the n-system); the coupling term of velocity error and position error: F(4,7) = -v N tanL / R M F (5,8) = v E / (R NcosL); Dynamic coefficient of ocean current velocity term: F (16:18,16:18) = 0 (assuming the ocean current is short-term steady with no acceleration); The diagonal elements of the system process noise matrix G(t) are set as follows: gyroscope noise 0.001° / √h, accelerometer noise 10μg / √Hz, ocean current velocity noise 0.05m / s / √Hz, and w(t) is zero-mean Gaussian white noise.

[0021] System observation equation construction: Calculate the DVL velocity relative to water in the n-system: v DVLn = C bn [v DVLx , v DVLy , v DVLz ] T = [v DVLE, v DVLN , v DVLU ] T ; Observation Z k = v nk - v DVLn = [v Ek - v DVLE , v Nk - v DVLN , v Uk - v DVLU ] T (Physically, this means the difference between the velocity relative to the ground and the velocity relative to the water, i.e., the superposition of the errors in ocean current velocity and INS velocity); Observation matrix H k This is a 3×18 matrix, related only to velocity error and ocean current velocity: H k (1,4) = 1, H k (1,16) = 1; H k (2,5) = 1, H k (2,17) = 1; H k (3,6) = 1, H k (3,18) = 1; all other elements are 0; the variance of the measurement noise v(t) is set to the square of the DVL speed measurement error (0.01m / s). 2 .

[0022] (v) Step 4: Perform adaptive Kalman filter iterative estimation 1. Equation Discretization The continuous state equations are discretized using the first-order Runge-Kutta method, and the state transition matrix Φ k,k-1 = I + F(t)Δt (Δt=0.1s), process noise covariance matrix Q k-1= G(t) Q c G(t) T Δt(Q) c (This is the noise power spectral density matrix for a continuous process).

[0023] 2. Time Update State prediction in one step: X̂ k|k-1 =Φ k,k-1 X̂ k-1 Covariance one-step prediction: P k|k-1 =Φ k,k-1 P k-1 Φ k,k-1 T +Q k-1 .

[0024] 3. Measurement Update New information calculation: ν k = Z k - H k X̂ k|k-1 ; Kalman gain: K k = P k|k-1 H k T (H k P k|k-1 H k T + R k-1 ) -1 (R) k-1 (This is the measurement noise covariance matrix of the previous cycle). Status Update: X̂ k = X̂ k|k-1 + K k ν k ; Covariance update: P k = (I - K k H k ) P k|k-1 (I - K k H k ) T + K k R k-1 K k T (Consider the impact of measurement noise on covariance).

[0025] 4. Online adaptive estimation of noise statistical characteristics Update Q using the Sage-Husa algorithm k With R k : New covariance estimation: Sk = (1 - β) S k-1 + βν k ν k T (β=0.05 is the forgetting factor); Measurement noise covariance update: R k = S k - H k P k|k-1 H k T ; Process noise covariance update: Q k = (1 - β) Q k-1 + β[K k ν k ν k T K k T + (I - K k H k ) P k|k-1 (I -K k H k ) T - P k-1 ]; Constraint handling: If Q k Or R k If a negative definite value occurs, the effective matrix value from the previous cycle is used as a replacement to ensure filter stability.

[0026] (vi) Step 5: State feedback correction and ocean current output 1. INS Feedback Correction From the state estimate X̂ k Extracting INS error terms: Platform misalignment angle error δφ = [δφ E , δφ N , δφ U ]^T, through the attitude matrix correction amount ΔC bn = I -[δφ]×([·]× is an antisymmetric matrix), for C bn Perform correction: C bn_corr = ΔC bn C bn ; Velocity error δv = [δv E , δv N , δv U ] T INS ground velocity correction: v nk_corr = v nk - δv; Position error δp = [δL, δλ, δh] T INS position correction: L k_corr = L k - δL, λ k_corr =λ k - δλ, h k_corr = h k - δh; The corrected attitude matrix, velocity, and position are used as the initial values ​​for the next INS cycle solution, forming a closed-loop correction.

[0027] 2. Ocean current output From X̂ k Extracting the ocean current velocity component V c = [V cE V cN V cU ] T The data is output to the AUV control system and host computer via the CAN bus. Output format: timestamp (ms) + eastward current velocity (m / s) + northward current velocity (m / s) + vertical current velocity (m / s) + estimation error (m / s, taken from P... k (Corresponding to the square root of the diagonal elements); Data storage: Ocean current data and navigation data are synchronously stored to the SD card every 1 second for post-analysis and model optimization.

[0028] (VII) Implementation Effect Verification: A sea trial was conducted in Fuxian Lake, Yunnan Province, lasting 2 hours at a speed of 2 m / s. During the trial, the vessel experienced surface currents (0.5-1.2 m / s) and bottom vortex currents (0.3-0.8 m / s). Comparison with a shipborne ADCP (Acoustic Doppler Current Profiler, accuracy ±0.05 m / s) verified that the root mean square error (RMSE) of the ocean current estimation of this invention was 0.08 m / s eastward, 0.07 m / s northward, and 0.05 m / s vertically, a reduction of 40%-50% compared to traditional Kalman filtering (without augmented ocean current conditions). The AUV positioning error (relative to GPS) was 3.2 m horizontally, a reduction of 35% compared to INS / DVL integrated navigation without ocean current compensation, thus verifying the effectiveness and practicality of this invention.

[0029] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. An improved Kalman filtering ocean current estimation method based on inertial information and DVL velocity, characterized in that: Comprising the following steps: S1, system initialization and data acquisition, specifically comprising the following steps: S110, define the navigation coordinate system (north east sky coordinate system, n system) and the carrier coordinate system (right front up coordinate system, b system); S120, initialize the initial position, velocity and attitude parameters of the underwater vehicle; S130, real-time acquisition of three-axis angular rate and three-axis specific force data of the inertial measurement unit (IMU), and synchronous acquisition of three-dimensional water speed vector of the Doppler velocity log (DVL) in the carrier coordinate system; S2, inertial navigation system (INS) solution: based on IMU data, mechanical arrangement solution is performed, specifically including the following three aspects: S210, attitude update: the attitude transformation matrix of the carrier coordinate system to the navigation coordinate system is obtained by integrating the three-axis angular rate of the inertial measurement unit (IMU); S220, velocity update: the three-axis specific force is projected to the navigation coordinate system, and after compensation of the earth rotation Coriolis acceleration and the gravity acceleration, the vehicle's ground speed is obtained by integration; S230, position update: the updated ground speed is integrated to output the vehicle's longitude, latitude and height information; S3, fusion filter state model and observation model construction, specifically comprising the following steps: S310, establish an eighteen-dimensional augmented system state space model, and the state vector covers platform misalignment angle error, velocity error, position error, gyroscope constant drift, accelerometer constant zero offset and northeast sky direction current velocity component; S320, establish an observation model: the Doppler velocity log (DVL) water speed is converted to the navigation coordinate system through the attitude transformation matrix, and the difference between the inertial navigation system (INS) ground speed is taken as the observation; S4, adaptive Kalman filter iterative estimation: after discretization of the state equation and the observation equation, the filter iteration is performed according to the period; S5, state feedback correction and current output, specifically comprising the following steps: S510, INS feedback correction: the inertial navigation system (INS) error term output by the filter is fed back to the inertial navigation system (INS) solution module for closed-loop correction of attitude, velocity and position; S520, current output: the current velocity component is extracted from the final state estimation value as the current estimation result output.

2. The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity according to claim 1, characterized in that: After step S130, the following steps are included: S140, zero offset compensation (based on factory calibration parameters) and outlier rejection (using 3σ criterion) are performed on the IMU data; S150, the effectiveness of the Doppler velocity log (DVL) data is judged, if the signal quality is lower than the threshold, it is marked as invalid data, and the extrapolated value of the last period valid Doppler velocity log (DVL) data is used in this filter.

3. The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity according to claim 1, characterized in that: Step S210 specifically includes the following steps: S211, within a certain integration period, the IMU angular rate sampling points are segmented, and the quaternion increment is calculated by using the Pica integration method for each segment; S212, all increments are struck to obtain the current period quaternion, and the attitude transformation matrix of the carrier coordinate system to the navigation coordinate system is calculated through the conversion relationship between the quaternion and the attitude matrix.

4. The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity according to claim 1, characterized in that: In step S4, the first-order Runge-Kutta method is used to discretize the continuous state equation.

5. The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity according to claim 1, characterized in that: The filtering iteration of step S4 specifically includes the following aspects of update: S410, time update: based on the last cycle state estimation value and the covariance matrix, the one-step prediction value and the one-step prediction covariance matrix of the current cycle are calculated; S420, measurement update: innovation sequence and Kalman gain are calculated, and the current state estimation value and the covariance matrix are updated; S430, noise adaptive adjustment: using the Sage-Husa adaptive filtering algorithm, the innovation sequence is used to update the process noise covariance matrix and the measurement noise covariance matrix online, and the robustness of the filter is improved.

6. The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity according to claim 1, characterized in that: The INS feedback correction in step S510 specifically includes the following steps: S511, extracting the inertial navigation system (INS) error term from the state estimation value: S512, the platform misalignment angle error is corrected by the attitude matrix correction amount to the attitude transformation matrix; S513, the velocity error corrects the inertial navigation system (INS) ground speed; S514, the position error corrects the inertial navigation system (INS) position; S515, the corrected attitude transformation matrix, ground speed and position are used as the initial value of the next cycle INS solution, forming a closed loop correction.

7. The improved Kalman filter ocean current estimation method based on inertial information and DVL velocity according to claim 1, characterized in that: Step S520 is followed by the step: S530, data storage: the ocean current data and the navigation data are stored synchronously to the SD card every unit time.

Citation Information

Patent Citations

  • SINS / DVL tight combination navigation method in complex environment

    CN110567454A

  • Motion constraint assisted underwater integrated navigation method based on improved Sage-Husa adaptive filtering

    CN112254718A

  • Ocean current velocity estimation method based on SINS and GNSS combination and DVL

    CN114236173A

  • Underwater unmanned underwater vehicle flow measurement DVL / SINS integrated navigation method based on virtual speed measurement

    CN114459476A

  • HMM-based electromagnetic log ocean current estimation and integrated navigation method and system

    CN115752453A

Cited By

  • Microgrid power collaboration method and system based on novel state augmented Kalman filtering

    CN121791130A

  • External velocity damping inertial navigation method based on frequency domain ocean current identification

    CN122062654A

  • An external velocity damping inertial navigation method based on frequency domain ocean current identification

    CN122062654B