Method for calibrating sensing device in non-ideal environment

Abstract

The invention discloses a self-adaptive kernel bandwidth maximum entropy Kalman filtering DVL calibration method based on interactive multiple models, and belongs to the technical field of navigation sensor device calibration in the natural environment. The method comprises the following steps: S1, initializing a calibration system; s2, collecting data of a real sensor at the moment q, and calculating a proportional error at the current moment by using the collected data; s3, realizing bandwidth self-adaption by using a multi-model interaction algorithm framework; s4, filtering the installation angle by using maximum entropy Kalman filtering; s5, calculating the likelihood probability of the model, and updating the probability of the model; s6, performing interactive output, and calculating an output estimation state and an estimation covariance matrix of the normalized model; and S7, correcting the system state by using the estimated state error. According to the method, the installation error and the structure parameter of the DVL can be accurately calibrated, and the navigation precision is improved.

Description

Technical Field

[0001] This invention belongs to the field of calibration technology for navigation sensor devices in natural environments, specifically relating to an adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model. Background Technology

[0002] As underwater special mission vehicles, amphibious unmanned platforms require accurate real-time position information to complete other autonomous tasks. Currently, one of the most widely used navigation methods in underwater vehicles is the strapdown inertial navigation / Doppler velocimeter (SINS / DVL) integrated navigation system. DVL measures the velocity of the vehicle relative to the seabed or water column based on the Doppler effect, and combined with an inertial navigation system, it can achieve high-precision short-term autonomous navigation. In typical underwater missions lacking GPS signals and external base station support, DVL provides indispensable motion constraint information. However, in practical applications, installation errors in DVL can significantly affect navigation accuracy; therefore, accurate modeling and online correction of these errors are crucial for long-endurance autonomous operations.

[0003] Especially for amphibious unmanned platforms, the installation space for DVL (Depth-to-Low) sensors is extremely limited due to the constraints of miniaturization design. Furthermore, their unique operating conditions, such as bottom-crawling, make it difficult to precisely align the sensors with the SINS (Sinking Inertial Navigation System) coordinate system, often resulting in significant installation deviations. Traditional calibration methods based on small-angle approximations are therefore less applicable in such scenarios. In addition, DVL observation signals in underwater environments are susceptible to multipath reflections, obstruction by suspended objects, and flow field disturbances. Their measurement noise often exhibits non-Gaussian, time-varying statistical characteristics, further increasing the difficulty of calibration.

[0004] Existing calibration methods mainly include offline calibration techniques based on static or specific maneuver trajectories, and online estimation methods based on extended Kalman filtering (EKF) or unscented Kalman filtering (UKF). While offline calibration is simple and reliable, it cannot adapt to dynamic changes in the carrier and fluctuations in the underwater acoustic environment. Most online filtering methods are based on least squares or Gaussian noise assumptions, which can easily lead to estimation bias or even divergence in non-Gaussian noise environments. In recent years, some studies have attempted to introduce robust estimation theories (such as the Huber kernel function) to improve noise adaptability, but these methods typically rely on manual adjustment of kernel parameters, making them difficult to handle real-world scenarios where noise statistics vary over time. Furthermore, a single kernel bandwidth is prone to overfitting or underfitting when facing different noise distributions, lacking the ability to adaptively adjust to complex noise environments.

[0005] To address the aforementioned issues, this application proposes an adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-models. The aim is to achieve high-precision, adaptive online calibration for large installation angle errors by combining a multi-model framework with maximum entropy robust estimation, thereby improving the navigation reliability of amphibious unmanned platforms in complex underwater environments. Summary of the Invention

[0006] To address the problems mentioned in the background art, this invention proposes an adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model, which solves the problems of Doppler velocity meter (DVL) measurement errors not following a Gaussian distribution and the presence of outlier interference in non-ideal underwater environments, significantly improving the robustness and adaptability of the calibration algorithm; this method can accurately measure the installation error between SINS and DVL and the calibration coefficient error of DVL.

[0007] Technical Solution: To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0008] An adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-models includes the following steps:

[0009] S1. Perform calibration system initialization. Based on prior information, set the Markov transition matrix and initial model probability between different sub-filters of the multi-model system to complete the initialization of each sub-filter.

[0010] S2. Collect the actual sensor data at time q, and calculate the proportional error at the current time using the collected data according to the working principle of DVL.

[0011] S3. Install an angle estimation system and use a multi-model interaction algorithm framework to achieve bandwidth adaptation;

