Unscented Kalman filtering ship integrated navigation method based on variational Bayesian principle

Abstract

Aiming at the problem of low filtering precision caused by noise time variation and outlier interference in ship navigation, the invention provides an unscented Kalman filtering ship integrated navigation method based on a variational Bayesian principle. According to the method, a 16-dimensional ship nonlinear motion model is constructed, student t distribution modeling is innovatively adopted to measure noise so as to enhance robustness, and noise covariance and auxiliary variables are modeled as inverse Gamma distribution and Gamma distribution respectively. By introducing variational Bayesian iterative optimization in UKF updating, state posteriori distribution and noise parameters are alternately updated until convergence, and real-time adaptive estimation of process and measurement noise covariance and outlier weight is realized. According to the method, filtering divergence can be effectively suppressed, and the estimation precision and robustness of the attitude, position and speed of the ship under the complex sea condition are remarkably improved.

Description

Technical Field

[0001] This invention relates to the field of ship navigation and signal processing, specifically to a ship integrated navigation method based on variational Bayesian principles and unscented Kalman filtering. Background Technology

[0002] With the implementation of the maritime power strategy, higher requirements have been placed on the accuracy and reliability of ship navigation systems. Ship integrated navigation systems typically consist of a Global Navigation Satellite System (GNSS) and an Inertial Measurement Unit (IMU), fusing information from both using Kalman filtering technology. Due to the highly nonlinear nature of ship motion equations, unscented Kalman filtering (UKF) is widely used because it requires no linearization and offers high estimation accuracy.

[0003] However, traditional UKF algorithms typically assume that the process noise covariance (Q) and measurement noise covariance (R) are constant, known quantities. In actual navigation, the marine environment in which ships operate is complex and variable, affected by factors such as wind and wave disturbances, hull vibrations, satellite signal blockage, and multipath effects. Consequently, the noise statistical characteristics of sensors exhibit significant time-varying characteristics. Using fixed noise parameters can lead to a decrease in filter estimation accuracy, and in severe cases, even filter divergence. Existing adaptive filtering methods (such as Sage-Husa filtering) exhibit poor stability when handling high-dimensional nonlinear systems. Therefore, how to accurately estimate time-varying noise parameters online is a key issue in improving the performance of ship integrated navigation. Summary of the Invention

[0004] To address the aforementioned shortcomings, this invention provides an unscented Kalman filter-based ship integrated navigation method based on variational Bayesian principles. This invention introduces variational Bayesian inference, utilizes the inverse Gamma distribution to model the noise variance, assumes that each noise component is independent, and achieves joint estimation of the state vector, process noise covariance, and measurement noise covariance through iterative optimization of the variational lower bound (ELBO), thereby improving the accuracy and robustness of ship navigation under complex sea conditions.

[0005] The present invention adopts the following technical solution:

[0006] Step 1: Multi-source navigation data acquisition

[0007] Acquire and synchronize observation data from multiple sources of the ship's integrated navigation system; the multiple sources of the sensors include at least one inertial measurement unit (IMU) and one BeiDou Navigation Satellite System (GNSS) receiver; the collected observation data covers complete information describing the ship's dynamics, specifically including the three-axis angular velocity and three-axis specific force (acceleration) output by the IMU, and the ship's velocity in the East / North / Sky coordinate system, longitude, latitude and altitude in the geodetic coordinate system provided by the BeiDou system;

[0008] Step 2: High-dimensional nonlinear state-space modeling

[0009] A nonlinear state-space model capable of comprehensively describing the motion characteristics of a ship is established. It defines a state vector x (16-dimensional) containing sixteen key state variables, including: quaternions describing the ship's three-dimensional attitude; three-axis zero-bias of the IMU gyroscope and accelerometer; the ship's longitude, latitude, and altitude (3-dimensional); and the ship's eastward, northward, and azimuth velocities (3-dimensional). The dynamic evolution of this model is described by a nonlinear state transition function. Specifically, the expression for the ship's nonlinear motion state model is:

[0010] x k =f(x) k-1 ,u k-1 )+w k-1 (1)

[0011] In the formula: x k Let u be the state vector at time k, f(·) be the state transition function, and u be the state vector at time k. k-1 For the sensor input data at time k-1, w k-1 The process noise (follows a Gaussian distribution with mean 0 and covariance Q).

[0012] Specifically, to address observational anomalies under complex sea conditions, the measurement noise in the measurement equation is modeled as following a Student's t-distribution. A hierarchical Gaussian model is then used to characterize it as a conditional Gaussian distribution conditioned on the auxiliary variable λ, thus enabling adaptive description of non-Gaussian noise with heavy-tailed characteristics. Specifically, assuming the measurement noise v... k It follows a student t-distribution and is characterized by an auxiliary variable λ. k A conditional Gaussian distribution with respect to: v k ~N(0,R k / λ k ), where λ k It follows a Gamma distribution;

