A combined acoustic and optical guidance information processing method for AUV recovery from ice wells

Abstract

This invention proposes a combined acoustic and optical guidance information processing method for AUV recovery from ice wells, belonging to the field of underwater vehicle recovery technology. It solves the problem of low accuracy of AUV guidance information under ice cover conditions. The steps include: Step 1: Obtaining acoustic guidance information based on an acoustic positioning system; Step 2: Obtaining optical guidance information based on an optical positioning system; Step 3: Processing the acoustic and optical guidance information respectively to obtain acoustic guidance information processing results and optical guidance information processing results, and then transmitting these results to the AUV to obtain an accurate estimate of the ice well center position and complete the AUV recovery.

Description

Technical Field

[0001] This invention relates to a combined acoustic and optical guidance information processing method for ice well recovery of AUVs, belonging to the field of underwater vehicle recovery technology. Background Technology

[0002] With the deepening of marine exploration in the Arctic and Antarctic regions in recent years, the operational requirements for AUVs (Aeronautical Valves) in scientific research operations are also increasing. Researching safe recovery technologies for AUVs under ice cover has become a key issue that needs to be addressed for the widespread application of AUVs in polar regions. Currently, AUV recovery in ice-covered areas typically employs acoustic guidance and optical guidance positioning. Acoustic guidance positioning has a larger effective positioning range, but its near-field positioning accuracy is lower than that of optical guidance; while optical guidance is affected by seawater and light in the underwater environment, resulting in a shorter effective distance. Furthermore, the operating environment of polar AUVs is covered by ice, and AUVs themselves lack icebreaking capabilities. Deployment and recovery of AUVs usually require drilling ice wells in open water areas between ice sheets or on the ice surface. Ice well recovery of AUVs requires high-precision guidance information to ensure successful recovery of AUVs under ice. Summary of the Invention

[0003] To address the problem of low guidance information accuracy for AUVs under ice cover conditions, this invention proposes a combined acoustic and optical guidance information processing method for AUVs recovering from ice wells, comprising the following steps:

[0004] Step 1: Obtain acoustic guidance information based on the acoustic positioning system;

[0005] Step 2: Obtain optical guidance information based on the optical positioning system;

[0006] Step 3: Process the acoustic guidance information and optical guidance information respectively to obtain the acoustic guidance information processing results and the optical guidance information processing results. Send the acoustic guidance information processing results and the optical guidance information processing results to the AUV to obtain an accurate estimate of the ice well center position and complete the AUV recovery.

[0007] Preferably, step 1 specifically includes:

[0008] Step 1.1: Establish an acoustic guidance model and provide three-dimensional distance information of the AUV relative to the ice-based acoustic positioning device based on the acoustic positioning system. The ice-based acoustic positioning device is an ultra-short baseline positioning device installed near the ice well.

[0009] Step 1.2: Based on the AUV's heading and the orientation information of the ice-based acoustic positioning device, determine the coordinate transformation relationship between the AUV's hull coordinate system and the ice-based acoustic positioning device's coordinate system. Combined with the AUV's position, depth, and speed information, as well as the relative position information of the ice-based acoustic positioning device and the center of the ice well, calculate the acoustically guided ice well position information through coordinate transformation.

[0010] Preferably, step 2 specifically includes:

[0011] An optical guidance model was established. Based on the optical system, the uncalibrated pixel coordinates, AUV heading, position, and distance to the water surface of the ice well center in the image coordinate system of the camera mounted on the AUV were obtained. Combined with the camera's installation position information, the position information of the ice well center in the latitude and longitude coordinate system of the optical guidance was calculated through coordinate transformation.

[0012] Preferably, step 3, obtaining an accurate estimate of the ice well center location and completing AUV recovery, specifically includes:

[0013] During the long-distance approach of the AUV to the ice well, an N-round clustering analysis algorithm is used to remove spurious values ​​in the acoustic guidance information, and the processed acoustic guidance ice well location information is obtained based on mathematical statistics. The AUV then approaches the ice well location based on the processed acoustic guidance ice well location information.

[0014] During the process of aligning the AUV with the ice well, a Kalman filter and a nonlinear weighted average method based on data quality are used to process the optical guidance information, obtain an accurate estimate of the ice well center position, and complete the AUV recovery.