[0012] S4. Use maximum entropy Kalman filtering to filter the installation angle;

[0013] S5. Calculate the model likelihood probability and update the model probability;

[0014] S6. Interactive output: Calculate the output estimate of the normalized model. And estimate the covariance matrix ;

[0015] S7. Correct the system state using the estimated state error.

[0016] As a preferred option, the specific implementation details in S1 are as follows:

[0017] The collected data includes the raw output velocity of DVL, the output of the SINS / GPS fusion navigation system, and the output of the IMU gyroscope. Based on the DVL velocity measurement model, the collected data is used to calculate the proportional error at the current moment.

[0018] Based on the working principle of DVL, the speed error model is expressed as:

[0019] ,

[0020] in, Indicates the current output speed of the DVL; Indicates the proportional error of DVL; Represents the transformation matrix between DVL and SINS; Represents the coordinate transformation matrix from the vehicle system to the navigation system; This represents the true velocity value at the current DVL position; This represents the projection of the angular velocity of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle coordinate system. Indicates the lever arm of DVL and SINS; Represents the velocity error vector;

[0021] When calculating the proportional error, the velocity is integrated over time to effectively smooth out random noise, yielding the proportional error at the current moment, specifically:

[0022] ,

[0023] ,

[0024] in, Indicates the proportional error of DVL; Indicates the lever arm of DVL and SINS. This represents the projection of the angular velocity of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle coordinate system. This indicates the zero bias of the gyroscope; This represents the projection of the Earth's rotation speed onto the navigation system. This represents the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. This represents the estimated value of the coordinate transformation matrix from the carrier system to the navigation system; Indicates the sampling period; It represents the projection of the angular velocity of the carrier relative to the inertial coordinate system onto the carrier coordinate system.

[0025] As a preferred option, the specific implementation details in S3 are as follows:

[0026] S31. Initialize the multi-model system model and set the Markov transition matrix between different sub-filters based on prior information;

[0027] S32. Input interaction: Input the posterior state and covariance of each sub-filter at time q-1, the Markov transition matrix, and the prior probability of each model, and calculate the transition probability of the mixed normalized model from model j to model k.

[0028] S33. Input the estimated states of each model. and the corresponding normalized transition probabilities of the mixture model Calculate the mixed estimated state of model k;

[0029] S34. Input the estimated covariance matrix of each model and the corresponding normalized transition probability of the mixture model. Calculate the mixed estimate covariance of model k.

[0030] As a preferred option, the specific implementation details in S32 are as follows:

[0031] ,

[0032] ,

[0033] in, Represents the normalization constant. This represents the transition probability from model j to model k in the mixed normalized model; This represents the transition probability from model j to model k; Indicates time The probability of model k; Indicates time The probability of model j.

[0034] As a preferred option, the specific implementation details in S33 are as follows:

[0035] ,

[0036] in, Indicates time After mixing, the mixed initial state estimate is prepared for model k; Indicates time State estimation of model j; This represents the transition probability from model j to model k in the mixed normalized model.

[0037] As a preferred option, the specific implementation details in S34 are as follows:

[0038] ;

[0039] in, Indicates time The initial covariance matrix after mixing is prepared for model k; This represents the transition probability from model j to model k in the mixed normalized model; Indicates time The posterior estimation error covariance matrix of model j; Indicates time State estimation of model j; Indicates time After mixing, the mixed initial state estimate is prepared for model k; T represents the transpose operation.

[0040] As a preferred option, the specific implementation process in S4 is as follows:

[0041] S41. Initialize the filtering system;

[0042] S42. Define the system's installation angle error. This is the difference between the theoretical installation angle and the actual error angle.

[0043] S43. Construct a system observation error filtering model;

[0044] S44. Perform predictive updates on the system;

[0045] S45. Calculate the relevant entropy;

[0046] S46. Calculating New Information and new information covariance .

[0047] As a preferred option, the specific implementation process in S5 is as follows:

[0048] Calculate the likelihood probability of the model Specifically:

[0049] ;

[0050] in, Represent the new information covariance matrix; Represents the innovation vector; This represents the transpose of the innovation vector; represents the inverse matrix of the information covariance matrix; m represents the dimension of the multivariate normal distribution;

[0051] Update model probability The calculation formula is:

[0052] ,

[0053] in, Represents the model likelihood probability; represents the normalization constant; l represents the total number of mixed models.