[0013] Step 3: Construct a robust adaptive variational Bayesian unscented Kalman filter

[0014] Design and implement a robust adaptive unscented Kalman filter based on the variational Bayesian (VB) principle to estimate the process noise covariance Q and measurement noise covariance R of the state model in step 2 online, as well as the auxiliary variable λ. The core of this approach lies in jointly estimating the system state, the noise covariance matrix, and the auxiliary variable representing the weights of outliers. Specifically, this includes:

[0015] Step 3.1: Probabilistic Model Initialization: Set initial probabilistic assumptions for the algorithm; assume that the posterior probability of the system state follows a Gaussian distribution; model the measurement noise as a Gaussian-inverse gamma mixture distribution (or Gaussian-inverse Wieshardt mixture distribution), where the diagonal elements of the measurement noise covariance matrix R follow an inverse gamma distribution, and the introduced auxiliary variable λ follows a gamma distribution; this distribution choice provides mathematical convenience for subsequent Bayesian inference. Specifically, initialize the posterior probability distribution of VB, assuming state x k The noise follows a Gaussian distribution, and the process noise covariance Q and measurement noise covariance R are assumed to be... k All are diagonal matrices, and their diagonal elements are independent and follow an inverse Gamma distribution; assume the auxiliary variable λ. k It follows a Gamma distribution;

[0016] Step 3.2: State Prediction: Using the standard unscented Kalman filter Sigma point sampling strategy, a set of deterministic sampling points that can accurately capture the mean and covariance of the current state are generated. These sampling points are propagated through the nonlinear state transition function defined in Step 2 to obtain the preliminary predicted value of the state at the next time step and its covariance. Specifically, 2n+1 Sigma points (n is the state dimension of 16) are generated using the unscented Kalman filter Sigma point sampling strategy to predict the state model in Step 2, obtaining the predicted state at time k. And the predicted covariance P k|k-1 ;

[0017] Step 3.3: Variational Iterative Optimization: Initiate an internal iterative loop to collaboratively optimize the state estimate, noise covariance estimate, and auxiliary variable estimate. In each iteration, update the system state using the current noise parameter and the expectation of the auxiliary variable. Subsequently, based on the updated state residuals, update the gamma distribution parameters of the auxiliary variable to identify the measurement field value. Finally, update the inverse gamma distribution parameters of the noise covariance using the weighted residual information. This process continues until the "lower bound of evidence (ELBO)" converges, indicating that the optimal and most self-consistent joint estimate of the state and noise at the current moment has been found. Specifically: Construct a variational iterative loop, and in each iteration, use the current expectation of the auxiliary variable E[λ]... k Correcting the measurement noise covariance using the currently estimated Q k and R k The posterior distribution of the updated state is then used to update the shape and scale parameters of the inverse Gamma distribution, as well as the parameters of the Gamma distribution itself, to identify outliers, thereby obtaining a new Q. k R k and λ k Estimate the value until the variational lower bound (ELBO) converges;

[0018] Step 4: Optimal State Estimation and Output

[0019] The multi-source sensor observation data acquired in real time in step 1 is input into the adaptive filter optimized in step 3. Utilizing the noise covariance matrix Q and R, which best match the current environment and is finally converged in the variational iteration loop, along with the correction effect of the auxiliary variable λ, the final measurement update step is executed. This measurement update step optimally fuses the IMU prediction results with the GNSS observation results to calculate the most accurate state estimate and its covariance at the current moment. Finally, the ship's real-time navigation parameters, including attitude angles (roll, pitch, heading), geographical location (latitude, longitude, and altitude), and three-dimensional velocity, are parsed from the optimal state vector and output. Specifically: the multi-source sensor observation data from step 1 is input into the improved unscented Kalman filter, combined with the Q determined after iteration convergence in step 3... k and R k Perform the final measurement update to obtain the optimal state estimate at time k. and optimal covariance P k It outputs real-time navigation parameters for the ship (attitude angle, position, speed).

[0020] As a further improvement of the present invention, in step 3.3, the core update mechanism of variational iterative optimization is specifically manifested as follows:

[0021] For each diagonal element of the measurement noise covariance matrix R, an inverse gamma probability model defined by shape and scale parameters is constructed. A joint posterior distribution approximation using Gaussian-Inverse Gamma-Gamma is employed, and the model is updated alternately in each variational iteration using the following three steps:

[0022] Step 1: State Update: Calculate the corrected measurement noise covariance matrix It is obtained by dividing the expected value of the current measurement noise covariance by the expected value of the auxiliary variable λ; using The Kalman gain is calculated to correct the ship's state vector and covariance matrix; a small expected value for the auxiliary variable indicates that outliers may exist in the current measurement. The value increases, thereby automatically reducing the weight of the measurement in state updates;

[0023] Step 2: Auxiliary variable update: Using the state updated in Step 1, calculate the squared Mahalanobis distance of the measurement residuals; use the Mahalanobis distance to update the shape and scale parameters of the gamma distribution followed by the auxiliary variable λ, and calculate its new expected value; this step is used to quantify the degree to which the current observation deviates from the predicted value, thereby identifying outliers;

[0024] Step 3: Noise Covariance Update: The second moment of the measurement residual is weighted using the expected value of the auxiliary variable λ, and the parameters of the inverse gamma distribution followed by the measurement noise covariance matrix R are updated using the weighted statistics, thereby obtaining the new expected value of the R matrix.

[0025] For the process noise covariance matrix Q, the algorithm maintains the inverse gamma distribution modeling and the update logic based on the state prediction error.

[0026] Specifically, the update process of the variational iterative loop is as follows:

[0027] Let the measurement noise covariance matrix R k The i-th diagonal element is R k,i It follows an inverse Gamma distribution IG(R) k,i ;α k,i ;β k,i Let the auxiliary variable λ be... k G(λ) follows a Gamma distribution k ;a k b k )

[0028] In the m-th variational iteration, update as follows:

[0029] (1) Update state estimate: Based on the expected value of the measurement noise covariance E[R] obtained in the (m-1)th iteration. k ] (m-1) and the expected value of auxiliary variables E[λ k ] (m-1) Calculate the corrected effective measurement noise covariance. Calculate the Kalman gain K k (m) and update the state estimate. and posterior covariance P k (m) ;

[0030] (2) Update the inverse Gamma distribution parameters: Using the updated state statistics, calculate the second moment of the measurement residuals and update the shape parameter α. k,i (m) and scale parameter β k,i (m) :

[0031] α k,i (m) =α k-1,i +0.5(2)

[0032] β k,i (m) =β k-1,i +0.5*E[λ k ](m-1) *E[(y k,i -h(x k ) i ) 2 ] (m) (3)

[0033] In the formula, E[(y k,i -h(x k ) i ) 2 ] (m) The expectation of the squared residual of the i-th dimension measurement is estimated from the state of the current iteration. Covariance P k (m) Calculated using Sigma points; here, E[λ] is introduced. k The residuals are weighted to reduce the impact of outliers on covariance estimation;

[0034] (3) Update the noise covariance expectation: Based on the properties of the inverse Gamma distribution, calculate the estimated measurement noise covariance value for the m-th iteration:

[0035] E[R k,i ] (m) =β k,i (m) / α k,i (m) (4)

[0036] (4) Update Gamma distribution parameters (outlier identification): Update the auxiliary variable λ using the squared Mahalanobis distance of the current residuals. k Gamma distribution parameters and And calculate its expectation:

[0037]

[0038] In the formula, ν represents the degrees of freedom parameter of the student t-distribution, and m z To measure the dimension, tr(·) denotes the trace of the matrix; when the observation residuals are large, E[λ] k This reduces the equivalent observation noise, thereby suppressing outliers.

[0039] For the process noise covariance Q k Using R k The same inverse Gamma distribution modeling and parameter update method is used, and the state one-step prediction error is used for iterative updates.

[0040] As a further improvement of the present invention, in step 2, the physical and mathematical model inside the state transition function specifically includes:

[0041] The ship's attitude is represented by a quaternion, which is updated based on the attitude at the previous moment and the three-axis angular velocities measured by the IMU, and is obtained by integration through nonlinear quaternion kinematic equations.

[0042] The zero bias of the IMU's gyroscope and accelerometer is modeled as a slowly changing stochastic process. Therefore, in single-step prediction, its value remains constant, waiting for subsequent measurement information to make corrections.

[0043] The ship's geographical location, including longitude, latitude, and altitude, is updated based on the previous position and the current estimated speed, and is calculated by strictly following the Earth ellipsoid model of the World Geodetic Coordinate System 1984 (WGS84) to ensure high-precision geographic positioning.

[0044] The ship's speed, represented in the East / North / Sky navigation coordinate system, is updated based on the speed at the previous moment and the three-axis acceleration measured by the IMU. The update process requires first subtracting the influence of gravity components from the acceleration measurements, then using the currently estimated attitude (through a rotation matrix) to transform the pure motion acceleration in the carrier coordinate system to the navigation coordinate system, and finally performing time integration.

