Data filtering method and device for sonar altimeter of underwater gliding device
The covariance matrix of observed noise is adjusted through the adaptive Kalman filtering algorithm, which solves the accuracy problem of the sonar altimeter of the underwater gliding device, achieves higher accuracy and less data jumps, and improves the detection capability of the underwater gliding device.
Patent Information
- Application Number
- CN202311561692.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-11-20
- Publication Date
- 2025-07-08
AI Technical Summary
The sonar altimeter of the underwater gliding device has poor accuracy and frequent jumps, making it difficult to meet the needs of long-term and large-scale ocean exploration.
The adaptive Kalman filtering algorithm based on n iteration correction of observation noise is adopted. By establishing the state equation and observation equation, the filtering result is calculated using the Kalman filtering algorithm with steps of 1 and n, and the observed noise covariance matrix R is adjusted through the window distance matrix to ensure the accuracy of the filtering result.
It significantly improves the accuracy of the sonar altimeter, suppresses data jumps, and improves the detection performance of the underwater gliding device.
Smart Images
Figure CN120281296A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of underwater gliding devices, and particularly relates to a data filtering method and device for a sonar altimeter used in an underwater gliding device. Background Art
[0002] An underwater gliding device is a new type of underwater robot. It obtains propulsion force by using net buoyancy and attitude angle adjustment, with extremely low energy consumption, meeting the needs of long-term and large-scale ocean exploration. Figure 1 is a schematic diagram of the diving process of an underwater gliding device. In practical applications, it is often necessary to know the distance from the underwater gliding device to the seabed during the diving process to prevent collisions. An important tool for detecting the height from the bottom is a sonar altimeter. However, due to the complex operating environment of the underwater gliding device and the large noise of underwater acoustic signals, and the general disadvantages of poor accuracy and frequent jumps of underwater acoustic sensors, further processing of the measured data is required to obtain satisfactory accuracy.
[0003] Since the Kalman filter came out in 1960, after more than 50 years of development, it has formed a relatively complete theoretical system and has been successfully applied in fields such as aviation, aerospace, and industrial control. However, it requires an accurate system model, especially when the state noise and observation noise models are known, which is difficult to meet in practical applications or during the operation of the system, the model changes, which may lead to the divergence or reduced accuracy of the standard Kalman filter. The present invention solves this problem through an adaptive Kalman filter algorithm based on n - iteration correction of the observation noise. Summary of the Invention
[0004] In view of this, the present invention provides a data filtering method and device for a sonar altimeter used in an underwater gliding device, which can solve the technical problems of poor accuracy and frequent jumps of the sonar altimeter of the underwater gliding device.
[0005] To solve the above technical problems, the present invention is implemented as follows.
[0006] A data filtering method for a sonar altimeter used in an underwater gliding device, the method comprising:
[0007] Step S1: For the vertical plane movement process of the underwater gliding device in the underwater environment, establish the state vector of the underwater gliding device; based on the state vector, establish the state equation of the underwater gliding device;
[0008] Step S2: Based on the relationship between the sonar altimeter sampling value and the height of the underwater gliding device from the bottom, establish the observation equation of the underwater gliding device;
[0009] Step S3: Define the time interval between the sampling moments for two consecutive height samplings as a step. At each sampling moment, calculate the current state through the Kalman filtering algorithms with step sizes of 1 and n respectively, to obtain the first filtering result of the current state of the underwater glider device. And the second filtering result Respectively represent the i-th state at the k-th moment when the step sizes are 1 and n;
[0010] Step S4: Based on two preset window distance matrices And Judge whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation and enter Step S3; otherwise, use the state estimate value obtained from the state equation based on the state parameters of the underwater glider device at this time as the value of the filtered sonar altimeter.
[0011] Preferably, the state vector of the underwater glider device is x k = [h k d′ k d″ k T , where h k is the height from the bottom of the underwater glider device at time t k , d k is the depth of the underwater glider device at time t k , and d k ′, d k ″ are the first derivative and the second derivative of d k respectively.
[0012] Preferably, the state equation of the underwater glider device is
[0013]
[0014] Where T is the sampling period of the underwater glider device control system, h k-1 is the height from the bottom of the underwater glider device at time t k-1 , d k-1 ′ is the vertical velocity of the underwater glider device at time t k-1 , d k-1 ″ is the vertical acceleration of the underwater glider device at time t k-1 , and w k-1 is the process excitation noise at time t k-1 .
[0015] Preferably, the observation equation is:
[0016]
[0017] Among them, α is the inclination angle during the diving process of the underwater glider, β is the angle between the sonar altimeter and the horizontal plane of the underwater glider, and v k is the observation noise at time t k This noise is Gaussian white noise that follows a normal distribution N(0, R), and R represents the observation noise covariance matrix.
[0018] Preferably, based on two preset window distance matrices and judge whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation, including:
[0019] If Let R 1 = λ max R, where λ max is a fixed value greater than 1, so as to increase the value of the observation noise covariance and reduce its proportion in the filtering result. R 1 is the value of the adjusted observation noise covariance matrix R;
[0020] If Let R 1 = λ min R, where 0 < λ min < 1, reduce the value of the observation noise covariance and increase its proportion in the filtering result.
[0021] A data filtering device for a sonar altimeter of an underwater glider provided by the present invention, the device includes:
[0022] State equation module: configured to establish the state vector of the underwater glider for the vertical plane movement process of the underwater glider in the underwater environment; establish the state equation of the underwater glider based on the state vector;
[0023] Observation equation module: configured to establish the observation equation of the underwater glider based on the relationship between the sonar altimeter sampling value and the bottom clearance of the underwater glider;
[0024] Filtering result determination module: configured to define the time interval between the sampling times of two consecutive height samplings as a step length. At each sampling time, calculate the current state of the underwater glider through the Kalman filtering algorithm with a step length of 1 and a step length of n respectively, and obtain the first filtering result of the current state of the underwater glider and the second filtering result respectively represent the i-th state at the k-th moment when the step lengths are 1 and n;
[0025] Adjustment module: configured to be based on two preset window distance matrices and Determine whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation and enter step S3; otherwise, use the state estimate value obtained from the state equation based on the current state parameters of the underwater glider as the value of the filtered sonar altimeter.
[0026] A computer-readable storage medium provided by the present invention stores multiple instructions; the multiple instructions are used to be loaded and executed by a processor to perform the method as described above.
[0027] An electronic device provided by the present invention, wherein the electronic device includes:
[0028] A processor for executing multiple instructions;
[0029] A memory for storing multiple instructions;
[0030] Among them, the multiple instructions are used to be stored by the memory and loaded and executed by the processor to perform the method as described above.
[0031] The beneficial technical effects brought by the present invention:
[0032] (1) The present invention can greatly improve the accuracy of the sonar altimeter and suppress data jumps.
[0033] (2) The present invention considers the motion characteristics during the diving process of the underwater glider, automatically adjusts the estimated value of the observation noise covariance, so as to better conform to the actual observation situation, and finally achieves the purpose of effectively improving the performance of the sonar altimeter of the underwater glider.
[0034] (3) The present invention is simple to implement and reduces the requirement for the detection accuracy of the sonar altimeter carried by the underwater glider. Description of the Drawings
[0035] Figure 1 It is a schematic diagram of the diving process of the underwater glider;
[0036] Figure 2 It is a schematic diagram of the data filtering method for the sonar altimeter used in the underwater glider of the present invention;
[0037] Figure 3 It is a schematic diagram of the standard Kalman filter algorithm;
[0038] Figure 4 It is a schematic diagram of the adaptive Kalman filter algorithm based on n - time iterative correction of the observation noise of the present invention. Detailed Embodiments
[0039] The present invention will be described in detail below with reference to the accompanying drawings and embodiments.
[0040] As Figure 2 shown, the present invention provides a data filtering method for a sonar altimeter of an underwater gliding device, and the method includes:
[0041] Step S1: Establish a state vector of the underwater gliding device for the vertical plane movement process of the underwater gliding device in the underwater environment; establish a state equation of the underwater gliding device based on the state vector;
[0042] Step S2: Establish an observation equation of the underwater gliding device based on the relationship between the sonar altimeter sampling value and the bottom clearance of the underwater gliding device;
[0043] Step S3: Define the time interval between two consecutive height sampling moments as a step size. At each sampling moment, calculate the current state through the Kalman filter algorithms with step sizes of 1 and n respectively, and obtain a first filtering result of the current state of the underwater gliding device through the observation equation and a second filtering result respectively represent the i-th state at the k-th moment when the step sizes are 1 and n;
[0044] Step S4: Based on two preset window distance matrices and judge whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation and enter Step S3; otherwise, use the state estimate value obtained from the state equation based on the state parameters of the underwater gliding device at this time as the filtered value of the sonar altimeter.
[0045] Furthermore, the state vector of the underwater gliding device is x k =[h k d′ k d″ k T , where h k is the bottom clearance of the underwater gliding device at time t k , d k is the depth of the underwater gliding device at time t k , and d k ′, d k ″ are the first-order derivative and the second-order derivative of d k respectively.
[0046] The state equation of the underwater gliding device is
[0047]
[0048] Among them, T is the sampling period of the underwater glider control system, h k-1 is the height of the underwater glider from the bottom at time t, d k-1 ′ is the vertical velocity of the underwater glider at time t, d k-1 ′′ is the vertical acceleration of the underwater glider at time t, w k-1 is the process excitation noise at time t k-1 k-1 k-1 k-1 k k 1 max
[0049] The observation equation is as follows:
[0050]
[0051] Among them, α is the tilt angle during the diving process of the underwater glider, β is the angle between the sonar altimeter and the horizontal plane of the underwater glider, v k is the observation noise at time t k This noise is Gaussian white noise that follows a normal distribution N(0, R), and R represents the observation noise covariance matrix
[0052] Furthermore, based on two preset window distance matrices and judge whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation, including:
[0053] If Let R 1 = λ max R, where λ max is a fixed value greater than 1, so as to increase the value of the observation noise covariance and reduce its proportion in the filtering result, R 1 is the value of the adjusted observation noise covariance matrix R
[0054] If Let R 1 = λ min R, where 0 < λ min < 1, reduce the observation noise covariance value and increase its proportion in the filtering result
[0055] The sonar altimeter fluctuates greatly under the influence of the underwater environment. When the echo intensity is insufficient or the underwater noise is too high, its accuracy will be affected. That is to say, the actual observation noise is constantly changing. From the perspective of the Kalman filtering process, the observation noise covariance R is not a fixed value. Based on the standard Kalman filtering, the present invention designs an adaptive algorithm based on the n - iteration correction of the observation noise for the actual problems that the sonar altimeter of the underwater glider is vulnerable to environmental influence and has poor accuracy itself. By adjusting the observation noise covariance in real time to make it closer to the actual value, the accuracy of the altimeter data is improved.
[0056] The present invention provides a specific embodiment of a data filtering method for a sonar altimeter of an underwater glider.
[0057] During the diving process of the underwater glider, most of the time it works in a stable gliding state. That is, the position of the mass block and the magnitude of the net buoyancy are both set to a constant value, and it relies on the lift of water and the net buoyancy to complete gliding at a fixed pitch angle, as Figure 1 shown. Considering this stable vertical - plane motion process, the state vector is selected as
[0058] x k =[h k d′ k d″ k T
[0059] where h k is the height of the underwater glider from the bottom at time t k ; d k is the depth of the underwater glider at time t k .
[0060] The state equation is established as follows:
[0061]
[0062] where the control input is zero, w k-1 is the process excitation noise at time t k-1 , and T is the system sampling period.
[0063] The observation equation is:
[0064]
[0065] where α is the inclination angle during the diving process of the underwater glider, β is the angle between the altimeter and the horizontal plane of the underwater glider, and v k is the observation noise at time t k .
[0066] Assume w k and v k are independent and identically distributed Gaussian white noises with a normal distribution:
[0067] p(w) ~ N(0, Q)
[0068] p(v) ~ N(0, R)
[0069] where Q is the process excitation noise covariance matrix and R is the observation noise covariance matrix.
[0070] The state x k estimate can be solved by the following equations:
[0071] Predict the state vector forward
[0072] Predict the error covariance forward
[0073] Calculate the Kalman gain
[0074] Update the estimate using the observation vector
[0075] Update the error covariance
[0076] where A is the state transition matrix, B is the control input gain, and H is the measurement matrix.
[0077] As long as the initial state estimate and the initial error covariance P0 are given, according to the observation vector z at time t k the state estimate at time t k can be recursively calculated through the above equations k for the state estimate at time t
[0078] Define each sampling interval as a step. Then, at each sampling time, the filtering result of the current state can be obtained through two Kalman filters with step sizes 1 and n. Denote the two filtered state estimates at time k (k > n) as Set two window distance matrices D max and D min , and use the window distance matrix to determine whether there has been a significant change in the state of the system. When i.e., at the k-th moment of the i-th state, the window distance compensating for the difference between the filtering estimates of 1 and n is greater than the window upper limit, it is considered that there has been a significant change in the state of the system, the credibility of the observed value decreases, and the observation noise matrix R needs to be adjusted. When making R 1 = λ max R, where λ maxis a fixed value greater than 1, thereby increasing the observation noise covariance value and reducing its proportion in the filtering result. The noise covariance matrix R 1 is brought into the model for solution. If
[0079]
[0080] then the noise covariance matrix R 1 the obtained sampling estimate value is credible. Otherwise, let R 2 = λ min R 1 , until the above formula is satisfied. Similarly, if
[0081]
[0082] let R 1 = λ min R, where 0 < λ min < 1, reducing the observation noise covariance value and increasing its proportion in the filtering result. In order to make the sampling estimate value accurate, the two step sizes should not differ too much.
[0083] As Figure 3 , Figure 4 shown, it is a schematic diagram of the effect comparison between the adaptive algorithm based on n - iteration correction of observation noise in this embodiment and the standard Kalman filtering algorithm. According to the attached drawings, it can be seen that this algorithm can obtain higher sonar altimeter accuracy and better suppression effect on jump data.
[0084] The present invention provides a data filtering device for a sonar altimeter of an underwater gliding device, and the device includes:
[0085] State equation module: configured to establish the state vector of the underwater gliding device for the vertical - plane movement process of the underwater gliding device in the underwater environment; and establish the state equation of the underwater gliding device based on the state vector;
[0086] Observation equation module: configured to establish the observation equation of the underwater gliding device based on the relationship between the sonar altimeter sampling value and the bottom - clearance height of the underwater gliding device;
[0087] Filtering result determination module: configured to define the time interval between two consecutive height - sampling moments as a step size. At each sampling moment, calculate the current state through the Kalman filtering algorithm with step sizes of 1 and n respectively, and obtain the first filtering result and the second filtering result respectively represent the i - th state at the k - th moment when the step sizes are 1 and n;
[0088] Adjustment module: configured to be based on two preset window distance matrices and Determine whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation and enter step S3; otherwise, use the state estimation value obtained from the state equation based on the state parameters of the underwater glider at this time as the value of the filtered sonar altimeter.
[0089] A computer-readable storage medium provided by the present invention stores multiple instructions; the multiple instructions are used to be loaded and executed by a processor to perform the method as described above.
[0090] An electronic device provided by the present invention is characterized in that the electronic device includes:
[0091] A processor for executing multiple instructions;
[0092] A memory for storing multiple instructions;
[0093] Wherein, the multiple instructions are used to be stored by the memory and loaded and executed by the processor to perform the method as described above.
[0094] The above specific embodiments only describe the design principle of the present invention. The shapes and names of the components in this description can be different and are not limited. Therefore, those skilled in the art of the present invention can modify or equivalently replace the technical solutions recorded in the foregoing embodiments; and these modifications and replacements do not depart from the gist and technical solutions of the present invention and should all fall within the protection scope of the present invention.
Claims
1. A data filtering method for a sonar altimeter of an underwater glider device, characterized in that, Including: Step S1: For the vertical plane motion process of the underwater glider in the underwater environment, establish the state vector of the underwater glider; Based on the state vector, establish the state equation of the underwater glider; Step S2: Based on the relationship between the sonar altimeter sampling value and the bottom clearance of the underwater glider, establish the observation equation of the underwater glider; Step S3: Define the time interval between the sampling moments for two consecutive altitude samplings as a step. At each sampling moment, calculate the current state respectively through the Kalman filtering algorithms with step sizes of 1 and n to obtain the first filtering result of the current state of the underwater glider device and the second filtering result respectively represent the i-th state at the k-th moment when the step sizes are 1 and n; Step S4: Based on two preset window distance matrices and determine whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation and enter step S3; otherwise, use the state estimate value obtained from the state equation based on the state parameters of the underwater glider at this time as the value of the filtered sonar altimeter.
2. The method according to claim 1, characterized in that, The state vector of the underwater gliding device is x k = [h k d′ k d″ k T , where h k is the height of the underwater gliding device from the bottom at time t k , d k is the depth of the underwater gliding device at time t k , d k ′, d k ″ are the first derivative and the second derivative of d k respectively. 3. The method according to claim 2, wherein The state equation of the underwater glider is Among them, T is the sampling period of the underwater glider control system, h k-1 is the height of the underwater glider from the bottom at time t k-1 , d k-1 ′ is the vertical velocity of the underwater glider at time t k-1 , d k-1 ″ is the vertical acceleration of the underwater glider at time t k-1 , w k-1 is the process excitation noise at time t k-1 .
4. The method according to claim 3, wherein The observation equation is: where α is the inclination angle during the diving process of the underwater glider, β is the angle between the sonar altimeter and the horizontal plane of the underwater glider, and v k is the observation noise at time t k which is Gaussian white noise following a normal distribution N(0, R), and R represents the observation noise covariance matrix.
5. The method according to any one of claims 1 to 4, characterized in that, Based on the two pre-set window distance matrices and determine whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation, including: If Let R 1 = λ max R, where λ max is a fixed value greater than 1, thereby increasing the value of the observation noise covariance and reducing its proportion in the filtering result. R 1 is the value of the adjusted observation noise covariance matrix R; If Let R 1 = λ min R, where 0 < λ min < 1, reduce the observation noise covariance value and increase its proportion in the filtering result.
6. A data filtering device for a sonar altimeter of an underwater gliding device, characterized in that, Including: State equation module: Configured to establish the state vector of the underwater glider for the vertical plane motion process of the underwater glider in the underwater environment; Based on the state vector, establish the state equation of the underwater glider; Observation equation module: Configured to establish the observation equation of the underwater glider based on the relationship between the sonar altimeter sampling value and the bottom clearance of the underwater glider; Filter result determination module: configured to define the time interval between the sampling times for two consecutive altitude samplings as a step. At each sampling time, the current state is calculated respectively by the Kalman filtering algorithms with step sizes of 1 and n to obtain a first filtering result of the current state of the underwater glider device and a second filtering result respectively represent the i-th state at the k-th moment when the step sizes are 1 and n; Adjustment module: configured to be based on two preset window distance matrices and Determine whether the change amount between the first filtering result and the second filtering result is greater than a preset threshold. If it is greater than the preset threshold, adjust the observation noise covariance matrix R in the observation equation and enter step S3; otherwise, use the state estimate value obtained from the state equation based on the state parameters of the underwater glider at this time as the value of the filtered sonar altimeter.
7. A computer-readable storage medium storing multiple instructions; the multiple instructions are used to be loaded and executed by a processor to perform the method according to any one of claims 1-5.
8. An electronic device, characterized in that, The electronic device includes: A processor for executing multiple instructions; A memory for storing multiple instructions; Wherein, the multiple instructions are used to be stored by the memory and loaded and executed by the processor to perform the method according to any one of claims 1-5.