[0054] As a preferred option, the specific implementation process in S7 is as follows:

[0055] ,

[0056] in, This represents the rotation matrix for estimating the installation angle at time q; This represents the rotation matrix for estimating the installation angle at time q-1; Represents the identity matrix; This represents the installation angle error at time q.

[0057] Beneficial effects: Compared with the prior art, the present invention has the following advantages:

[0058] (1) The Welsch kernel function is used to construct the filtering cost function, which can reduce the response of large residuals faster, suppress the influence of abnormal measurements on state estimation more effectively, and significantly improve the stability of the system in extreme environments such as non-Gaussian noise and sudden disturbances.

[0059] (2) By replacing the traditional minimum mean square error (MMSE) criterion with the Welsch kernel, a loss function with saturation characteristics is established, which makes the residual influence tend to be stable, effectively preventing extreme errors from dominating state estimation, and improving the filtering convergence efficiency and estimation accuracy.

[0060] (3) Multiple parallel sub-filters are constructed using the IMM mechanism. Each filter corresponds to a different kernel bandwidth setting. The kernel function sensitivity is adaptively adjusted by real-time updating of the model probability, taking into account both convergence speed and filtering accuracy, and avoiding the problem of overfitting or underfitting of a single kernel bandwidth. Attached Figure Description

[0061] Figure 1 This is a flowchart of the interactive multi-model-based adaptive kernel bandwidth maximum entropy Kalman filter DVL calibration method according to an embodiment of the present invention;

[0062] Figure 2 This is a theoretical schematic diagram of the DVL calibration system in this embodiment of the invention;

[0063] Figure 3 This is a schematic diagram of the simulation trajectory in an embodiment of the present invention;

[0064] Figure 4 This is a root mean square error diagram for the heading installation angle calibration in an embodiment of the present invention;

[0065] Figure 5 This is a root mean square error diagram for the pitch installation angle calibration in an embodiment of the present invention;

[0066] Figure 6 This is a root mean square error diagram for the roll mounting angle calibration in an embodiment of the present invention. Detailed Implementation

[0067] The present invention will be further illustrated below with reference to specific embodiments. These embodiments are implemented based on the technical solutions of the present invention, and it should be understood that these embodiments are only used to illustrate the present invention and are not intended to limit the scope of the present invention.

[0068] The adaptive kernel bandwidth maximum entropy Kalman filter Doppler log (DVL) calibration method based on interactive multi-model provided in this embodiment is suitable for sensor installation angle estimation during high-precision navigation of underwater vehicles, amphibious unmanned platforms, and other carriers in complex underwater environments. In actual underwater operation scenarios, DVL measurement signals are easily affected by multipath effects, turbulence, and biological disturbances, resulting in observation noise exhibiting non-Gaussian and time-varying statistical characteristics. Traditional Kalman filtering methods based on minimum mean square error show significant performance degradation in such environments, causing accumulated installation angle estimation bias and thus affecting the accuracy of integrated navigation.

[0069] To address the aforementioned issues, this method uses the installation angle error as the state vector for propagation in the state estimation stage, employing a small-angle approximation to reduce model nonlinearity and improve computational efficiency. Specifically, the position and velocity information output from the previous time-instance integrated navigation system (SINS / GPS) are used as input, combined with the original DVL velocity observations, to construct a DVL velocity error model and calculate a scaling factor to proportionally correct the DVL velocity. In the filtering update step, a cost function based on the maximum entropy criterion of the Welsch kernel function is used to construct a cost function, replacing the traditional mean square error loss, achieving adaptive suppression of non-Gaussian noise and enhancing the system's robustness under abnormal observations.

[0070] Furthermore, to address the issue of underwater noise statistical characteristics dynamically changing with carrier maneuvering, this method introduces an interactive multi-model (IMM) framework, designing a set of sub-filters with different kernel bandwidths to run in parallel. Each sub-filter corresponds to a different noise adaptation hypothesis. By calculating the likelihood probability of each model in real time and performing model probability updates and state fusion based on Bayesian inference, the equivalent kernel bandwidth is autonomously adjusted, avoiding overfitting or underfitting caused by fixed bandwidth. In actual execution, the inputs include attitude and velocity information calculated by SINS, GPS positioning data, DVL raw velocity, and noise prior parameters. During processing, state prediction, innovation calculation, adaptive kernel bandwidth selection, maximum entropy filtering update, and model probability weighted fusion are performed sequentially. The final output is the optimal installation angle error estimate, covariance matrix, and posterior probabilities of each model, thereby completing the online calibration and compensation of the DVL installation angle.