[0045] Specifically, the expression for the state transition function f(·) is as follows:

[0046] f(x k-1 ,u k-1 )=[q k bg k ;ba k lat k lon k ;alt k V k ] T (8)

[0047] In the formula: q k The quaternion at time k (updated by quaternion multiplication of the quaternion at time k-1 and the IMU angular velocity), bg k The gyroscope bias is zero at time k (assuming the bias changes slowly, bg) k =bg k-1 ,ba k The accelerometer bias is zero at time k (similarly to ba). k =ba k-1 ),lat k lon k alt k V represents the latitude, longitude, and altitude at time k (updated from the position and velocity at time k-1 using the WGS84 ellipsoid model). k The velocity at time k is updated by transforming the velocity at time k-1 and the IMU acceleration through a rotation matrix.

[0048] As a further improvement of the present invention, in step 3.3, the variational lower bound (ELBO) is used as a convergence criterion to determine whether the variational iteration loop has reached the optimal solution. Mathematically, ELBO provides a measure of the similarity between the current variational approximation distribution and the true posterior probability distribution. In each iteration, the algorithm calculates the current ELBO value. The goal of the iteration process is to continuously maximize this ELBO value. When the ELBO value no longer increases significantly, that is, when the increment between two iterations is less than a preset small threshold, the algorithm considers that the current approximate distribution is close enough to the true distribution, the iteration process converges, and thus the optimal joint estimate of the state and noise covariance at the current moment is found.

[0049] Specifically, the expression for calculating the variational lower bound ELBO is:

[0050] ELBO = E[logp(y 1:k |x 1:k ,R,λ)+logp(x 1:k |Q)+logp(Q)+logp(R)+logp(λ)]-E[logq(x 1:k ,Q,R,λ)](9)

[0051] In the formula: y 1:k Given measurement data from time 1 to k, p(·) is the true probability distribution, q(·) is the variational approximation distribution, and E[·] represents the expectation; the variational approximation distribution is assumed to be decomposed into q(x 1:k ,Q,R,λ)≈q(x 1:k )q(Q)q(R)q(λ)

[0052] As a further improvement of the present invention, in step 4, the final measurement update process is implemented using the standard update procedure of unscented Kalman filtering: First, the predicted state Sigma point is mapped to the measurement space through a nonlinear observation function, and the predicted measurement value, measurement covariance, and cross-covariance are calculated; then, the corrected measurement noise covariance obtained after the iteration convergence in step 3.3 (i.e., the equivalent covariance considering the tail characteristics of the Student's t-distribution) is used to replace the fixed covariance in the standard Kalman filter, and the optimal Kalman gain is calculated; finally, the predicted state is corrected using the optimal Kalman gain and the actual measurement residual. This process ensures that when the GNSS signal is interfered with or experiences abrupt errors, the filter can automatically reduce the observation confidence level, maintaining the smoothness and accuracy of the navigation solution. Specifically, the predicted state is updated using the observation function h(·). Mapped to predictive measurement Calculate measurement residuals Based on the R updated in step 3.3 k Calculate the Kalman gain Kk =P k|k-1 H k T (H k P k|k-1 T +R k ) -1 (H k (For the observation matrix), the final state is updated. covariance P k =(IK k H k )P k|k-1 (I is the identity matrix).

[0053] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0054] 1. Significantly Enhanced Robustness to Outliers: This invention innovatively introduces the Student's t-distribution instead of the traditional Gaussian distribution to model measurement noise. Utilizing the "fat-tailed" characteristic of the Student's t-distribution, combined with auxiliary variables introduced by the hierarchical Gaussian model, the algorithm can automatically identify and "include" outliers caused by multipath effects, sensor malfunctions, or severe sea conditions. When excessively large measurement residuals are detected, the algorithm automatically expands the equivalent measurement noise covariance through auxiliary variables, reducing the weight of the outlier in the filter update, thereby effectively avoiding filter divergence caused by outliers and significantly improving the system's anti-interference capability. 2. Adaptive Joint Estimation of Noise Statistical Characteristics: Through a variational Bayesian inference framework, this invention can not only estimate the state but also simultaneously estimate the process noise covariance, measurement noise covariance, and auxiliary variables characterizing the degree of outliers online. This joint estimation mechanism enables the algorithm to respond quickly to sudden changes in noise statistics caused by drastic changes in sea state (such as moving from calm water to a windy and wavery area), solving the problem of decreased accuracy caused by fixed noise parameters in traditional Kalman filtering.