[0015] Preferably, step 3, which involves processing the acoustic guidance information to obtain the acoustic guidance information processing result, includes:

[0016] Step 3.1.1: Perform N rounds of cluster analysis on the acoustically guided ice well location information, remove spurious data in the cluster analysis results, and narrow down the dispersion range;

[0017] Step 3.1.2: Calculate the average value of the acoustically guided ice well location information after cluster analysis, and use the calculation result as the acoustic guidance information processing result.

[0018] Preferably, step 3.1.1, which involves performing N rounds of cluster analysis on the acoustically guided ice well location information, includes:

[0019] Step 3.1.1.1: Obtain the clustering dataset X = {x1, x2, ..., x...} M},x n ∈R d (n = 1, 2, ..., M) where x n =(x n1 ,y n1 ), (n=1,2,…,M);

[0020] Step 3.1.1.2: Cluster the acoustically guided ice well location information into two categories: true data and false data, and determine the maximum number of iterations N;

[0021] Step 3.1.1.3: Select the initial cluster center for the true value data using the mean method, and randomly select a point within the ice well location region D as the initial cluster center for the false value data;

[0022] Step 3.1.1.4: Calculate the distance between each data point and the initial cluster center of the true value class and the initial cluster center of the false value class using the Euclidean distance metric, and assign the corresponding data points to the nearest cluster.

[0023] Step 3.1.1.5: Recalculate the cluster centers of the two new clusters after the partitioning using the mean method, and recalculate the distance between each data point and the two cluster centers, and assign the data to the new cluster with the closest distance;

[0024] Step 3.1.1.6: Repeat steps 3.1.1.4-3.1.1.5 until the positions of the cluster centers after iterations no longer change, and output the results of this round of cluster analysis;

[0025] Step 3.1.1.7: Use the true value class data of the cluster analysis results in Step 3.1.1.6 as the cluster data set for the next round of cluster analysis, and repeat Step 3.1.1.1-Step 3.1.1.6 until the maximum number of iterations N is reached, and the cluster analysis is completed;

[0026] The expression for the location region D of the ice well is:

[0027]

[0028] In formula (1), (x,y) are the coordinates of the initial cluster centers of the false-value data.

[0029] Preferably, step 3, which involves processing the optical guidance information to obtain the optical guidance information processing result, includes:

[0030] Step 3.2.1: Combine AUV velocity information and inertial navigation information to construct a Kalman filter to fuse and filter the optically guided, uncalibrated ice well center position information;

[0031] Step 3.2.2: Sort the ice well center location information after fusion and filtering by distance and take the average value. Starting from the first set of data of the n sets of ice well center location information after fusion and filtering, for each set of data currently being processed, take the pixel coordinate distance of the ice well center from the center point of the image in the image coordinate system of the processed data in the dataset, and sort it in descending order by bubble sort. Take the average value of the m sets of data that are closest to the center of the image as the processing result of the current data set. Traverse all data to obtain the latest n sets of ice well center location information.

[0032] Step 3.2.3: Perform a weighted average on the latest n sets of ice well center position information to complete the processing of optical guidance information, and use a nonlinear mapping relationship to provide the weighted average ice well center position as the optical guidance information processing result to the AUV control system.

[0033] The expression for the weighting function of the nonlinear mapping used in the weighted average is:

[0034]

[0035] In formula (2), s is the pixel coordinate distance between the center of the ice well and the center point of the image in the image coordinate system.

[0036] Preferably, step 3.2.1, which involves fusing and filtering the optically guided, uncalibrated ice well center location information, includes:

[0037] Step 3.2.1.1: Define the state parameter X and the observation parameter Z;

[0038] Step 3.2.1.2: Import the uncalibrated ice well center location information;

[0039] Step 3.2.1.3: Set the initial values ​​of the state parameters X0 and P0, and set the covariance matrix Q of the system noise. k The covariance matrix R of observation noise k ;

[0040] Step 3.2.1.4: Establish the recursive equation for the Kalman filter, recursively update the system's position and velocity state information, and obtain the optimal estimate of the filter. The recursive equations for Kalman filtering include time update equations and measurement update equations.

[0041] Step 3.2.1.5: Perform coordinate transformation on the optimal location information of the filtered ice well center to calculate the location information of the filtered and fused ice well center.

[0042] The beneficial effects of this invention are:

[0043] This invention provides different processing methods for acoustic and optical guidance information during the approach to the ice well and the search for the guide light source in the recovery of sub-ice AUVs, and during the alignment of the AUV with the ice well. It employs a combined acoustic-optical guidance approach. During the approach to the ice well from a greater distance, an N-round clustering analysis algorithm is used to process the acoustic guidance information, effectively eliminating outliers. A mathematical statistical method is used to overcome the challenge of temporal filtering due to the long and variable period of the acoustic guidance information. During close-range alignment with the ice well, a Kalman filter and a nonlinear weighted average method based on data quality are used to process the optical guidance information. This improves the accuracy of the guidance information when the distance between the AUV and the ice bottom is stable and the optical positioning information is not calibrated and cannot provide precise location information. This provides a high-precision, high-efficiency, stable, and reliable guidance information processing method for the safe recovery of sub-ice AUVs. Attached Figure Description

[0044] Figure 1 A schematic flowchart of an acoustic-optical combined guidance information processing method for ice well recovery of AUVs provided by the present invention;

[0045] Figure 2 A flowchart for processing acoustic guidance information provided by the present invention;

[0046] Figure 3 A flowchart for processing optical guidance information provided by the present invention;

[0047] Figure 4 The flowchart of the N-round clustering analysis algorithm provided by this invention is shown. Detailed Implementation

[0048] Combination Figure 1-4 This embodiment will be described as follows: Figure 1 As shown, the steps of the acoustic-optical joint guidance information processing method for AUV recovery from ice wells described in this embodiment include:

[0049] S1: Acoustic guidance information is obtained based on the acoustic positioning system and the acoustic guidance information is processed;

[0050] In this implementation, during the long-distance approach of the AUV to the ice well, an N-round clustering analysis algorithm is used to remove spurious values ​​from the acoustic guidance information, and the processed acoustically guided ice well location information is obtained based on mathematical statistics. The AUV then approaches the ice well location according to this processed acoustically guided ice well location information. Figure 2 As shown, the acoustic guidance process includes the following steps:

[0051] S101: Establish an acoustic guidance model and collect ice well location information;

[0052] The acoustic positioning system used in this embodiment is an ultra-short baseline positioning system. The array of the ice-based acoustic positioning device, namely the USBL array, is installed near the ice well, and the acoustic transponder is installed on the target AUV.

[0053] When the AUV approaches the ice well from a considerable distance and searches for a guiding light source, this embodiment utilizes its onboard acoustic positioning system to directly measure the three-dimensional distance of the target AUV relative to the USBL array. Then, combining this with the AUV's heading and the orientation information of the ice-based acoustic positioning device, the coordinate transformation relationship between the AUV's hull coordinate system and the ice-based acoustic positioning device's coordinate system is determined. Furthermore, by combining the AUV's position, depth, and speed information, as well as the relative position information between the ice-based acoustic positioning device and the center of the ice well, the ice well's location information in the geodetic coordinate system under acoustic guidance is calculated after coordinate transformation.

[0054] S102: Perform N rounds of cluster analysis on the collected ice well location information, and cluster them into two categories: true value class and false value class. The false value class information is the outlier that needs to be removed due to the long period and non-fixed period of the location information collected by the acoustic positioning system, as well as the movement of AUV and the reflection of ice layer.

[0055] like Figure 4 As shown, the algorithmic processing steps for N rounds of clustering analysis include:

[0056] S10201: Determine the ice well location data set X = {x1, x2, ..., x...} for clustering. M},x n ∈R d ,(n=1,2,···,M), where x n =(x n1 ,y n1 Given a subset of n(n=1,2,...,M), determine the maximum number of iterations N.

[0057] The expression for the location region D of the ice well is:

[0058]

[0059] In formula (1), (x,y) are the coordinates of the initial cluster centers of the false-value data;

[0060] S10202: Selecting initial cluster centers: Take the mean of all cluster data as the initial center of the true value cluster; randomly select a point within the range of ice well location coordinates of all clusters, i.e., region D, as the initial center of the false value cluster;

[0061] S10203: For all data x n Using the Euclidean distance metric, the distance to the cluster centers of each type of cluster is calculated. If the data x n With cluster center ci The distance between them is less than their distance to the cluster center c. j If the distance is (i≠j), then the data x n Classified to C i In the cluster;

[0062] S10204: Recalculate C for each cluster i Cluster center c i The mean method is used:

[0063]

[0064] In formula (2), n i For cluster C i The number of data points in the middle;

[0065] S10205: Repeat S10203-S10204, iterating until the iteration number n is reached or the data within each cluster no longer changes. Then the iteration ends, and the clustering result C is output. i If a cluster is identified, the clustering process is considered complete; otherwise, the next iteration continues.

[0066] S10206: After each clustering is completed, the true value data from the output of the previous S10205 is used to update X, and the true and false value clustering of S10201-S10205 is continued in a loop until the maximum number of iterations N is reached, and the Nth round of clustering analysis algorithm ends.

[0067] S103: Calculate the average value of the acoustically guided ice well location information after cluster analysis, and use the calculation result as the acoustic guidance information processing result.

[0068] S2: Obtain optical guidance information based on the optical positioning system and process the optical guidance information;

[0069] In this implementation, during the alignment of the AUV with the ice well, a Kalman filter and a nonlinear weighted averaging method based on data quality are used to process optical guidance information, obtain an accurate estimate of the ice well's center position, and complete the AUV retrieval. Figure 3 As shown, the processing steps for optical guidance information include:

[0070] S201: Establishing an optical guidance model:

[0071] The optical positioning system employed in this embodiment uses a camera as the basic tool to collect optical guidance information. With the AUV's distance from the ice bottom stable, a close-range alignment process with the ice well is performed. The onboard camera captures images of the guide light source positioned at the ice well, thus obtaining the pixel coordinates of the ice well's center in the image coordinate system of the camera mounted on the AUV. Combined with the camera's installation position information, the relative position between the AUV and the ice well's center is calculated. Then, based on the AUV's heading, position, and distance from the water surface, coordinate transformation is performed to calculate the ice well's center position in the geodetic coordinate system for optical guidance.

[0072] S202: Combine the speed information of the AUV to construct a Kalman filter to perform fusion filtering on the uncalibrated ice well center location information;

[0073] In this implementation, the relative position of the AUV to the center of the ice well and the speed of the AUV are taken as state variables and a state equation is established. The target AUV can be considered to be moving at a uniform linear speed in a short time. The state transition matrix Φ in the state equation is constructed.

[0074] The expression for the state variable is:

[0075]

[0076] In formula (3), x is the relative positional deviation between the AUV and the ice well center in the x-direction, and y is the relative positional deviation between the AUV and the ice well center in the y-direction. Let T be the AUV speed and T be the matrix transpose.

[0077] The expression for the state equation is:

[0078] X k =Φ k,k-1 X k-1 +W k-1 (4);

[0079] In formula (4), X k W is the state of the system at time k. k It is system process noise, which is Gaussian white noise, Φ k,k-1 This is the state transition matrix from time k-1 to time k, expressed as:

[0080]

[0081] The relative position of the AUV to the center of the ice well, obtained by coordinate transformation of the images captured by the camera, and the AUV velocity measured by the inertial navigation system are taken as the observations, and the observation equations are established.

[0082] The expression for the observed quantity is:

[0083]

[0084] The expression for the observation equation is:

[0085] Z k =HX k +V k (7);

[0086] In formula (7), Z k Let X be the observation at time k. k Let H be the state of the system, H be the observation matrix, and V be the observation matrix. k The noise used for observation is Gaussian white noise;

[0087] The expression for the observation matrix is:

[0088]

[0089] The covariance matrices of the system noise W and the observation noise V are Q, respectively. k and R k The initial state value is taken from the first set of observed data values. The fusion filtering process includes:

[0090] S20201: Define the state parameter X and the observation parameter Z;

[0091] S20202: Importing observation data: Ice well center coordinates in the AUV hull coordinate system The velocity (u,v) of the AUV as measured by inertial navigation. T ;

[0092] S20203: Set the initial value of the state parameter X0 to the first set of observed data values, set the initial value of the state variance P0, and set the covariance matrix Q of the system noise. k The covariance matrix R of observation noise k ;

[0093] S20204: Establish the recursive equations for the Kalman filter, including time update equations and measurement update equations, to recursively update the system's position and velocity state information, and obtain the optimal estimate of the filter.

[0094] S20205: Perform coordinate transformation on the optimal location information of the filtered data to calculate the optimal estimated latitude and longitude coordinates of the ice well center.

[0095] S203: Sort the ice well center location information after fusion and filtering by distance and take the average value. Starting from the first set of data of the n sets of ice well center location information after fusion and filtering, for each set of data currently being processed, take the pixel coordinate distance of the ice well center from the center point of the image in the image coordinate system of the processed data in the dataset, and sort it in descending order by bubble sort. Take the average value of the m sets of data that are closest to the center of the image as the processing result of the current data set. Traverse all data to obtain the latest n sets of ice well center location information.

[0096] S204: The latest n sets of ice well center position information are weighted and averaged to complete the processing of optical guidance information. The weighted average ice well center position is then provided to the AUV control system as the optical guidance information processing result using a nonlinear mapping relationship.

[0097] The expression for the weighting function of the nonlinear mapping used in the weighted average is:

[0098]

[0099] In formula (9), s is the pixel coordinate distance between the center of the ice well and the center point of the image in the image coordinate system.

[0100] S3: The acoustic guidance information processing results and optical guidance information processing results are sent to the AUV to obtain an accurate estimate of the ice well center position and complete the AUV recovery.

[0101] In summary, this implementation method employs a combined acoustic and optical guidance approach. During the approach to the ice well from a considerable distance, an N-round clustering analysis algorithm is used to process the acoustic guidance information, effectively eliminating outliers in the obtained guidance information. A mathematical statistical method is used to overcome the challenge of temporal filtering due to the long and variable period of the acoustic guidance information. During the close-range alignment with the ice well, a Kalman filter and a nonlinear weighted average method based on data quality are used to process the optical guidance information. This improves the accuracy of the guidance information when the distance between the AUV and the ice bottom is stable and the optical positioning information is not calibrated and cannot provide a precise position. This provides a high-precision, high-efficiency, stable, and reliable guidance information processing method for the safe recovery of AUVs under ice.

[0102] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent substitutions, and improvements made to the above embodiments without departing from the scope of the present invention, based on the technical essence of the present invention and within the spirit and principles of the present invention, shall still fall within the protection scope of the present invention.

Claims

1. A method for combined acoustic and optical guidance information processing for AUV recovery from ice wells, characterized in that, The steps of the aforementioned method for combined acoustic and optical guidance information processing for AUVs recovering from ice wells include: Step 1: Obtain acoustic guidance information based on the acoustic positioning system; Step 2: Obtain optical guidance information based on the optical positioning system; Step 3: Process the acoustic guidance information and optical guidance information respectively to obtain the acoustic guidance information processing result and the optical guidance information processing result. Send the acoustic guidance information processing result and the optical guidance information processing result to the AUV to obtain an accurate estimate of the ice well center position and complete the AUV recovery. Step 3, which involves processing the acoustic guidance information to obtain the processing result, includes: Step 3.1.1: Perform N rounds of cluster analysis on the acoustically guided ice well location information, remove spurious data in the cluster analysis results, and narrow down the dispersion range; Step 3.1.2: Calculate the average value of the acoustically guided ice well location information after cluster analysis, and use the calculation result as the acoustic guidance information processing result; Step 3, which involves processing the optical guidance information to obtain the processing result, includes: Step 3.2.1: Combine AUV velocity information and inertial navigation information to construct a Kalman filter to fuse and filter the optically guided, uncalibrated ice well center position information; Step 3.2.2: Sort the ice well center location information after fusion and filtering by distance and take the average value. Starting from the first set of data of the n sets of ice well center location information after fusion and filtering, for each set of data currently being processed, take the pixel coordinate distance of the ice well center from the center point of the image in the image coordinate system of the processed data in the dataset, and sort it in descending order by bubble sort. Take the average value of the m sets of data that are closest to the center of the image as the processing result of the current data set. Traverse all data to obtain the latest n sets of ice well center location information. Step 3.2.3: Perform a weighted average on the latest n sets of ice well center position information to complete the processing of optical guidance information, and use a nonlinear mapping relationship to provide the weighted average ice well center position as the optical guidance information processing result to the AUV control system. The expression for the weighting function of the nonlinear mapping used in the weighted average is: （2）； In formula (2), s This represents the pixel coordinate distance between the center of the ice well and the center point of the image in the image coordinate system.

2. The method for combined acoustic and optical guidance information processing for AUV recovery from ice wells according to claim 1, characterized in that, Step 1 specifically includes: Step 1.1: Establish an acoustic guidance model and provide three-dimensional distance information of the AUV relative to the ice-based acoustic positioning device based on the acoustic positioning system. The ice-based acoustic positioning device is an ultra-short baseline positioning device installed near the ice well. Step 1.2: Based on the AUV's heading and the orientation information of the ice-based acoustic positioning device, determine the coordinate transformation relationship between the AUV's hull coordinate system and the ice-based acoustic positioning device's coordinate system. Combined with the AUV's position, depth, and speed information, as well as the relative position information of the ice-based acoustic positioning device and the center of the ice well, calculate the acoustically guided ice well position information through coordinate transformation.

3. The method for combined acoustic and optical guidance information processing for AUV recovery from ice wells according to claim 1, characterized in that, Step 2 specifically includes: An optical guidance model was established. Based on the optical system, the uncalibrated pixel coordinates, AUV heading, position, and distance to the water surface of the ice well center in the image coordinate system of the camera mounted on the AUV were obtained. Combined with the camera's installation position information, the position information of the ice well center in the latitude and longitude coordinate system of the optical guidance was calculated through coordinate transformation.

4. The method for combined acoustic and optical guidance information processing for AUV recovery from ice wells according to claim 1, characterized in that, Step 3, obtaining an accurate estimate of the ice well's center location and completing AUV recovery, specifically includes: During the long-distance approach of the AUV to the ice well, an N-round clustering analysis algorithm is used to remove false values ​​in the acoustic guidance information, and the processed acoustic guidance ice well location information is obtained based on mathematical statistics. The AUV then approaches the ice well location according to the processed acoustic guidance ice well location information. During the process of aligning the AUV with the ice well, a Kalman filter and a nonlinear weighted average method based on data quality are used to process the optical guidance information, obtain an accurate estimate of the ice well center position, and complete the AUV recovery.

5. The method for combined acoustic and optical guidance information processing for AUV recovery from ice wells according to claim 1, characterized in that, Step 3.1.1, which involves performing N rounds of cluster analysis on the acoustically guided ice well location information, includes: Step 3.1.1.1: Obtain the clustering dataset. ,in ; Step 3.1.1.2: Cluster the acoustically guided ice well location information into two categories: true data and false data, and determine the maximum number of iterations N; Step 3.1.1.3: Select the initial cluster center for the true value data using the mean method, and randomly select a point within the ice well location region D as the initial cluster center for the false value data; Step 3.1.1.4: Calculate the distance between each data point and the initial cluster center of the true value class and the initial cluster center of the false value class using the Euclidean distance metric, and assign the corresponding data points to the nearest cluster. Step 3.1.1.5: Recalculate the cluster centers of the two new clusters after the partitioning using the mean method, and recalculate the distance between each data point and the two cluster centers, and assign the data to the new cluster with the closest distance; Step 3.1.1.6: Repeat steps 3.1.1.4-3.1.1.5 until the positions of the cluster centers after iterations no longer change, and output the results of this round of cluster analysis; Step 3.1.1.7: Use the true value class data of the cluster analysis results in Step 3.1.1.6 as the cluster data set for the next round of cluster analysis, and repeat Step 3.1.1.1-Step 3.1.1.6 until the maximum number of iterations N is reached, and the cluster analysis is completed; The expression for the location region D of the ice well is: （1）； In formula (1), These are the coordinates of the initial cluster centers for the false-value data.

6. The method for combined acoustic and optical guidance information processing for AUV recovery from ice wells according to claim 1, characterized in that, Step 3.2.1, which involves fusing and filtering the optically guided, uncalibrated ice well center location information, includes: Step 3.2.1.1: Define state parameters and observation parameters ; Step 3.2.1.2: Import the uncalibrated ice well center location information; Step 3.2.1.3: Set initial values ​​for state parameters and initial value of state variance Set the covariance matrix of the system noise. Covariance matrix of observation noise ; Step 3.2.1.4: Establish the recursive equation for the Kalman filter, recursively update the system's position and velocity state information, and obtain the optimal estimate of the filter. The recursive equations for Kalman filtering include time update equations and measurement update equations. Step 3.2.1.5: Perform coordinate transformation on the optimal location information of the filtered ice well center to calculate the location information of the filtered and fused ice well center.

Images