[0071] This application specifically includes the following steps:

[0072] S1. Perform pre-calibration preparations, fixing the sensors in their designated positions to ensure no relative movement between them during calibration. The mounting rod between the DVL and the inertial navigation system is measured using the three-dimensional installation position as a known input to the calibration system. Initialize the calibration system by setting the Markov transition matrices between different sub-filters of the multi-model system based on prior information. and initial model probability Complete the initialization of each sub-filter and set its initial state. Initial covariance Initialize timestamp .

[0073] S2. Collect the actual sensor data at time q and calculate the proportional error;

[0074] The data to be collected includes the raw output velocity of the DVL and the output of the SINS / GPS fusion navigation system, as well as the output of the IMU gyroscope. Based on the DVL velocity measurement model, the proportional error at the current moment is calculated using the collected data. The data collected in this step comes from the SINS / GPS integrated navigation system and DVL sensor onboard the vehicle, specifically including:

[0075] Velocity measurements of the amphibious platform in the platform coordinate system obtained from the amphibious platform's SINS / GPS fusion navigation system. The attitude transformation matrix from the vehicle coordinate system to the local navigation coordinate system ;

[0076] Velocity measurement values ​​in the DVL coordinate system directly output by DVL ;

[0077] The projection of the angular velocity of the carrier relative to the inertial coordinate system, measured by a gyroscope, onto the carrier's coordinate system. .

[0078] Based on the working principle of DVL, the proportional error of DVL is calculated using the collected data. Specifically:

[0079] Based on the working principle of DVL, its speed error model can be expressed as:

[0080]

[0081] in, Indicates the current output speed of the DVL; Indicates the proportional error of DVL; Represents the transformation matrix between DVL and SINS; Represents the coordinate transformation matrix from the vehicle system to the navigation system; This represents the true velocity value at the current DVL position; This represents the projection of the angular velocity of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle coordinate system. Indicates the lever arm of DVL and SINS; This represents the velocity error vector.

[0082] because It is an orthogonal identity matrix with a modulus of 1. Taking the norm of both sides of the formula, we get:

[0083]

[0084] When calculating the proportional error, integrating the velocity over time can effectively smooth out random noise and improve the accuracy of the calculation, thus obtaining the proportional error at the current moment, specifically:

[0085]

[0086]

[0087] in, Indicates the proportional error of DVL; Indicates the lever arm of DVL and SINS. This represents the projection of the angular velocity of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle coordinate system. This indicates the zero bias of the gyroscope; This represents the projection of the Earth's rotation speed onto the navigation system. This represents the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. This represents the estimated value of the coordinate transformation matrix from the carrier system to the navigation system; Indicates the sampling period. It represents the projection of the angular velocity of the carrier relative to the inertial coordinate system onto the carrier coordinate system.

[0088] S3. Install the angle estimation system and use a multi-model interaction algorithm framework to achieve bandwidth adaptation. Specifically, set different bandwidth kernel functions according to the intensity of the model. The bandwidth value range is (0.8, 4). The larger the value, the more intense the model. The smaller the value, the slower the convergence and the more stable the system.

[0089] Due to the kernel bandwidth The size of directly affects the number of iterations and accuracy of the algorithm. As the value increases, the number of system iterations decreases, but the accuracy decreases accordingly. At infinity, the system degenerates into a classic Kalman filter. When the core bandwidth is small, it consumes more system resources. Choosing an appropriate core bandwidth has a significant impact on improving system performance. Traditional maximum robust filtering, which uses a single core bandwidth, limits its flexibility. This invention adopts a multi-model interaction approach, setting maximum entropy filtering models with different core bandwidths to adaptively adjust the core bandwidth for the actual system.

[0090] like Figure 2 As shown, the steps of the method based on the multi-model interaction concept are as follows:

[0091] S31. Initialize the multi-model system model and set the Markov transition matrix between different sub-filters based on prior information;

[0092] Specifically:

[0093]

[0094] in, Represent the Markov transition matrix; This represents the transition probability from model j to model k. This represents the transition probability from model m1 to model n.

[0095] Set initial model probabilities , This represents the probability that the model is model k at the initial time step.

[0096] S32. Input interaction: Input the posterior state and covariance of each sub-filter at time q-1, the Markov transition matrix, and the prior probability of each model. Calculate the transition probability of the mixture normalized model from model j to model k. ;