[0055] 2. Maintains low computational complexity and is easy to implement in engineering: Although it introduces a complex Student's t-distribution and variational iteration mechanism, this invention successfully transforms the complex matrix inversion and high-dimensional integration problems into a series of simple diagonal element-wise algebraic operations by selecting conjugate priors (inverse Gamma distribution and Gamma distribution) and assuming that the noise components are independent of each other. This allows the algorithm to achieve advanced robustness without significantly increasing the computational burden, making it very suitable for real-time operation on resource-constrained shipborne embedded computing platforms.

[0056] 3. Improved overall navigation performance in adverse sea conditions: Combining the above advantages, the algorithm proposed in this invention not only ensures high-precision positioning under normal sea conditions, but also enhances survivability and attitude determination capabilities under adverse sea conditions, providing more reliable navigation data support for the safe navigation and precise control of ships. Attached Figure Description

[0057] Figure 1 This is a flowchart of an unscented Kalman filter-based ship integrated navigation method based on variational Bayesian principles, according to the present invention.

[0058] Figure 2 This is a schematic diagram illustrating the principle of the variational Bayesian iterative update mechanism in this invention.

[0059] Figure 3 The results of the combined navigation and positioning experiment of the filtering algorithm of the present invention in scenario one are shown.

[0060] Figure 4 The results of the combined navigation and positioning experiment of the filtering algorithm of the present invention in scenario two are shown. Detailed Implementation

[0061] The following detailed description provides a wealth of specific and in-depth information to help readers form a comprehensive and thorough understanding of the technical solutions of this invention. However, those skilled in the art should recognize that even without some of these details, this invention can still be implemented without relying on them, and this will not affect the core ideas and practical applications of this invention.

[0062] To fully and clearly explain the technical principles and implementation methods of this invention, the following content will gradually unfold, detailing the key links and operational steps in the algorithm flow one by one. Preferred embodiments of this invention have been described in detail through specific implementation methods for reference and understanding. However, in addition to these explicitly described implementation methods, this invention can also adopt various different implementation forms and variations according to the needs of actual application scenarios.

[0063] Reference Figure 1 and Figure 2 As shown, this invention proposes a robust unscented Kalman filter-based ship integrated navigation method based on variational Bayesian principles. This method possesses strong adaptability and robustness, and can effectively handle navigation tasks under complex sea conditions. The specific implementation process of this algorithm includes the following key steps:

[0064] Step (1): Acquire multi-source sensor observation data of the ship's integrated navigation system.

[0065] Specifically, this includes the angular velocity vector ω output by the inertial measurement unit (IMU). kand the specific force (i.e., acceleration) vector f k It also includes position coordinate information provided by the Global Navigation Satellite System (GNSS). gnss,k =[lat,lon,alt] T and velocity vector V gnss,k =[V E V N V U ] T These raw data constitute the basic information source for the algorithm's input.

[0066] Step (2): Establish a nonlinear motion state model of the ship.

[0067] In this invention, the state vector x is designed to be 16-dimensional, and its specific composition structure is defined as follows:

[0068] xk = [q k T ,b g,k T ,b a,k T ,p k T V k T ] T (Formula 1)

[0069] Where, q k Let b represent a 4-dimensional attitude quaternion. g,k b is the zero bias vector of the 3D gyroscope. a,k p is the zero bias vector of the 3D accelerometer. k V represents 3D location information (including latitude, longitude, and elevation). k This represents 3D velocity information (eastward, northward, and upward).

[0070] Furthermore, the system state equations and measurement equations in discrete-time form are established as follows:

[0071] x k =f(x) k-1 ,u k-1 )+w k-1 (Formula 2)

[0072] y k =h(x k )+v k (Formula 3)

[0073] Where f(·) represents the nonlinear state transition function calculated based on inertial navigation, and h(·) is the observation function, whose main function is to extract position and velocity components from the system state and match them with the actual observation data provided by GNSS.k-1 To represent the process noise term, and specifically, to improve robustness to observed field values, this invention assumes a measurement noise term v. k It follows a student's t-distribution. Using a hierarchical Gaussian model, this distribution is characterized by the auxiliary variable λ. k The conditional Gaussian distribution is given by λ. k ~N(0,R k / λ k ), where λ k It follows a Gamma distribution.

[0074] Step (3): Construct an improved unscented Kalman filter based on the variational Bayesian (VB) principle and complete the initialization.

[0075] In a preferred embodiment of the present invention, it is assumed that the process noise covariance matrix Q k And the measurement noise covariance matrix R k All are diagonal matrices, and the elements on each diagonal are independent of each other. This means that the noise terms corresponding to different state components or observation components are uncorrelated.

