Underwater positioning method based on Sage-Husa adaptive Kalman filtering
By combining a ternary triangular array with time delay estimation and Sage-Husa adaptive Kalman filtering, the problems of complex structure and noise interference of mine sensors were solved, enabling high-precision positioning and speed prediction of underwater target ships and extending underwater working time.
Patent Information
- Application Number
- CN202511961167.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-24
- Publication Date
- 2026-05-08
AI Technical Summary
Existing mines have complex external sensor structures and high power module loads. Acoustic signals suffer from background noise and multipath propagation during underwater detection, leading to inaccurate direction and velocity information. Furthermore, the existing array method increases the power system load and conflicts with the miniaturization of mines.
A three-element triangular array is used for acoustic signal detection. Combined with time delay estimation and coordinate system transformation, Sage-Husa adaptive Kalman filtering is used to correct the ship's position and speed, reduce the impact of noise, and improve detection accuracy.
By reducing the number of array elements to extend underwater working time, improve detection accuracy, and reduce noise interference, a real-time and simple underwater positioning method can be achieved, which can determine the accurate position and speed of the target ship.
Smart Images
Figure CN121995316A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of target detection technology, and in particular to an underwater localization method based on Sage-Husa adaptive Kalman filtering. Background Technology
[0002] Environmental perception is the primary step for mines to interact with their surroundings, and the arrangement of array elements and the selection of algorithms have a significant impact on perception accuracy and speed. Environmental perception systems acquire useful information from raw data from multiple sensors using various technical means to obtain the current and future status of surface target ships, providing mines with relatively accurate target positions and motion parameters. Since mines need to operate underwater for extended periods in a relatively small size, it is essential to simplify their external sensor structure as much as possible to reduce the load on the power module from multiple sensors.
[0003] Acoustic signals underwater are subject to background noise and multipath propagation, which can lead to deviations in the detection space. This can cause significant fluctuations in the direction finding and velocity measurement information acquired by underwater acoustic arrays, or even outliers. It is necessary to use multi-sensor arrays to offset acquisition errors or to use high-precision data optimization methods to reduce noise interference to the system.
[0004] Currently, the field of acoustic signal sensing utilizes array element arrangements with multiple symmetries, such as five-element cross arrays and eight-element circular arrays, to improve detection accuracy and range. However, this also increases the load on the power supply system, which conflicts with the miniaturization of sea mines. Summary of the Invention
[0005] This disclosure provides an underwater positioning method based on Sage-Husa adaptive Kalman filtering. It employs a minimum positioning array element, a three-element triangular array, to detect surface ships. To address the insufficient accuracy of the three-element triangular array, it improves the detection accuracy by obtaining the distance and azimuth of the ship target in the geodetic coordinate system from the detection platform through time delay estimation and coordinate system transformation. Furthermore, to address the issue of underwater background noise in the acoustic signal, a positioning data processing method based on Sage-Husa adaptive Kalman filtering is proposed. This effectively improves the detection accuracy of the platform for target ships, reduces the significant spatial offset of the target due to noise, and obtains more accurate surface positioning results.
[0006] This method mainly includes the following steps: S1, set up a planar three-element array to collect signals from the target ship; S2, the time delay estimation method is used to calculate the pitch and azimuth angles of the target ship relative to the planar three-element array; S3, using the coordinate rotation method to calculate the three-axis x, y, z coordinates and x and y-axis velocities of a ship on the water surface in geodetic coordinates; S4 employs a Sage-Husa adaptive Kalman filter to correct the x and y axis coordinates and velocity, obtaining stable ship position and axial velocity, which is used to predict the ship's position in a short period of time and improve strike accuracy.
[0007] Furthermore, the three elements of the planar ternary array are distributed in an equilateral triangle.
[0008] Furthermore, the specific method of step S2 includes: Let ABC be the three elements of the planar ternary array; when the position T of the target ship is much larger than the array element spacing, the target ship is regarded as a point sound source, and spherical waves are emitted to the three points of the planar ternary array. Let the arrival time delay of the signal from B relative to A be . , Speed of sound; When the signal sampling period is At that time, the final arrival time delay of the signal from point B relative to point A is:
[0009] In the formula, , is the signal arrival delay of point B relative to point A when the correlation function of the received signals at points A and B reaches its maximum value;
[0010] In the formula, The peak-to-peak value of the correlation function. , The values are the two points adjacent to the left and right of the peak of the correlation function; Using the same method, obtain the final arrival time delay of the signal between the other two points; Center point of the planar ternary array Assuming the coordinate origin is the point where array elements A, B, and C are all at a distance r from the center point, then the azimuth angle α of the target ship is: ; The target ship's pitch angle β is:
[0011] In the formula, , Let B be the signal arrival delay relative to C.
[0012] Furthermore, the specific method of step S3 includes: The angle between the plane coordinate system and the geodetic coordinate system is measured using a compass on the measuring platform: pitch angle. yaw angle Azimuth ; Based on the principle of spatial coordinate system transformation, the transformation matrix from the observation platform coordinate system to the geodetic coordinate system is obtained: Record
[0013] Solve for the actual pitch angle of the target ship in the geodetic coordinate system. and azimuth for:
[0014]
[0015] in , ; Once the height of the observation platform above the water surface is determined, the position and velocity of the target ship in the geodetic coordinate system are determined by the actual pitch and azimuth angles.
[0016] Furthermore, the specific method of step S4 includes: The target ship's position coordinates in the geodetic coordinate system are used as the observation variable: The ship's position coordinates and axial velocity are denoted as state variables: , At this point, the observation equation based on the Saga-Husa adaptive Kalman filter is as follows:
[0017] Both sensor accuracy and underwater noise are white noise. In this case, the observation equation satisfies the following condition:
[0018]
[0019]
[0020] in This represents the average process noise. To measure the noise mean, The prediction process is as follows:
[0021] in, To predict the state vector, The prediction error covariance matrix; The Kalman filter update process is as follows:
[0022] in It is the identity matrix. This is the filter gain matrix; Considering the unbiasedness of the Sage-Husa adaptive Kalman filter, the expression for the suboptimal maximum a posteriori estimator is obtained based on the maximum a posteriori estimation principle:
[0023] An improvement to the traditional Sage-Husa estimator is achieved by adding a negative exponential weight to the above suboptimal maximum a posteriori estimation expression, resulting in the following expression:
[0024] in As a weighting factor, Forgetting factor and .
[0025] Furthermore, the method also includes the following steps: S5, based on current data, predicts in advance the minimum distance between surface ships and the observation point.
[0026] Based on the current data, the minimum distance between surface ships and the observation point is predicted in advance; if the minimum distance gradually decreases, the prediction continues with the next set of data; if the minimum distance reaches a minimum value, it means that the ship will pass over the observation point within a few seconds.
[0027] Furthermore, the specific method for predicting the minimum distance between the surface ship and the observation point in step S5 includes: After obtaining stable geodetic coordinates and axial velocity output through time-varying Kalman filtering, the target is estimated using the least squares linear fitting method to predict the minimum distance between the target and the observation point. The least squares linear fitting method assumes that the target is moving at a constant velocity in a straight line, and its equation of motion is: The observations obtained at time t are:
[0028] Where n is the number of fitting points; Least squares parameter estimation:
[0029] The minimum distance calculated using the least squares method when the target passes directly above the observation point is: .
[0030] Since the minimum distance between the target and the observation point is a minimum when the target passes directly above the observation point, based on this, the minimum distance between the target and the observation point is estimated based on the target's current position and velocity information. If the minimum distance gradually decreases, the Kalman filter is returned to continue iterating the next data for fitting calculation; if the minimum distance reaches a minimum value, it means that the ship will pass above the observation point within a few seconds, so the calculation stops and the predicted final position and velocity values are output.
[0031] Compared with the prior art, the beneficial effects of this disclosure are: 1. By reducing the number of array elements, the underwater working time of the system is extended, and the accuracy loss caused by the reduction of the number of array elements is reduced by the time delay estimation algorithm; 2. The proposed Sage-Huge-based time-varying adaptive Kalman filter method optimizes the Kalman filter method by adding a weight factor in the form of a negative exponential law, which greatly improves the tracking ability of the original algorithm when the observed parameters change significantly. 3. Compared with common underwater positioning methods, it reduces the impact of external noise (environmental noise and sensor noise, etc.) on the system, and has the advantages of good real-time performance and simple implementation; 4. The underwater positioning method proposed in this disclosure has been implemented in engineering and can perform trajectory fitting on ships in the target area to determine the minimum position and speed at the test point. Attached Figure Description
[0032] The above and other objects, features and advantages of this disclosure will become more apparent from the more detailed description of exemplary embodiments of this disclosure taken in conjunction with the accompanying drawings, in which the same reference numerals generally represent the same components.
[0033] Figure 1 A diagram of a planar ternary array positioning model according to an exemplary embodiment; Figure 2 This is a schematic diagram illustrating the principle of spatial coordinate system transformation. Figure 3 This is the overall flowchart. Detailed Implementation
[0034] Preferred embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While preferred embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure will be thorough and complete, and will fully convey the scope of the present disclosure to those skilled in the art.
[0035] This disclosure provides an underwater positioning method based on Sage-Husa time-varying adaptive Kalman filtering. Based on a planar three-element array, the method estimates the target ship's position on the water surface through time delay estimation, coordinate system transformation, Kalman filtering, and least squares method, and calculates the shortest distance and speed of the target passing the observation point.
[0036] In one exemplary implementation, the main flow of the method is as follows: Figure 3 As shown, the main steps are as follows: 1. Set up a planar ternary array Planar ternary array positioning model as follows Figure 1 As shown, A, B, and C represent three elements of a planar three-element array, which are distributed in an equilateral triangle. T is the position of the target ship, T' is the projection of the target ship onto the three-element array plane, α is the azimuth angle, and β is the elevation angle.
[0037] 2. The time delay estimation method is used to calculate the elevation and azimuth angles of surface ships relative to the planar three-element array. If we assume that the signals received at points A and B are respectively
[0038] in: For the target sound source signal, , This is the received Gaussian ocean noise signal.
[0039] The cross-correlation function of the two signals is then given by
[0040] because , , If they are unrelated, then
[0041] At this time When the correlation function reaches its maximum value, it can be obtained by the generalized correlation theorem.
[0042] but
[0043] In the formula , for , The Fourier transform of the Fourier transform is calculated using FFT. , Then ask again .
[0044] According to the inverse Fourier transform (IFFT), we can obtain
[0045] To improve the accuracy of the delay algorithm, a three-point interpolation method is used. The relevant function reaches its maximum value. For interpolation, the following should be true:
[0046] in Peak-to-peak value of the correlation function , These are the values of the two points adjacent to the peak of the correlation function.
[0047] When the sampling period is At that time, the final delay is .
[0048] In this embodiment, for example Figure 1 As shown in the planar three-element array elements A, B, and C, when the target ship's position T is much larger than the array element spacing, the target ship can be regarded as a point source, emitting spherical waves towards the three points of the planar three-element array, with the center point of the planar three-element array as the target. As the origin of the coordinate system, array elements A, B, and C are all located at a distance r from the center point, and their coordinates are as follows: , , The time delays at the three points are respectively , , , For the speed of sound, according to the spherical wave formula, we can obtain...
[0049] Solving
[0050]
[0051] Its azimuth can be calculated from this.
[0052] Since the target ship's position T is relatively far away, there is
[0053]
[0054] At this point, we can perform further approximate calculations of the azimuth angle.
[0055] The pitch angle was calculated based on the target ship's horizontal and vertical coordinates.
[0056] Similarly, to make an approximate estimate of the pitch angle, we have:
[0057] Introducing the array element spacing The formula for obtaining the azimuth angle
[0058] 3. Calculate the three-axis x, y, z coordinates and x, y-axis velocities of the ship on the water surface in geodetic coordinates using the coordinate rotation method. The pitch and azimuth angles obtained using the time delay estimation method are calculated relative to the measurement plane coordinate system. However, in the actual measurement process, it is necessary to obtain the corresponding position of the ship target in the geodetic coordinate system, calculate the ship's position and relative speed on the horizontal plane of the earth, so as to facilitate the analysis of the closest position between the target and the acquisition platform.
[0059] Using a compass on a surveying platform, one can observe the angle (pitch angle) between the plane coordinate system and the geodetic coordinate system. yaw angle Azimuth ).
[0060] like Figure 2 The principle of spatial coordinate system transformation shown can be used to obtain the transformation matrix from the observation platform coordinate system to the geodetic coordinate system:
[0061] Record
[0062] The actual pitch angle of the target ship in the geodetic coordinate system can be obtained. and azimuth for:
[0063]
[0064] in ,
[0065] 4. Correcting the target ship's position and velocity based on Sage-Husa adaptive Kalman filtering. Once the height of the observation platform above the water surface is determined, the position and velocity of the target ship in the geodetic coordinate system can be determined using the actual pitch and azimuth angles. However, the ship's position and velocity are difficult to stabilize due to fluctuations caused by sensor accuracy and underwater noise interference. Therefore, this embodiment proposes to use a time-varying Kalman filter based on Sage-Husa to obtain stable ship position and axial velocity. At this point, the ship's position coordinates in the geodetic coordinate system are taken as the observation variable: , The ship's position coordinates and axial velocity are denoted as state variables: , At this point, the observation equation based on the Saga-Husa adaptive Kalman filter is as follows:
[0066] Both sensor accuracy and underwater noise are white noise. In this case, the observation equation satisfies the following condition:
[0067]
[0068]
[0069] in This represents the average process noise. To measure the noise mean, The prediction process is as follows:
[0070] in, To predict the state vector, This is the prediction error covariance matrix.
[0071] The Kalman filter update process is as follows:
[0072] in It is the identity matrix. This is the filter gain matrix.
[0073] Considering the unbiasedness of the Sage-Husa adaptive Kalman filter, the expression for the suboptimal maximum a posteriori estimator, based on the maximum a posteriori estimation principle, should be:
[0074] In the state estimation process, since the weighted average of each old estimate is... However, in practical use, there may be significant changes in the new data compared to the old data (ship target turning, speed change, braking). Therefore, this embodiment improves the traditional Sage-Husa estimator to make the estimator time-varying. Negative exponential weighting is added to the above suboptimal maximum a posteriori estimation expression, resulting in the following expression:
[0075] in As a weighting factor, Forgetting factor and .
[0076] 5. Calculate the ship's trajectory in advance using the least squares method, and predict the minimum distance to the observation point a few seconds before the ship passes the observation point. After obtaining stable geodetic coordinates and axial velocity output through time-varying Kalman filtering, the target is estimated using the least squares straight-line fitting method to determine whether the target will pass through the observation point. If not, the minimum distance directly above the observation point is calculated. The least squares linear fitting method assumes that the target is moving at a constant velocity in a straight line, and its equation of motion is: The observations obtained at time t are:
[0077] Where n is the number of fitting points Least squares parameter estimation:
[0078] The minimum distance calculated using the least squares method when the target passes directly above the observation point is: At this point, based on the target's current position and velocity information, it is determined whether the target has passed the observation point within the set time. If it has not passed the observation point, the calculation returns to Kalman filtering to continue fitting. If it has passed the observation point, the calculation stops and the predicted final position and velocity values are output.
[0079] Algorithm flowchart as follows Figure 3 As shown. Based on the current data, the position of the surface ship is estimated a few seconds in advance. If the estimated minimum distance between the surface ship and the observation point gradually decreases within those few seconds, the next data is used to continue the estimation. If the estimated minimum distance between the surface ship and the observation point reaches a minimum value, it means that the ship will pass over the observation point within a few seconds, and preparations can be made to strike the target. At this time, the ship's position and speed are recorded a few seconds later.
[0080] In this embodiment, based on the minimum array elements of the planar array, a derivation method for the time delay estimation of the planar triangular array is given, and specific expressions for the pitch angle and azimuth angle of the target ship relative to the planar array are derived. Compared with the commonly studied five-element cross array and eight-element circular array, the number of transducers is reduced and the underwater working time of the system is extended. To address the issue of insufficient accuracy of the ternary triangular array, a method is proposed to obtain the distance and azimuth of the ship target to the detection platform in the geodetic coordinate system by using time delay estimation and coordinate system transformation. This improves the detection accuracy of the ternary triangular array, and a detailed derivation process is given. To address the issue of underwater background noise in acoustic signals, a time-varying adaptive Kalman filter based on Sage-Huge is proposed. This method processes the position and axial velocity in geodetic coordinates after coordinate transformation, which can effectively improve the detection accuracy of the detection platform for target ships, reduce the problem of large spatial offset of the target caused by noise, obtain more accurate water surface positioning results, and obtain stable geodetic coordinates and axial velocity output.
[0081] The above technical solutions are merely exemplary embodiments of the present invention. For those skilled in the art, based on the application methods and principles disclosed in the present invention, it is easy to make various types of improvements or modifications, and not limited to the methods described in the specific embodiments of the present invention. Therefore, the methods described above are merely preferred and not restrictive.
Claims
1. An underwater positioning method based on Sage-Husa adaptive Kalman filtering, characterized in that, Includes the following steps: S1, set up a planar three-element array to collect signals from the target ship; S2, the time delay estimation method is used to calculate the pitch and azimuth angles of the target ship relative to the planar three-element array; S3, using the coordinate rotation method to calculate the three-axis x, y, z coordinates and x and y-axis velocities of a ship on the water surface in geodetic coordinates; S4 uses Sage-Husa adaptive Kalman filtering to correct the x and y axis coordinates and velocity, thereby obtaining a stable ship position and axial velocity.
2. The method according to claim 1, characterized in that, The three elements of the planar ternary array are arranged in an equilateral triangle.
3. The method according to claim 1, characterized in that, The specific method of step S2 includes: Let ABC be the three elements of the planar ternary array; when the position T of the target ship is much larger than the array element spacing, the target ship is regarded as a point sound source, and spherical waves are emitted to the three points of the planar ternary array. Let the arrival time delay of the signal from B relative to A be . , Speed of sound; When the signal sampling period is At that time, the final arrival time delay of the signal from point B relative to point A is: In the formula, , where is the signal arrival delay of point B relative to point A when the correlation function of the received signals at points A and B reaches its maximum value; In the formula, The peak-to-peak value of the correlation function. , The values are the two points adjacent to the left and right of the peak of the correlation function; Using the same method, obtain the final arrival time delay of the signal between the other two points; Center point of the planar ternary array Assuming the coordinate origin is the point where array elements A, B, and C are all at a distance r from the center point, then the azimuth angle α of the target ship is: ； The target ship's pitch angle β is: In the formula, , Let B be the signal arrival delay relative to C.
4. The method according to claim 1, characterized in that, The specific method of step S3 includes: The angle between the plane coordinate system and the geodetic coordinate system is measured using a compass on the measuring platform: pitch angle. yaw angle Azimuth ; Based on the principle of spatial coordinate system transformation, the transformation matrix from the observation platform coordinate system to the geodetic coordinate system is obtained: Record Solve for the actual pitch angle of the target ship in the geodetic coordinate system. and azimuth for: in , ; Once the height of the observation platform above the water surface is determined, the position and speed of the target ship in the geodetic coordinate system are determined by the actual pitch and azimuth angles.
5. The method according to claim 1, characterized in that, The specific method of step S4 includes: The target ship's position coordinates in the geodetic coordinate system are used as the observation variable: The ship's position coordinates and axial velocity are denoted as state variables: ， At this point, the observation equation based on the Saga-Husa adaptive Kalman filter is as follows: Both sensor accuracy and underwater noise are white noise. In this case, the observation equation satisfies the following condition: in This represents the mean of the process noise. To measure the noise mean, The prediction process is as follows: in, To predict the state vector, The prediction error covariance matrix; The Kalman filter update process is as follows: in It is the identity matrix. This is the filter gain matrix; in As a weighting factor, Forgetting factor and .
6. The method according to any one of claims 1-5, characterized in that, It also includes the following steps: S5, based on current data, predicts in advance the minimum distance between surface ships and observation points.
7. The method according to claim 6, characterized in that, The specific method of step S5 includes: After obtaining stable geodetic coordinates and axial velocity output through time-varying Kalman filtering, the target trajectory is estimated using the least squares linear fitting method to predict the minimum distance of the target relative to the observation point. The least squares linear fitting method assumes that the target is moving at a constant velocity in a straight line, and its equation of motion is: The observations obtained at time t are: Where n is the number of fitting points; Least squares parameter estimation: The minimum distance is .