[0097] Specifically:

[0098]

[0099]

[0100] in, Represents the normalization constant. This represents the transition probability from model j to model k in the mixed normalized model; This represents the transition probability from model j to model k; Indicates time The probability of model k; Indicates time The probability of model j.

[0101] S33. Taking one of the models k as an example, input the estimated states of each model. and the corresponding normalized transition probabilities of the mixture model Calculate its mixed estimated state;

[0102] Specifically:

[0103]

[0104] in, Indicates time After mixing, the mixed initial state estimate is prepared for model k; Indicates time State estimation of model j; This represents the transition probability from model j to model k in the mixed normalized model.

[0105] S34. Input the estimated covariance matrix of each model and the corresponding normalized transition probability of the mixture model. The mixture estimate covariance of model k is calculated as follows:

[0106]

[0107] in, Indicates time The initial covariance matrix after mixing is prepared for model k; This represents the transition probability from model j to model k in the mixed normalized model; Indicates time The posterior estimation error covariance matrix of model j; Indicates time State estimation of model j; Indicates time After mixing, the mixed initial state estimate is prepared for model k.

[0108] S4. Perform maximum entropy Kalman filter filtering on each subsystem to calculate the one-step predicted state of model k. and predicted covariance Estimate the state and estimate covariance ;

[0109] The installation angle is filtered using a maximum entropy Kalman filter; a maximum entropy robust filter for the subsystem is implemented in parallel to obtain the estimated state of the output. Estimate covariance Observe new information and new information covariance .

[0110] The classic Kalman filter algorithm uses the MMSE (Multi-Modal Matrix Sequence) to construct the cost function. Due to the lack of higher-order moment information, its accuracy drops significantly in non-Gaussian environments. In contrast, the Gaussian kernel function, in its Taylor expansion, contains higher-order moment information, enabling the algorithm to achieve higher accuracy and making the filtering system more robust in non-Gaussian environments. This invention uses the Welsch kernel function. Instead of the Gaussian kernel function, it is more sensitive to large residuals and converges faster. The performance of maximum entropy-based algorithms is mainly affected by the kernel function and kernel bandwidth.

[0111] To obtain an update equation with higher robustness to non-Gaussian noise, this invention uses the Welsch kernel function to define a new cost function. Since the state of this system is theoretically constant, the mean squared error (MMSE) is still used to describe the difference between the state and the predicted state. The cost function constructed using the Welsch kernel function is used for the observation error. The specific cost function is as follows:

[0112]

[0113]

[0114] Where J represents the cost function; This represents the state vector to be estimated; This indicates a predicted state estimate; Represents the prediction error covariance matrix The inverse matrix; k1 represents the weighting coefficient of the observation error term; Indicates time The observation vector; Represents the measurement matrix; The matrix representing the inverse of the measurement noise matrix; is the Mahalanobis distance of vector x; A represents the inverse of the covariance matrix of vector x; This represents the transpose of vector x.

[0115] The posterior estimated state vector then satisfies the following equation:

[0116]

[0117] in, J represents the optimal estimated solution; J represents the cost function. This represents the state vector to be estimated.

[0118] The update process of the estimated state is obtained by solving for the extrema of the cost function. Taking the derivative of the cost function yields:

[0119]

[0120] in:

[0121]

[0122] make ,get:

[0123]

[0124] extract Move the item to the left:

[0125]

[0126] Add and then subtract on the right side of the equation. :

[0127]

[0128] in, Represents the weight adjustment matrix; This indicates the transpose of the measurement matrix; The matrix representing the inverse of the measurement noise matrix; This represents the scaling parameter, whose value is the Mahalanobis distance of the innovation vector.

[0129] For cases where the original equation cannot be solved analytically, an in-situ iterative method is used to obtain an approximate solution. The initial value for the iteration is... Set iteration conditions .

[0130]

[0131]

[0132]

[0133] The corresponding error covariance matrix :

[0134]

[0135] in, Represents the iterative gain of the maximum correlation entropy; Represents the identity matrix. This represents the measurement noise matrix.

[0136] Maximum entropy Kalman filtering introduces a weight B to affect the gain. Adjustments are made to the gain when noise is high. This reduces noise, thereby enabling the system to adaptively adjust to noise under non-Gaussian noise conditions and increasing the system's robustness.

[0137] The specific sub-steps of S4 are as follows:

[0138] S41. Initialize the filtering system;

[0139] Define system state The three dimensions of the installation angles for DVL and SINS are: It is the installation error angle in the x-axis direction. It is the installation error angle in the y-axis direction. It is the installation error angle in the z-axis direction; its rotation matrix representation is C. Set the initial state values. .

[0140] S42. Define the system's installation angle error. The difference between the theoretical installation angle and the actual error angle is given by . It is the installation error angle in the x-axis direction. It is the installation error angle in the y-axis direction. It is the installation error angle in the z-axis direction;

[0141] definition (in, Represents the coordinate transformation matrix from the vehicle system to the navigation system; Represents the error attitude matrix; (This represents the attitude transformation matrix from the vehicle coordinate system to the local navigation coordinate system. Since the error angle can be considered a small angle...) ;in, This represents the rotation matrix for estimating the installation angle at time q; This represents the rotation matrix for estimating the installation angle at time q-1; Represents the identity matrix; This represents the installation angle error at time q; Representing vectors The antisymmetric matrix can be obtained by the following formula:

[0142] .

[0143] S43. Construct a system observation error filtering model;

[0144] The observation error propagation equation is constructed using the proportional error-corrected DVL velocity and the SINS / GPS velocity.

[0145] Define two vectors , ,in, The DVL velocity is calculated from the true value of the carrier velocity. This is the actual measured speed of DVL.

[0146]

[0147]

[0148] in, Indicates the proportional error of DVL; This represents the estimated value of the coordinate transformation matrix from the carrier system to the navigation system; This indicates the actual speed of the vehicle under the navigation system; This represents the projection of the angular velocity of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle coordinate system. Indicates the lever arm of DVL and SINS; This indicates the current raw output speed of the DVL.

[0149] The observation error filtering model of the system can be expressed as follows:

[0150]

[0151]

[0152]

[0153]

[0154] The error propagation equation of the system can be obtained as follows:

[0155]

[0156] in, It is the installation angle error at time q; Represents the observation error vector of the system; This represents the rotation matrix representing the actual mounting angle at time q. This represents the rotation matrix for estimating the installation angle at time q; Let q be the inverse matrix of the matrix used to estimate the installation angle rotation at time q; Represents the identity matrix; Represents the velocity error vector; This represents the installation angle error at time q-1; This indicates measurement noise.

[0157] S44. Perform predictive updates on the system;

[0158] Since the system state is a fixed value, the system state can be predicted in one step. And error covariance matrix It can be given by the following formula:

[0159]

[0160] in, Indicates at time q, based on until Prediction error angle estimation for time information; Represents the state transition matrix; The inverse matrix representing the state transition matrix; Indicates time Posterior state estimation; This represents the posterior estimation error covariance matrix; Represents the prediction error covariance matrix; Let represent the noise covariance matrix of process i in model i.

[0161] S45. Calculate the relevant entropy;

[0162] Correlation entropy is a measure of the similarity between two random vectors. Its joint probability density function is Then its related entropy can be obtained by the following formula:

[0163]

[0164] in, Let V be the mathematical expectation, and let V represent the correlation entropy, which characterizes the similarity between two random vectors. For kernel functions, the Gaussian kernel function is generally used. .in For residuals, For core bandwidth.

[0165] S46. Calculating New Information and new information covariance Specifically:

[0166]

[0167]

[0168] in, Indicates new information; Represents the observation vector; Represents the measurement matrix; This indicates a predicted state estimate; Represent the new information covariance matrix; Represents the prediction error covariance matrix; This represents the inverse of the weight adjustment matrix; This represents the measurement noise matrix. This represents the transpose of the measurement matrix.

[0169] S5. Calculate the model likelihood probability and update the model probability;

[0170] Calculate the likelihood probability of the model Specifically:

[0171]

[0172] in, Represent the new information covariance matrix; Represents the innovation vector; This represents the transpose of the innovation vector; denoted as the inverse matrix of the innovation covariance matrix; m represents the dimension of the multivariate normal distribution, i.e., the dimension of the innovation vector.

[0173] Update model probability The calculation formula is:

[0174]

[0175] in, Represents the model likelihood probability; represents the normalization constant; l represents the total number of mixed models.

[0176] S6. Interactive output: Calculate the output estimate of the normalized model. and the estimated error covariance matrix ;

[0177]

[0178]

[0179] in, This represents the mixture posterior estimate at time i; This represents the posterior state estimate of model k at time i; Let represent the posterior probability of model k at time i; l represents the total number of mixture models. This represents the posterior estimation error covariance matrix after final fusion; Let represent the posterior estimation error covariance matrix of model k at time i; T represents the transpose operation.

[0180] This paper utilizes an interactive multi-model framework to achieve kernel bandwidth adaptation for a maximum entropy Kalman filter. When a filter with a specific kernel bandwidth matches the real-world system, its model probability increases under the adjustment of the likelihood function value; conversely, the model probability decreases. Finally, the interactive output state obtained through weighted fusion of model probabilities is used as the final output state. By adaptively changing the kernel bandwidth within a certain range through model interaction, this approach addresses the problem that a single-kernel-bandwidth maximum entropy filter struggles to handle variable non-Gaussian noise environments, and avoids overfitting or underfitting issues associated with single-kernel-bandwidth filters.

[0181] S7. Correct the system state using the estimated state error;

[0182]

[0183] in, This represents the rotation matrix for estimating the installation angle at time q; This represents the rotation matrix for estimating the installation angle at time q-1; Represents the identity matrix; This represents the installation angle error at time q.

[0184] The system state is compensated using error states, and the error is cleared to zero. Waiting for the data to be filtered again.

[0185] If k is less than the set maximum number of iterations K, k = k + 1, return to step S2; otherwise, output the estimated installation angle rotation matrix. The calibration is now complete.

[0186] This invention proposes a Doppler velocities (DVL) calibration method based on interactive multi-model and Welsch kernel function maximum entropy Kalman filtering (Welsch-MCKF). This method addresses issues such as non-Gaussian distribution of measurement errors and outlier interference in non-ideal underwater environments, significantly improving the robustness and adaptability of the calibration algorithm. For non-Gaussian noise, a Kalman filter based on maximum correlation entropy is used to enhance the algorithm's robustness. Simultaneously, the multi-model interaction approach adaptively adjusts the kernel bandwidth, improving the system's convergence speed and stability.

[0187] In this embodiment, the specific experimental procedure is as follows:

[0188] The trajectory is set to a total motion time of 3600 seconds, with the initial position set at the initial latitude. initial longitude The IMU's sampling frequency is 200Hz, and the gyroscope's zero bias is... The random walk coefficient of a gyroscope is ; Accelerometer zero bias is The random walk coefficient is Set the DVL scaling factor to 0.4, the pole arm length between DVL and SINS / GPS to [0, 2, 0] m; the mounting angle to [31°, 3°, 5°], and the initial mounting angle error to [0.01°, 0.01°, 0.02°].

[0189] This invention uses an interactive model with three sub-filters, each with a different kernel bandwidth. Model transition probability Set to:

[0190]

[0191] The initial model probability is set to The simulated trajectory is in the shape of an "8", such as... Figure 2 As shown. The noise distribution uses time-varying non-Gaussian mixed noise, i.e.:

[0192]

[0193]

[0194] Where N represents a normal distribution; Represents the noise covariance matrix; t represents the total running time; t represents the current running time.

[0195] Figure 4 , Figure 5 and Figure 6 The figures represent the root mean square errors of the heading, pitch, and roll installation deviation angles in 50 Monte Carlo simulations using different algorithms. IMM-Welsch-MCKF is the algorithm used in this invention. As can be seen from the figures, the non-robust classical Kalman filter completely lacks the ability to adaptively handle noise in non-Gaussian noise environments, and its calibration error is significantly larger compared to the other two robust algorithms (IEKF (Iterative Extended Kalman Filter) and MCKF (Maximum Correlation Entropy Criterion Kalman Filter)). This algorithm (IMM-Welsch-MCKF) has a significantly smaller root mean square error, is more stable, and exhibits higher stability compared to the Gaussian kernel maximum entropy Kalman filter without kernel bandwidth adaptation.

[0196] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for DVL calibration of an adaptive kernel bandwidth maximum entropy Kalman filter based on interactive multi-model, characterized in that: Includes the following steps: S1. Perform calibration system initialization. Based on prior information, set the Markov transition matrix and initial model probability between different sub-filters of the multi-model system to complete the initialization of each sub-filter. S2. Collect the actual sensor data at time q, and calculate the proportional error at the current time using the collected data according to the working principle of DVL. S3. Install an angle estimation system and use a multi-model interaction algorithm framework to achieve bandwidth adaptation; S4. Use maximum entropy Kalman filtering to filter the installation angle; S5. Calculate the model likelihood probability and update the model probability; S6. Interactive output: Calculate the output estimate of the normalized model. And estimate the covariance matrix ; S7. Correct the system state using the estimated state error.

2. The adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model as described in claim 1, characterized in that: In S1, the specific implementation details are as follows: The collected data includes the raw output velocity of DVL, the output of the SINS / GPS fusion navigation system, and the output of the IMU gyroscope. Based on the DVL velocity measurement model, the collected data is used to calculate the proportional error at the current moment. Based on the working principle of DVL, the speed error model is expressed as: ， in, Indicates the current output speed of the DVL; Indicates the proportional error of DVL; Represents the transformation matrix between DVL and SINS; Represents the coordinate transformation matrix from the vehicle system to the navigation system; This represents the true velocity value at the current DVL position; This represents the projection of the angular velocity of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle coordinate system. Indicates the lever arm of DVL and SINS; Represents the velocity error vector; When calculating the proportional error, the velocity is integrated over time to effectively smooth out random noise, yielding the proportional error at the current moment, specifically: ， ， in, Indicates the proportional error of DVL; Indicates the lever arm of DVL and SINS. This represents the projection of the angular velocity of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle coordinate system. This indicates the zero bias of the gyroscope; This represents the projection of the Earth's rotation speed onto the navigation system. This represents the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. This represents the estimated value of the coordinate transformation matrix from the carrier system to the navigation system; Indicates the sampling period; It represents the projection of the angular velocity of the carrier relative to the inertial coordinate system onto the carrier coordinate system.

3. The adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model as described in claim 1, characterized in that: In S3, the specific implementation details are as follows: S31. Initialize the multi-model system model and set the Markov transition matrix between different sub-filters based on prior information; S32. Input interaction: Input the posterior state and covariance of each sub-filter at time q-1, the Markov transition matrix, and the prior probability of each model, and calculate the transition probability of the mixed normalized model from model j to model k. S33. Input the estimated states of each model. and the corresponding normalized transition probabilities of the mixture model Calculate the mixed estimated state of model k; S34. Input the estimated covariance matrix of each model and the corresponding normalized transition probability of the mixture model. Calculate the mixture estimate covariance of model k.

4. The adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model as described in claim 3, characterized in that: In S32, the specific implementation details are as follows: ， ， in, Represents the normalization constant. This represents the transition probability from model j to model k in the mixed normalized model; This represents the transition probability from model j to model k; Indicates time The probability of model k; Indicates time The probability of model j.

5. The adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model as described in claim 3, characterized in that: In S33, the specific implementation details are as follows: ， in, Indicates time After mixing, the mixed initial state estimate is prepared for model k; Indicates time State estimation of model j; This represents the transition probability from model j to model k in the mixed normalized model.

6. The adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model as described in claim 3, characterized in that: In S34, the specific implementation details are as follows: ； in, Indicates time The initial covariance matrix after mixing is prepared for model k; This represents the transition probability from model j to model k in the mixed normalized model; Indicates time The posterior estimation error covariance matrix of model j; Indicates time State estimation of model j; Indicates time After mixing, the mixed initial state estimate is prepared for model k; T represents the transpose operation.

7. The adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model as described in claim 1, characterized in that: In S4, the specific implementation process is as follows: S41. Initialize the filter system; S42. Define the system's installation angle error. This is the difference between the theoretical installation angle and the actual error angle. S43. Construct a system observation error filtering model; S44. Perform predictive updates on the system; S45. Calculate the relevant entropy; S46. Calculating New Information and new information covariance .

8. The adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model as described in claim 1, characterized in that: In S5, the specific implementation process is as follows: Calculate the likelihood probability of the model Specifically: ； in, Represent the new information covariance matrix; Represents the innovation vector; This represents the transpose of the innovation vector; represents the inverse matrix of the information covariance matrix; m represents the dimension of the multivariate normal distribution; Update model probability The calculation formula is: ， in, Represents the model likelihood probability; represents the normalization constant; l represents the total number of mixed models.

9. The adaptive kernel bandwidth maximum entropy Kalman filter (DVL) calibration method based on interactive multi-model as described in claim 1, characterized in that: In S7, the specific implementation process is as follows: , in, This represents the rotation matrix for estimating the installation angle at time q; This represents the rotation matrix for estimating the installation angle at time q-1; Represents the identity matrix; This represents the installation angle error at time q.

Images