[0076] Furthermore, the inverse Gamma distribution is chosen as the conjugate prior distribution of the noise variance. For R... k The j-th diagonal element R k,i (i.e., the variance σ of the j-th dimension measurement noise) R,j 2 Its prior distribution is set as follows:

[0077] p(R k,j |y 1:k-1 )=IG(r k,j ;α k|k-1 j ,β k|k-1 j ) (Formula 4)

[0078] Similarly, for Q k The i-th diagonal element Q k,i Its prior distribution is set as follows:

[0079] p(Q k,i |y 1:k-1 )=IG(Q k,j ;γ k|k-1 i ,δ k|k-1 i ) (Formula 5)

[0080] Here, α, β, γ, and δ are the shape and scale parameters of the inverse Gamma distribution, respectively, used to control the characteristics of the prior distribution. Furthermore, the auxiliary variable λ is initialized. k The prior distribution is the Gamma distribution G(λ). k ;a k|k-1 b k|k-1 ).

[0081] Step (4): Iteratively update the process noise covariance Q, the measurement noise covariance R, and the auxiliary variable λ.

[0082] This step constitutes the core of the entire algorithm. By introducing the variational Bayesian inference method, at each time k, the posterior distribution q(x) is approximated using the fixed-point iteration technique. k Q k ,R k ,λ k )≈q(x k )q(Q k )q(R k )q(λ k ).

[0083] The specific iterative process is as follows (assuming we are currently in the m-th iteration):

[0084] Step 1: Status update.

[0085] Based on the expected value of the noise parameter and the expected value of the auxiliary variable E[λ] obtained from the previous iteration k ] (m-1) Calculate the corrected effective measurement noise covariance. Perform the Unscented Kalman Filter (UKF) state update step to obtain the current state. Estimates and their covariance P k (m) matrix.

[0086] Step 2: Update the auxiliary variables.

[0087] Using the updated state, calculate the squared Mahalanobis distance of the measurement residuals, and update the auxiliary variable λ accordingly. k Gamma distribution parameter a k and b k .

[0088]

[0089] Step 3: Update the noise covariance R k .

[0090] Calculate the second-moment statistic of the measurement residuals. For the measurement noise covariance R... kEach of its diagonal elements R k,i The posterior distribution parameters are updated according to the following formula:

[0091] α k,i (m) =α k-1,i +0.5 (Formula 9)

[0092] To incorporate the robustness of the student t-distribution, the original scale parameter update formula (Formula 9) is modified by introducing weighted auxiliary variables:

[0093]

[0094] The expected term E[·] in formula (10) can be obtained by expanding the UKF posterior Sigma points and measurement residuals:

[0095]

[0096] Based on this, the noise covariance estimate (i.e., the expectation of the inverse Gamma distribution) under the current iteration step can be obtained:

[0097]

[0098] Similarly, if the algorithm simultaneously estimates the process noise Q k Then, the one-step prediction error information of the state can be used to update its inverse Gamma distribution parameters γ and δ, thereby obtaining... Q k The updated estimate.

[0099] Step (5): Update the measurements based on the updated noise parameters.

[0100] During the iteration process, the algorithm will continuously determine whether the variational lower bound (ELBO) has reached the convergence criterion, or whether the preset maximum number of iterations has been reached (e.g., N=5). After the iterations are complete, the algorithm will use the finally converged... Noise covariance estimate and Auxiliary variable values ​​are used to finally calculate the Kalman gain matrix K. k Output the final state estimation result:

[0101]

[0102] P k =P k|k-1 -K k S k K k T (Formula 14)

[0103] Among them, S k=H k P k|k-1 H k T +R k The information covariance matrix represents the statistical properties of the difference between the prediction and the observation.

[0104] This invention offers the following significant advantages: It significantly enhances the algorithm's robustness to outliers: This invention innovatively introduces the Student's t-distribution instead of the traditional Gaussian distribution to model measurement noise. Utilizing the "fat-tailed" characteristic of the Student's t-distribution, combined with auxiliary variables introduced by the hierarchical Gaussian model, the algorithm can automatically identify and "include" outliers caused by multipath effects, sensor malfunctions, or severe sea conditions. When excessively large measurement residuals are detected, the algorithm automatically expands the equivalent measurement noise covariance through auxiliary variables, reducing the weight of this outlier observation in the filter update, thereby effectively avoiding filter divergence caused by outliers and significantly improving the system's anti-interference capability. It achieves adaptive joint estimation of noise statistical characteristics: Through a variational Bayesian inference framework, this invention can not only estimate the state but also simultaneously estimate the process noise covariance, measurement noise covariance, and auxiliary variables characterizing the degree of outliers online. This joint estimation mechanism enables the algorithm to respond quickly to sudden changes in noise statistics caused by drastic changes in sea state (such as moving from calm water to a windy and wavery area), solving the problem of decreased accuracy caused by fixed noise parameters in traditional Kalman filtering.

[0105] Maintaining low computational complexity and ease of engineering implementation: Although introducing a complex Student's t-distribution and variational iteration mechanism, this invention successfully transforms complex matrix inversion and high-dimensional integration problems into a series of simple diagonal element-wise algebraic operations by selecting conjugate priors (inverse Gamma distribution and Gamma distribution) and assuming that noise components are independent of each other. This allows the algorithm to achieve advanced robustness without significantly increasing the computational burden, making it highly suitable for real-time operation on resource-constrained shipborne embedded computing platforms.

[0106] Improved overall navigation performance in adverse sea conditions: Combining the above advantages, the algorithm proposed in this invention not only ensures high-precision positioning in normal sea conditions, but also enhances survivability and attitude determination capabilities in adverse sea conditions, providing more reliable navigation data support for the safe navigation and precise control of ships.

[0107] The above description provides a detailed explanation of preferred embodiments of the present invention. It should be noted that the present invention is not limited to the specific embodiments described above, and various devices and structures not described in detail should be understood as reasonable applications of conventional techniques in the art. For example, although the inverse Gamma distribution is used to describe the diagonalized noise covariance in the embodiments, the inverse Wishart distribution can also be used to describe the noise covariance of the entire matrix, provided that computational resources permit. Although the Student's t-distribution is introduced for measurement noise in the embodiments, the same modeling concept can also be extended to process noise to address abnormal disturbances in the system dynamics model. Various conventional mathematical derivations and hardware structures not described in detail should be understood as reasonable applications of conventional techniques in the art. Any person skilled in the art can make various possible adjustments, modifications, or alterations to the technical solutions of the present invention using the methods and techniques disclosed in this description without departing from the scope of the present invention, and can also modify them into alternative embodiments with equivalent technical effects. These changes will not affect the essential technical content of the present invention. Therefore, any simple modifications, equivalent substitutions, and alterations that do not depart from the technical essence of this invention should be considered to fall within the protection scope of this invention.

Claims

1. A ship integrated navigation method based on variational Bayesian principle using unscented Kalman filtering, characterized in that, Includes the following steps: Step 1: Multi-source navigation data acquisition Acquire and synchronize observation data from multiple sources of the ship's integrated navigation system; the multiple sources of the sensors include at least one inertial measurement unit (IMU) and one BeiDou navigation satellite system (GNSS) receiver; the collected observation data covers complete information describing the ship's dynamics, specifically including the three-axis angular velocity and three-axis specific force output by the IMU, and the ship's speed in the East / North / Sky coordinate system, longitude, latitude, and altitude in the geodetic coordinate system provided by the BeiDou system; Step 2: High-dimensional nonlinear state-space modeling Establish a nonlinear state-space model that can comprehensively describe the motion characteristics of a ship; It defines a state vector containing sixteen key state variables, including: quaternions describing the ship's three-dimensional attitude; three-axis zero bias of the IMU gyroscope and three-axis zero bias of the accelerometer; the ship's longitude, latitude, and altitude; and the ship's eastward, northward, and celestial velocities; the dynamic evolution of the model is described by a nonlinear state transition function; in order to cope with observational anomalies under complex sea conditions, the measurement noise is modeled as following a Student's t-distribution, and is characterized by a hierarchical Gaussian model as a conditional Gaussian distribution with the auxiliary variable λ as a condition; Step 3: Construct a robust adaptive variational Bayesian unscented Kalman filter Design and implement a robust adaptive unscented Kalman filter based on the variational Bayesian (VB) principle. The core of this design lies in jointly estimating the system state, the noise covariance matrix, and auxiliary variables representing the weights of outliers. Specifically, this includes: Step 3.1: Probabilistic Model Initialization: Set initial probability assumptions; assume that the posterior probability of the system state follows a Gaussian distribution; model the measurement noise as a Gaussian-inverse gamma mixture distribution or a Gaussian-inverse Wissaud mixture distribution, where the diagonal elements of the measurement noise covariance matrix R follow an inverse gamma distribution, and the introduced auxiliary variable λ follows a gamma distribution. Step 3.2: State Prediction: Using the Sigma point sampling strategy of standard unscented Kalman filtering, a set of deterministic sampling points that can accurately capture the mean and covariance of the current state are generated; these sampling points are propagated through the nonlinear state transition function defined in Step 2 to obtain the preliminary predicted value of the state at the next time step and its covariance. Step 3.3: Variational Iterative Optimization: Initiate an internal iterative loop to jointly optimize the state estimate, noise covariance estimate, and auxiliary variable estimate. In each iteration, update the system state using the current noise parameters and the expected values ​​of the auxiliary variables. Subsequently, based on the updated state residuals, update the gamma distribution parameters of the auxiliary variables to identify measurement field values. Finally, update the inverse gamma distribution parameters of the noise covariance using the weighted residual information. The above process continues until the "Evidence Lower Bound ELBO" converges, indicating that the optimal and most self-consistent joint estimate of the state and noise at the current moment has been found. Step 4: Optimal State Estimation and Output The multi-source sensor observation data acquired in real time in step 1 is input into the adaptive filter optimized in step 3. The final measurement update step is then performed using the noise covariance matrices Q and R, which are ultimately converged and best match the current environment, and the correction effect of the auxiliary variable λ, determined in the variational iterative loop. This measurement update step optimally fuses the IMU's prediction results with the GNSS observation results to calculate the most accurate state estimate and its covariance at the current moment. Finally, the ship's real-time navigation parameters, including attitude angles, geographical location, and three-dimensional velocity, are parsed from the optimal state vector and output.

2. The method according to claim 1, characterized in that, In step 2, the physical and mathematical model inside the state transition function specifically includes: The ship's attitude is represented by a quaternion, which is updated based on the attitude at the previous moment and the three-axis angular velocities measured by the IMU, and is obtained by integration through nonlinear quaternion kinematic equations. The zero bias of the IMU's gyroscope and accelerometer is modeled as a slowly changing stochastic process. Therefore, in single-step prediction, its value remains constant, waiting for subsequent measurement information to make corrections. The ship's geographical location, including longitude, latitude, and altitude, is updated based on the previous position and the current estimated speed, and is calculated by strictly following the Earth ellipsoid model of the World Geodetic Coordinate System 1984 to ensure high-precision geographic positioning. The ship's speed, represented in the East / North / Sky navigation coordinate system, is updated based on the speed at the previous moment and the three-axis acceleration measured by the IMU. The update process requires first subtracting the influence of gravity components from the acceleration measurements, then using the currently estimated attitude to transform the pure motion acceleration in the carrier coordinate system to the navigation coordinate system, and finally performing time integration.

3. The method according to claim 1, characterized in that, In step 3.3, the core update mechanism of variational iterative optimization is specifically manifested as follows: For each diagonal element of the measurement noise covariance matrix R, an inverse gamma probability model defined by shape and scale parameters is constructed. A joint posterior distribution approximation using Gaussian-inverse gamma-gamma is employed, and the model is updated alternately in each variational iteration using the following three steps: Step 1: State Update: Calculate the corrected measurement noise covariance matrix It is obtained by dividing the expected value of the current measurement noise covariance by the expected value of the auxiliary variable λ; using Calculate the Kalman gain to correct the ship's state vector and covariance matrix; When the expected value of the auxiliary variable is small, it indicates that there may be outliers in the current measurement. Get bigger; Step 2: Auxiliary variable update: Using the state updated in Step 1, calculate the squared Mahalanobis distance of the measurement residuals; use the Mahalanobis distance to update the shape and scale parameters of the gamma distribution followed by the auxiliary variable λ, and calculate its new expected value; Step 3: Noise Covariance Update: The second moment of the measurement residual is weighted using the expected value of the auxiliary variable λ, and the parameters of the inverse gamma distribution followed by the measurement noise covariance matrix R are updated using the weighted statistics, thereby obtaining the new expected value of the R matrix. For the process noise covariance matrix Q, the algorithm maintains the inverse gamma distribution modeling and the update logic based on the state prediction error.

4. The algorithm according to claim 1, characterized in that, In step 3.3, the variational lower bound ELBO is used as a convergence criterion to determine whether the variational iteration loop has reached the optimal solution. When the value of ELBO no longer increases significantly, that is, when the increment between two iterations is less than a preset small threshold, it is considered that the current approximate distribution is close enough to the true distribution, the iteration process converges, and thus the optimal joint estimate of the state and noise covariance at the current moment is found.

5. The method according to claim 1, characterized in that, In step 4, the final measurement update process is implemented using the standard update procedure of unscented Kalman filtering: First, the predicted state Sigma point is mapped to the measurement space using a nonlinear observation function, and the predicted measurement value, measurement covariance, and cross-covariance are calculated. Next, the corrected measurement noise covariance obtained after the iteration convergence in step 3.3 is used to replace the fixed covariance in the standard Kalman filter, and the optimal Kalman gain is calculated. Finally, the predicted state is corrected using the optimal Kalman gain and the actual measurement residual.

Images