A multi-model adaptive ship GNSS / IMU integrated navigation method and system

Abstract

This invention provides a multi-model adaptive GNSS / IMU integrated navigation method and system for ships, comprising the following steps: S1, initialization phase; S2, multi-model establishment phase: establishing a uniform motion model for a large ship, a uniform acceleration motion model for a large ship, a uniform motion model for a small ship, and a uniform acceleration motion model for a small ship; S3, prediction phase; S4, measurement update phase; S5, model probability update and state fusion phase; S6, ship type identification and parameter adaptation phase; S7, system output and anomaly handling phase; S8, navigation termination phase. This method can adaptively identify ship type and adjust navigation parameters by evaluating the credibility of different models in real time.

Description

Technical Field

[0001] This invention relates to a ship GNSS / IMU integrated navigation method and system based on multi-model adaptation, belonging to the field of ship navigation technology. Background Technology

[0002] In the field of ship navigation, ensuring navigational safety hinges on acquiring high-precision position, velocity, and attitude information. However, traditional single-path navigation schemes suffer from numerous problems that can negatively impact navigational safety. First, navigation schemes relying solely on Global Navigation Satellite Systems (GNSS) are susceptible to signal blockage and multipath effects. Signal blockage can be caused by tall buildings, bridges, or other obstacles, while multipath effects arise from errors caused by signal reflection and refraction along different paths. Second, navigation schemes relying solely on Inertial Measurement Units (IMUs) suffer from error accumulation. IMUs calculate position and attitude by measuring acceleration and angular velocity, but due to inherent errors in their internal sensors, these errors accumulate over time, leading to decreased navigation accuracy. Third, traditional integrated navigation algorithms often fail to adapt to the motion characteristics of different ship types. Each type of ship exhibits different motion characteristics; for example, container ships, tankers, and fishing vessels have significantly different dynamic characteristics during navigation. Existing integrated navigation algorithms often fail to adequately consider these differences, thus affecting navigation accuracy. Finally, existing navigation algorithms lack the ability to adaptively identify ship hull features. Different ship types differ in design and structure, and these differences affect the ship's motion characteristics. However, existing navigation algorithms often fail to automatically recognize and adapt to these ship morphological characteristics, leading to reduced navigation accuracy and reliability. To overcome these problems, it is necessary to develop more advanced navigation systems that combine multiple sensors and algorithms to improve navigation accuracy and reliability. Summary of the Invention

[0003] This invention provides a ship GNSS / IMU integrated navigation method and system based on multi-model adaptation, which can effectively solve the above-mentioned problems.

[0004] This invention is implemented as follows:

[0005] In a first aspect, the present invention provides a ship GNSS / IMU integrated navigation method based on multi-model adaptation, comprising the following steps:

[0006] S1, Initialization Phase: Acquire GNSS and IMU measurement data from the ship and perform initialization processing on the GNSS position data;

[0007] S2, Multi-model establishment stage: Establish the large ship uniform motion model, the large ship uniform acceleration motion model, the small ship uniform motion model, and the small ship uniform acceleration motion model;

[0008] S3, Prediction Phase: At the beginning of each sampling period, the state interaction between models is first performed, the interaction probability is calculated, and then based on the interaction probability, the mixed initial state of each model is calculated. Based on the mixed initial state, each model uses its own dynamic equation to make a prediction.

[0009] S4, Measurement Update Phase: First, process GNSS measurement data, then process IMU measurement data, and then calculate the measurement information covariance based on the two types of measurement data. Next, calculate the Kalman gain and perform state update. After the state update is completed, calculate the likelihood function of each model.

[0010] S5, Model Probability Update and State Fusion Stage: Based on the calculated likelihood, update the probabilities of each model, introduce smoothing to avoid abrupt changes in model probabilities, and obtain the final state estimate through weighted fusion of all models;

[0011] S6, Ship type identification and parameter adaptation stage: Based on the probabilities of each model, calculate the ship type discrimination index, and adjust the adaptive parameters according to the index. First, adjust the process noise matrix. At the same time, evaluate the system performance based on the residual sequence of state estimation. Based on the performance evaluation, dynamically adjust the model transition probability matrix.

[0012] S7, System Output and Anomaly Handling Stage: Smooth the fusion state, detect anomalies by calculating the normalized state deviation, and take the following measures when an anomaly is detected: increase the noise covariance of the corresponding measurement channel, reduce the probability weight of the anomaly model, or perform cumulative statistics on the duration of the anomaly.

[0013] S8, Navigation Termination Phase: The operation is terminated when predetermined conditions are met.

[0014] In some embodiments, in step S1, the GNSS measurement data includes latitude (Lat), longitude (Lon), and altitude (h). An initial position is selected as a reference point (Lat0, Lon0, h0), and coordinate transformation is performed. The coordinate transformation consists of the following two steps:

[0015] Step 1: Convert the geodetic coordinates (Lat, Lon, h) to geocentric Earth-Fixed (ECEF) coordinates. The conversion formula is as follows:

[0016] X = (N + h)cos(Lat)cos(Lon)

[0017] Y = (N + h)cos(Lat)sin(Lon)

[0018] Z=(N(1-e 2 )+h)sin(Lat)

[0019] Where: N is the radius of the zonal circle: N = a / sqrt(1-e 2 sin 2 (Lat)), where a is the semi-major axis of the ellipsoid and e is the eccentricity of the ellipsoid;

[0020] Step 2: Convert ECEF coordinates to local ENU coordinates. The conversion formula is as follows:

[0021] [e]=[-sin(Lon0)cos(Lon0)0][X-X0]

[0022] [n]=[-sin(Lat0)cos(Lon0)-sin(Lat0)sin(Lon0)cos(Lat0)][Y-Y0]

[0023] [u]=[cos(Lat0)cos(Lon0)cos(Lat0)sin(Lon0)sin(Lat0)][Z-Z0]

[0024] Where (X0, Y0, Z0) are the ECEF coordinates of the reference point;

[0025] The transformed ENU coordinates will be used as the observation values ​​of the position component in the state vector;

[0026] Establish a system state vector, which contains nine components:

[0027] X = [e,n,u,v] e ,v n ,v u ,a e ,a n ,a u ] T

[0028] The position components [e,n,u] are obtained from the GNSS receiver and represent the position coordinates of the carrier in the northeast-northeast coordinate system: e is the eastward position provided by GNSS, n is the northward position provided by GNSS, and u is the celestial position provided by GNSS.

[0029] velocity component [v] e ,v n ,v u Obtained from GNSS receiver, v e v is the eastward velocity measured by GNSS. n v is the northward velocity measured by GNSS. u The astronomical velocity measured by GNSS;

[0030] acceleration components [a] e ,a n ,au ] Obtained through IMU measurement, a e : Eastward acceleration after coordinate transformation of IMU accelerometer measurements, a n : Northward acceleration after coordinate transformation of IMU accelerometer measurements, a u : The axial acceleration after coordinate transformation of the IMU accelerometer measurement.

[0031] In some embodiments, in step S2, four models are run simultaneously and interact through the IMM algorithm. Their probabilities are dynamically adjusted based on actual observation data. Initially, the probabilities of the four models are set to be equal, denoted as μ. i(0) =1 / 4, i = 1, 2, 3, 4, where μ i This represents the probability of the i-th model;

[0032] The state transition equation for the uniform motion model of the large ship is:

[0033]

[0034] The process noise matrix of the uniform motion model of the large ship is:

[0035]

[0036] Where: q l1 Let q be the noise intensity during the ship's position and velocity process. l2 The noise intensity during the acceleration process of a large ship;

[0037] The state transition equation for the uniformly accelerated motion model of the large ship is:

[0038]

[0039] The process noise matrix of the uniformly accelerated motion model of the large ship:

[0040]

[0041] Where, q l3 The noise intensity during the acceleration process of a large ship;

[0042] The state transition equation for the uniform motion model of the small boat is the same as that for the uniform motion model of the large boat, and its process noise matrix is:

[0043] Where: q s1 Let q be the noise intensity during the position and velocity process of the small boat. s2 The noise intensity during the acceleration process of the small boat;

[0044] The state transition equation for the uniformly accelerated motion model of the small boat is the same as that for the uniform motion model of the large boat, and its process noise matrix is:

[0045] Where, q s3 The noise intensity during the acceleration of the small boat;

[0046] The inter-model transition probability matrix is ​​as follows:

[0047]

[0048] Where: p 11 ,p 22 ,p 33 ,p 44 To maintain the probability of the model itself, p 12 ,p 21 and p 34 ,p 43 1 represents the conversion probability between different motion models of the same type of ship, and the rest represent the conversion probability between different ship types;

[0049] In some embodiments, in step S3, the formula for calculating the interaction probability is:

[0050] μ ij =(π) ij *μ i (k-1)) / c j ,

[0051] Where: μ ij π represents the probability of switching from model i to model j at time k. ij c represents the model transition probability. j Normalization factor;

[0052] The formula for calculating the mixed initial state of each model is as follows:

[0053] X 0j =Σ(X i (k-1)*μ ij )

[0054] Each model uses its own dynamic equations for prediction, and the prediction formula is as follows:

[0055] X i (k|k-1)=F i *X 0i (k-1|k-1), where F i This is the state transition matrix;

[0056] The state transition matrix F_cv for the uniform motion model is:

[0057] Where: I is a 3×3 identity matrix, and τ is the system sampling period, which comes from the system clock;

[0058] The state transition matrix of the uniformly accelerated motion model is:

[0059]

[0060] In some embodiments, in step S4: the formula for processing GNSS measurement data is:

[0061] v i (k)=Z gnss (k)-H gnss *X i (k|k-1)

[0062] Among them: Z gnss (k) represents GNSS measurements, including position and velocity information, H gnss This is the GNSS measurement matrix, indicating which state variables can be directly observed by GNSS, v i (k) is the information sequence, reflecting the difference between the predicted and measured values;

[0063] The processing of IMU measurement data involves: IMU measurement data is acceleration and angular velocity data in the carrier coordinate system, which undergoes coordinate transformation: a n =C b ^n*a b +g n ,

[0064] Where: a b C represents the acceleration in the carrier coordinate system measured by the IMU. b ^n is the transformation matrix from the vehicle coordinate system to the navigation coordinate system, obtained from the IMU attitude calculation, g n This is the local gravitational acceleration vector, calculated from latitude and altitude;

[0065] The calculated covariance of the measurement information is: S i (k)=H*P i (k|k-1)*H T +R

[0066] Where H is the observation matrix, H T It is H T P i (k|k-1) is the prediction error covariance matrix of the i-th state in the k-th iteration, and R is the measurement noise covariance matrix, which includes: GNSS position measurement noise, accuracy indicators provided by the GNSS receiver, GNSS velocity measurement noise, accuracy indicators provided by the GNSS receiver, and IMU acceleration measurement noise, indicators provided by the IMU device.

[0067] The calculated Kalman gain is:

[0068] Ki (k)=P i (k|k-1)*H T *S i (k) -1 ;

[0069] The state update is performed as follows:

[0070] X i (k|k)=X i (k|k-1)+K i (k)*v i (k)

[0071] P i (k|k)=(IK i (k)*H)*P i (k|k-1)

[0072] The likelihood function for each model is calculated as follows:

[0073] L i (k)=(2π)^(-n / 2)*|S i (k)|^(-1 / 2)*exp(-0.5*v i (k) T *S i (k) -1 *v i (k))

[0074] Where n is the dimension of the measurement vector.

[0075] In some embodiments, in step S5, the probability of updating each model is: μ i (k)=(L i (k)*μ i (k-1)) / c

[0076] Where: L i (k) represents the likelihood of the i-th model, indicating the model's explanatory power for the current observations, μ i (k-1) represents the model probability at the previous time step, and c is the normalization factor. To ensure that the sum of all model probabilities is 1, we obtain c = Σ(L) i (k)*μ i (k-1));

[0077] The smoothing process is as follows:

[0078] μ ismooth (k)=α*μ i (k)+(1-α)*μ i (k-1)

[0079] Where α is the smoothing coefficient, which is set based on the dynamic characteristics of the system;

[0080] The final state estimate obtained through the weighted fusion of all models is as follows:

[0081] X fusion (k)=Σ(μ ismooth (k)*X i (k|k))

[0082] The fused state covariance, taking into account the state differences between models, is as follows:

[0083] P fusion (k)=Σ(μ ismooth (k)*[P i (k|k)+dX i *dX i T ]),

[0084] Among them, dX i dX is the difference between each model state and the fused state. i =X i (k|k)-X fusion (k).

[0085] In some embodiments, in step S6, the calculation of the ship type discrimination index based on the probabilities of each model is as follows:

[0086] Ship score =ln((μ lcv +μ lca ) / (μ scv +μ sca ))

[0087] Where: μ lcv and μ lca Let μ be the probability of the uniform speed model and the uniform acceleration model of the large ship, respectively. scv and μ sca These are the probabilities of the uniform speed model and the uniform acceleration model of the small boat, respectively.

[0088] The process noise matrix is ​​adjusted as follows:

[0089] Q new =Q base *(1+β*Ship score )

[0090] Among them: Q base The basic process noise matrix is ​​β, which is an adjustment coefficient controlling the adaptive intensity; when Ship score When positive, it favors the characteristics of large ships; when negative, it favors the characteristics of small ships.

[0091] The performance of the evaluation system is as follows:

[0092] J(k)=v(k) T *S(k) -1 *v(k)

[0093] Where: v(k) is the measurement residual of the fused state, S(k) is the residual covariance matrix, and J(k) reflects the reliability of the current state estimate;

[0094] The model transition probability matrix is ​​dynamically adjusted based on performance evaluation as follows:

[0095] π new =(1-γ)*π base +γ*π adapted

[0096] Where: π base The basic transition probability matrix, π adapted The probability matrix is ​​adjusted based on the ship type identification results, and γ is an adaptive coefficient that varies with J(k).

[0097] In some embodiments, in step S7, the system output is to smooth the fused state:

[0098] X output (k)=X fusion (k)+λ*(X output (k-1)-X fusion (k))

[0099] Wherein, λ is the smoothing coefficient, which is determined based on the dynamic characteristics of the system;

[0100] The anomaly detection is performed by calculating the normalized state deviation.

[0101] D(k) = |X fusion (k)-X predict (k)| / σ(k)

[0102] Where σ(k) is the prediction uncertainty, which comes from the diagonal elements of the state covariance matrix.

[0103] In some embodiments, in step S8, the predetermined conditions are one or more of the following: receiving a system stop command, complete loss of GNSS signal for more than a preset duration, abnormal IMU data for more than a threshold duration, or system reinitialization count exceeding a set value.

[0104] In some embodiments, in step S3, the uncertainty is further described by predicting the covariance during the prediction phase.

[0105] The formula for predicting covariance is: P i(k|k-1)=F i *P 0i (k-1|k-1)*F i T +Q i ,

[0106] Where Fi is the state transition matrix, and Q... i Let be the process noise covariance matrix.

[0107] Secondly, the present invention provides a ship GNSS / IMU integrated navigation system based on multi-model adaptation, including a memory and a processor, characterized in that the memory stores computer-readable instructions, which, when executed by the processor, implement the functions according to the ship GNSS / IMU integrated navigation method based on multi-model adaptation.

[0108] Beneficial effects of this invention:

[0109] This invention dynamically evaluates the reliability levels of various models in real time, adaptively identifies the specific ship type, and flexibly adjusts navigation parameters based on the identification results to ensure the accuracy and reliability of the navigation system. The algorithm employs a modular design concept, dividing the entire system into multiple functionally independent modules, each undertaking a specific task. These modules interact and communicate closely through efficient data flow, working collaboratively to construct a complete and efficient adaptive filtering system capable of responding to changes in the external environment in real time and optimizing navigation performance. Attached Figure Description

[0110] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained from these drawings without creative effort.

[0111] Figure 1 This is a flowchart of a ship GNSS / IMU integrated navigation method based on multi-model adaptation, according to an embodiment of the present invention. Detailed Implementation

[0112] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to represent selected embodiments of the invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0113] Example 1

[0114] A ship GNSS / IMU integrated navigation method based on multi-model adaptation, the detailed steps of which are as follows:

[0115] S1, Initialization Phase: First, the system needs to acquire GNSS (Global Navigation Satellite System) measurement data and IMU (Inertial Measurement Unit) measurement data from the ship. After acquiring this data, the position data provided by GNSS is initialized. This step is crucial, aiming to ensure the accuracy and reliability of the data, thus laying a solid foundation for the subsequent navigation process.

[0116] S2, Multi-Model Establishment Phase: In this phase, the system constructs multiple motion models to adapt to different types of ship motion states. Specifically, these models include a large ship uniform motion model, a large ship uniform acceleration motion model, a small ship uniform motion model, and a small ship uniform acceleration motion model. Through these diverse models, the system can comprehensively cover the motion characteristics of ships under different navigation conditions, thereby significantly improving the adaptability and accuracy of the navigation system.

[0117] S3, Prediction Phase: At the beginning of each sampling period, state interactions between models are first performed, and the interaction probabilities between each model are calculated to evaluate the reliability of each model in the current state. Based on these interaction probabilities, the system further calculates the mixed initial state of each model, providing the necessary initial conditions for subsequent predictions. Subsequently, each model uses its own dynamic equations to make predictions to accurately predict the ship's motion state over a future period.

[0118] S4, Measurement Update Phase: First, the GNSS measurement data undergoes fine processing to extract useful information and eliminate potential errors as much as possible. Next, similar processing is applied to the IMU measurement data to ensure accuracy and consistency. Based on these two types of measurement data, the system calculates the measurement innovation covariance, then calculates the Kalman gain and performs a state update. After the state update is complete, the system also calculates the likelihood function of each model to evaluate the reliability of each model under the current measurement data.

[0119] S5, Model Probability Update and State Fusion Stage: Based on the calculated likelihood, the system updates the probabilities of each model and introduces a smoothing mechanism to avoid abrupt changes in model probabilities and ensure the smoothness of the model switching process. Through weighted fusion of all models, the system ultimately obtains a more accurate and reliable state estimate, further improving the overall performance of the navigation system.

[0120] S6, Ship Type Identification and Parameter Adaptation Stage: Based on the probability distribution of each model, the system calculates the ship type discrimination index and adjusts the parameters adaptively according to this index. First, the process noise matrix is ​​adjusted to better adapt to the noise characteristics of different ship types and motion states. Then, the system performance is evaluated based on the residual sequence of state estimation to ensure the stability and accuracy of the navigation system. Based on the performance evaluation results, the system dynamically adjusts the model transition probability matrix to flexibly adapt to changes in the ship's motion state.

[0121] S7, System Output and Anomaly Handling Stage: The fused state is smoothed to effectively eliminate potential noise and errors, ensuring the accuracy of the output state. By calculating the normalized state deviation, the system performs anomaly detection, promptly identifying and handling abnormal situations. When an anomaly is detected, the system takes a series of measures: increasing the noise covariance of the corresponding measurement channel to reduce the impact of abnormal data; reducing the probability weight of the anomaly model to decrease its contribution to the final state estimation; and accumulating statistics on the duration of the anomaly for further analysis and processing.

[0122] S8, Navigation Termination Phase: The system will terminate operation when predetermined conditions are met. These predetermined conditions may include reaching a predetermined sailing time, sailing distance, or a specific geographical location. By setting these conditions, the system ensures that the navigation process can safely and accurately terminate at the appropriate time, guaranteeing the overall safety of the ship's navigation.

[0123] In some embodiments, step S1 involves collecting GNSS measurement data, including latitude (Lat), longitude (Lon), and altitude (h). Specifically, latitude (Lat) is the latitude value provided by the GNSS receiver, longitude (Lon) is the longitude value provided by the GNSS receiver, and altitude (h) is the ellipsoidal height provided by the GNSS receiver. An initial location is selected as a reference point (Lat0, Lon0, h0), which will serve as the origin of the local ENU (East-North-Sky) coordinate system. Next, a coordinate transformation is performed, which consists of two steps:

[0124] The first step is to convert the geodetic coordinates (Lat, Lon, h) to geocentric Earth-fixed coordinates (ECEF). The conversion formula is as follows:

[0125] X = (N + h)cos(Lat)cos(Lon)

[0126] Y = (N + h)cos(Lat)sin(Lon)

[0127] Z=(N(1-e 2 )+h)sin(Lat)

[0128] Where N is the radius of the zonal circle, calculated using the formula: N = a / sqrt(1-e 2 sin 2 (Lat)), where a is the semi-major axis of the ellipsoid and e is the eccentricity of the ellipsoid. All angles should be converted to radians.

[0129] The second step is to convert the ECEF coordinates to local ENU coordinates. The conversion formula is as follows:

[0130] [e]=[-sin(Lon0)cos(Lon0)0][X-X0]

[0131] [n]=[-sin(Lat0)cos(Lon0)-sin(Lat0)sin(Lon0)cos(Lat0)][Y-Y0]

[0132] [u]=[cos(Lat0)cos(Lon0)cos(Lat0)sin(Lon0)sin(Lat0)][Z-Z0]

[0133] Where (X0, Y0, Z0) are the ECEF coordinates of the reference point. The transformed ENU coordinates will be used as the observation values ​​of the position component in the state vector.

[0134] Establish a system state vector, which contains nine components:

[0135] X = [e,n,u,v] e ,vn ,v u ,a e ,a n ,a u ] T

[0136] The position component [e,n,u] is obtained from the GNSS receiver and represents the carrier's position coordinates in the northeast-northeast coordinate system: e is the eastward position provided by the GNSS, n is the northward position provided by the GNSS, and u is the celestial position provided by the GNSS. The velocity component [v...] e ,v n ,v u Also obtained from GNSS receivers, v e It is the eastward velocity measured by GNSS, v n It is the northward velocity measured by GNSS, v u This is the astronomical velocity measured by GNSS. Acceleration component [a] e ,a n ,a u ] Obtained through IMU (Inertial Measurement Unit) measurement, a e It is the eastward acceleration after coordinate transformation of the IMU accelerometer measurement, a n It is the northward acceleration after coordinate transformation of the IMU accelerometer measurement, a u It is the celestial acceleration after coordinate transformation of the IMU accelerometer measurement value.

[0137] In some embodiments, step S2 involves establishing four parallel-running models to accommodate different types of ship motion characteristics. These models are differentiated based on typical characteristics of ships in actual shipping.

[0138] The Large-CV model of uniform motion for large ships is built upon the motion characteristics of large ships, reflecting the difficulty of steering and changing speed. Once a large ship reaches a steady speed, it tends to maintain that constant speed. The state transition equations for this model are as follows:

[0139]

[0140] Where I is the identity matrix and τ is the system sampling period, which comes from the system clock.

[0141] The process noise matrix of the uniform motion model of the large ship is:

[0142]

[0143] Where, q l1 q represents the intensity of the noise during the ship's position and velocity process. l2This represents the intensity of noise during the ship's acceleration process, where I is the identity matrix and τ is the system sampling period, derived from the system clock.

[0144] The Large-Action-Compact (CA) model for large ships aims to describe the dynamic changes of a large ship during acceleration changes. Considering the significant inertia of a large ship, its acceleration typically changes slowly. The state transition equations for this model are as follows:

[0145]

[0146] Where I is the identity matrix and τ is the system sampling period, which comes from the system clock.

[0147] The process noise matrix of the uniformly accelerated motion model of the large ship:

[0148]

[0149] Where I is the identity matrix, τ is the system sampling period, derived from the system clock; q l3 The noise intensity during the acceleration process of a large ship.

[0150] The small boat uniform motion model (Small-CV) considers that small boats are easily affected by external factors, and fluctuations will occur even during uniform motion. Its state transition equation is consistent with the large ship uniform motion model, but its process noise matrix is ​​shown below:

[0151]

[0152] Where: I is the identity matrix, τ is the system sampling period, derived from the system clock; q s1 Let q be the noise intensity during the position and velocity process of the small boat. s2 The noise intensity during the acceleration process of the small boat.

[0153] The Small-Calibrated Motion (Small-CA) model is designed to describe the dynamic behavior of a small boat during rapid maneuvers. This model considers rapid changes in acceleration, and its state transition equations are consistent with those of the large ship uniform motion model. However, due to the rapid changes in acceleration, its process noise matrix exhibits specific characteristics.

[0154]

[0155] Where I is the identity matrix, τ is the system sampling period, derived from the system clock; q s3 The noise intensity during the acceleration of the small boat.

[0156] The inter-model transition probability matrix is ​​as follows:

[0157]

[0158] Where, p 11 p 22 p 33 p 44 These represent the probability that the model itself maintains its state; these values ​​are relatively large. 12 p 21 and p 34 p 43 These represent the probability of switching between different motion models for the same type of ship, and these values ​​are the next most important; while the remaining conversion probabilities involve the conversion between different ship types, and their values ​​are relatively small.

[0159] Four models are launched simultaneously and interact using the IMM algorithm. These models dynamically adjust their probabilities based on real-time observation data. Initially, since the ship type cannot be predetermined, the probabilities of the four models are initialized to be equal, at a value of μ. i(0) =1 / 4, i = 1, 2, 3, 4, where μ i This represents the probability of the i-th model.

[0160] In some embodiments, in step S3, the formula for calculating the interaction probability is:

[0161] μ ij =(π) ij *μ i (k-1)) / c j ,

[0162] Where: μ ij c represents the probability of switching from model i to model j at time k. j This is the normalization factor. π ij The model transition probability is a probability matrix that is pre-defined based on the ship's motion characteristics: the transition probability is higher within the large ship model group, higher within the small ship model group, and lower between the two model groups.

[0163] The formula for calculating the mixed initial state of each model is as follows:

[0164] X 0j =Σ(X i (k-1)*μ ij );

[0165] Among them, X 0j For each model's mixed initial state, X i (k-1) is the state of the i-th model in the (k-1)-th iteration, μ ijThis is the transition probability from the i-th model to the j-th model. This formula is used to mix states in a multi-model system, allowing each model to learn from the information of other models before making a prediction. This mixing process can improve the robustness of the system because it allows the system to smoothly transition between different models and can reduce prediction errors caused by model switching.

[0166] Each model uses its own dynamic equations for prediction, and the prediction formula is as follows:

[0167] X i (k|k-1)=F i *X 0i (k-1|k-1),

[0168] Among them, F i The state transition matrix has different forms for uniform velocity and uniform acceleration models; X 0i (k-1|k-1) is the mixed initial state of the i-th model in the (k-1)-th iteration. The state transition matrix F i The form of F depends on the dynamic equations of the model. For uniform velocity and uniform acceleration models, F i The forms are different. For example, for the uniform velocity model, F i It might be a simple integral matrix, but for a uniformly accelerated model, F i It may include an acceleration term. This formula is used for state prediction in multi-model systems, where each model makes predictions based on its own dynamic equations. This prediction process is a key step in the Kalman filter algorithm, used to calculate the state estimate for the next time step.

[0169] The state transition matrix of the uniform motion model is:

[0170]

[0171] Where: I is the identity matrix, and τ is the system sampling period, which comes from the system clock.

[0172] The state transition matrix of the uniformly accelerated motion model is:

[0173]

[0174] Where I is the identity matrix, and τ is the system sampling period, derived from the system clock. This matrix additionally considers the effect of acceleration on position and velocity.

[0175] In some embodiments, in step S4: the formula for processing GNSS measurement data is:

[0176] v i (k)=Z gnss(k)-H gnss *X i (k|k-1).

[0177] Among them: Z gnss (k) represents GNSS measurements, including position and velocity information, H gnss This is the GNSS measurement matrix, indicating which state variables can be directly observed by GNSS, v i (k) is the innovation sequence, reflecting the difference between the predicted and measured values. GNSS innovation v i (k) reflects the deviation of the i-th model from the current GNSS measurement and is an important basis for subsequent calculation of likelihood and updating of state estimate.

[0178] The processing of IMU measurement data involves: IMU measurement data is acceleration and angular velocity data in the carrier coordinate system, which undergoes coordinate transformation: a n =C b ^n*a b +g n ,

[0179] Where: a b C represents the acceleration in the carrier coordinate system measured by the IMU. b ^n is the transformation matrix from the vehicle coordinate system to the navigation coordinate system, obtained from the IMU attitude calculation, g n This is the local gravitational acceleration vector, calculated from latitude and altitude.

[0180] Among them, C b The process of obtaining ^n is as follows:

[0181] (1) First, obtain the angular velocity measurement value [ω] from the IMU. x ,ω y ,ω z ] represents the rotational angular velocity about the three axes in the carrier coordinate system.

[0182] (2) Calculate the Euler angular rate:

[0183]

[0184]

[0185]

[0186] Where: φ is the roll angle, θ is the pitch angle, and ψ is the yaw angle.

[0187] (3) Euler angles are obtained through numerical integration:

[0188]

[0189]

[0190]

[0191] Where Δt is the sampling period.

[0192] (4) Constructing the attitude matrix C based on Euler angles b ^n:

[0193] C b ^n=R z (ψ)*R y (θ)*R x (φ), where:

[0194]

[0195]

[0196]

[0197] (5) To avoid gimbal lock, when the pitch angle θ is close to ±90°, the quaternion method needs to be used for attitude update:

[0198]

[0199] Where: q is the attitude quaternion, and Ω(ω) is the antisymmetric matrix of angular velocity. It is the time derivative of the quaternion, representing the rate of change of attitude.

[0200] The calculated measurement information covariance is:

[0201] S i (k)=H*P i (k|k-1)*H T +R

[0202] Among them, S i (k) represents the measurement innovation covariance of the i-th model at the current time k, and H is the observation matrix. T It is the transpose of H, P i (k|k-1) is the prediction error covariance matrix for the i-th state in the k-th iteration. R is the measurement noise covariance matrix, containing: GNSS position measurement noise (from accuracy specifications provided by the GNSS receiver); GNSS velocity measurement noise (from accuracy specifications provided by the GNSS receiver); and IMU acceleration measurement noise (from IMU device specifications). Measurement innovation covariance S i (k) reflects the uncertainty of measurement information.

[0203] The calculated Kalman gain is:

[0204] K i (k)=P i (k|k-1)*H T *S i (k) -1

[0205] Where Ki(k) is the Kalman gain of the i-th model at the current time k, and P i (k|k-1) is the prediction error covariance matrix of the i-th state in the k-th iteration, S i (k) -1 H is the transpose of Si(k) of the prediction error covariance matrix. T This is the transpose of the observation matrix H. The Kalman gain K is obtained by multiplying these three matrices. i (k) is used to adjust the predicted state in the state update step to incorporate new observation information.

[0206] The state update is performed as follows:

[0207] X i (k|k)=X i (k|k-1)+K i (k)*v i (k)

[0208] P i (k|k)=(IK i (k)*H)*P i (k|k-1)

[0209] Among them, X i (k|k): The state estimate of the i-th model at time k, X i (k|k-1): The predicted state of the i-th model in the k-th iteration, K i (k): Kalman gain of the i-th model at time k, v i (k): The innovation of the i-th model at the current time k, P i (k|k): The state covariance of the i-th model at the current time k, I: identity matrix, H: observation matrix.

[0210] The likelihood function for each model is calculated as follows:

[0211] L i (k)=(2π)^(-n / 2)*|S i (k)|^(-1 / 2)*exp(-0.5*v i (k) T *Si (k) -1 *v i (k))

[0212] Among them, L i (k) represents the likelihood of the i-th model at time k, v i (k) is the innovation of the i-th model, representing the difference between the observed values ​​and the model predictions; S i (k) is the innovation covariance matrix of the i-th model, representing the uncertainty of the innovation. n is the dimension of the measurement vector. This likelihood reflects the degree of fit between each model and the actual measurement.

[0213] In some embodiments, in step S5, the probability of updating each model is:

[0214] μ i (k)=(L i (k)*μ i (k-1)) / c.

[0215] Where: L i (k) represents the likelihood of the i-th model, indicating the model's explanatory power for the current observations, μ i (k) represents the probability of the i-th model at time k, μ i (k-1) represents the model probability at the previous time step k-1, and c is the normalization factor to ensure that the sum of all model probabilities is 1. This is obtained by summing: c = Σ(L i (k)*μ i (k-1)).

[0216] The smoothing process is as follows:

[0217] μ ismooth (k)=α*μ i (k)+(1-α)*μ i (k-1)

[0218] Where, μ ismooth (k): The smoothed probability of the i-th model at time k, μi(k): The probability of the i-th model at time k, μi i (k-1): The probability of the i-th model at time k-1 in the previous time step; α is the smoothing coefficient.

[0219] This formula is used to smooth the model probabilities, reducing probability fluctuations and improving system stability. In the formula, α*μ i (k) represents the contribution of the probability at the current time step, (1-α)*μ i(k-1) represents the contribution of the probability at the previous time step. The smoothing coefficient α determines the degree of influence of the current time step probability on the smoothed probability. When α is larger, the influence of the current time step probability on the smoothed probability is greater, and the smoothing effect is weaker; conversely, when α is smaller, the influence of the previous time step probability on the smoothed probability is greater, and the smoothing effect is stronger. The smoothing coefficient α is usually set based on the dynamic characteristics of the system to achieve the best smoothing effect.

[0220] The final state estimate obtained through the weighted fusion of all models is as follows:

[0221] X fusion (k)=Σ(μ ismooth (k)*X i (k|k))

[0222] Among them, X fusion (k): The final state estimate at time k, μ ismooth (k): The smoothed probability of the i-th model at time k, X i (k|k): The state estimate of the i-th model at time k.

[0223] In the formula, the state estimate X for each model i (k|k) multiplied by its corresponding smoothed probability μ ismooth (k), then sum over all models. Smoothed probability μ ismooth (k) serves as the weight, representing the contribution of each model to the final state estimate. Weighted fusion allows for a comprehensive consideration of the estimation results from all models, improving the accuracy and reliability of the final state estimate.

[0224] The fused state covariance, taking into account the state differences between models, is as follows:

[0225] P fusion (k)=Σ(μ ismooth (k)*[P i (k|k)+dX i *dX i T ]),

[0226] Among them, P fusion (k): The fused state covariance at time k, μ ismooth (k): The smoothed probability of the i-th model at time k, P i (k|k): The state covariance of the i-th model at time k, dX i Let dX be the difference between each model state and the fused state. i =X i (k|k)-X fusion(k). In the formula, the state covariance P of each model i (k|k) plus the difference dXi between the model state and the fused state and its transpose dX i T The product of these factors, then multiplied by the smoothed probability μ of the model. ismooth (k). Summing over all models yields the fused state covariance P. fusion (k). This method takes into account the state differences between models and can more accurately reflect the uncertainty of the fused state.

[0227] In some embodiments, in step S6, the calculation of the ship type discrimination index based on the probabilities of each model is as follows:

[0228] Ship score =ln((μ lcv +μ lca ) / (μ scv +μ sca ))

[0229] Ship score Ship type discrimination index. In this embodiment, two important probability parameters are defined, namely μ. lcv and μ lca These two parameters represent the probabilities of the large ship moving under uniform velocity and uniform acceleration models, respectively. In addition, we define two other probability parameters μ. scv and μ sca These probabilities correspond to the probabilities of the small boat under uniform motion and uniform acceleration motion models, respectively. These probability parameters allow for the modeling and analysis of the behavior of different types of vessels under various motion states. Notably, the logarithmic ratios of these probability parameters provide a quantitative indicator reflecting the system's bias in judging different ship types. In other words, analyzing the logarithmic ratios of these probability parameters provides a deeper understanding of the system's preferences and decision-making logic when processing data from different types of vessels.

[0230] Ship score The value can be used to determine the ship type: if Ship score A positive value indicates a greater likelihood of a large ship; if the ship... score A negative value indicates that it is more likely to be a small boat.

[0231] The process noise matrix is ​​adjusted as follows:

[0232] Q new =Q base *(1+β*Ship score )

[0233] Among them: Q newFor the new process noise matrix, Q base The basic process noise matrix is ​​used; the adjustment coefficient β controls the adaptive strength, and the larger β is, the stronger the influence of the ship type score on the process noise matrix. score Rate the ship type; (1+β×Ship) score ) is an adjustment factor, which is based on the ship type rating. score The basic process noise matrix is ​​adjusted using the adjustment coefficient β.

[0234] When Ship score When Q is positive, it indicates a bias towards large ship characteristics, and the adjustment factor is greater than 1, thus Q... new Will be more than Q base Larger indicates greater process noise; when Ship score When Q is negative, it indicates a bias towards small boat characteristics, and the adjustment factor is less than 1, thus Q... new Will be more than Q base Smaller indicates less process noise.

[0235] The performance of the evaluation system is as follows:

[0236] J(k)=v(k) T *S(k) -1 *v(k)

[0237] Where: v(k) is the measurement residual of the fused state, S(k) is the residual covariance matrix, and J(k) reflects the reliability of the current state estimate; the smaller the value of J(k), the better the matching degree between the measurement residual and the residual covariance matrix, and thus the higher the reliability of the current state estimate.

[0238] The model transition probability matrix is ​​dynamically adjusted based on performance evaluation as follows:

[0239] π new =(1-γ)*π base +γ*π adapted

[0240] Where: π base The basic transition probability matrix, π adapted The probability matrix is ​​adjusted based on the ship type identification results, and γ is an adaptive coefficient that varies with J(k).

[0241] In the formula, (1-γ)*π base Represents the basic transition probability matrix π base Partial contribution. γ*π adapted This represents the probability matrix π adjusted based on the ship type identification results. adapted The adaptive coefficient γ determines π. adapted For the new transition probability matrix π newThe degree of influence. The adaptive coefficient γ changes with J(k), that is, the value of γ is dynamically adjusted according to the reliability index J(k) of the current state estimate. If J(k) is small, it indicates that the reliability of the current state estimate is high, and γ can be appropriately reduced to decrease π. adapted The influence of γ; conversely, if J(k) is large, it indicates that the reliability of the current state estimate is low, and γ and π can be appropriately increased. adapted The impact.

[0242] In some embodiments, in step S7, the system output is to smooth the fused state:

[0243] X output (k)=X fusion (k)+λ*(X output (k-1)-X fusion (k))

[0244] Where λ is the smoothing coefficient, determined based on the system's dynamic characteristics; Xoutput(k): the output state at the current moment; Xfusion(k): the fused state at the current moment; and Xoutput(k-1): the output state at the previous moment. This formula calculates the current output state Xoutput(k) by multiplying the difference between the current fused state Xfusion(k) and the previous output state Xoutput(k-1) by the smoothing coefficient λ, and then adding this difference to the current fused state. The smoothing coefficient λ determines the degree of influence of the previous output state on the current output state. The larger λ is, the greater the influence of the previous output state on the current output state, and the more pronounced the smoothing effect.

[0245] The anomaly detection is performed by calculating the normalized state deviation.

[0246] D(k) = |X fusion (k)-X predict (k)| / σ(k)

[0247] Where D(k): the normalized state deviation at the current time, Xfusion(k): the fused state at the current time, Xpredict(k): the predicted state at the current time; σ(k) is the prediction uncertainty, derived from the diagonal elements of the state covariance matrix. This formula calculates the normalized state deviation D(k) by dividing the absolute value of the difference between the fused state Xfusion(k) and the predicted state Xpredict(k) by the prediction uncertainty σ(k). The normalized state deviation D(k) can be used to determine whether the system is abnormal. If D(k) exceeds a certain preset threshold, it may indicate that the system is abnormal.

[0248] In some embodiments, the predetermined conditions mentioned in step S8 may include, but are not limited to, the following situations: when the system receives a stop command, when the Global Navigation Satellite System (GNSS) signal is completely lost and this situation persists for more than a preset time length, when the inertial measurement unit (IMU) data is abnormal and this abnormal situation persists for more than a set threshold, or when the number of system reinitializations exceeds a preset value. These conditions may occur individually or in any combination thereof.

[0249] In some embodiments, in step S3, the prediction phase not only relies on the basic prediction model, but also introduces the concept of prediction covariance to further describe the uncertainty of the system. Prediction covariance can provide statistical information about prediction errors, thereby enabling the system to more accurately assess the uncertainty of future states and providing more reliable data support for subsequent decision-making and control.

[0250] The formula for predicting covariance is: P i (k|k-1)=F i *P 0i (k-1|k-1)*F i T +Q i

[0251] Among them, F i It is the state transition matrix, P i (k|k-1) is the estimation error covariance matrix of the i-th state in the (k-1)-th iteration. In the formula, F... i *P 0i (k-1|k-1)*F i T This indicates that the state transition matrix F i The estimated error covariance matrix P at the (k-1)th iteration 0i (k-1|k-1) is the prediction error covariance matrix transferred to the k-th iteration. Q i This represents the process noise covariance matrix, which is added to the transformed covariance matrix to account for the impact of process noise on the prediction error.

[0252] Example 2

[0253] The embodiments of this invention aim to provide a ship GNSS / IMU integrated navigation system based on multi-model adaptation. This system possesses significant technical advantages and practical application value. Specifically, the system comprises two main components: a memory and a processor. The memory pre-stores computer-readable instruction sets. When executed by the processor, these instruction sets can implement the function of a specific navigation method. The core of this method lies in utilizing multi-model adaptation technology to effectively fuse data from the Global Navigation Satellite System (GNSS) and the Inertial Measurement Unit (IMU), thereby providing ships with more accurate and reliable navigation information.

[0254] In embodiments of this invention, multi-model adaptive technology refers to establishing multiple navigation models and dynamically selecting or adjusting the weights of these models based on changes in the actual navigation environment and conditions to achieve optimal navigation performance. GNSS systems provide precise global position information, while IMUs calculate the vehicle's motion state by measuring acceleration and angular velocity. Combining these two technologies can effectively compensate for the shortcomings of a single navigation system in certain environments, improving the robustness and accuracy of the overall navigation system.

[0255] Furthermore, embodiments of the present invention may also include other auxiliary modules and functions, such as data fusion algorithms, error correction mechanisms, and fault detection and isolation systems, to further improve the system's performance and reliability. Through these technical means, the ship's navigation system can provide continuous, stable, and high-precision navigation services in various complex marine environments, ensuring the safety and efficiency of ship navigation.

[0256] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the invention by those skilled in the art. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the invention should be included within the scope of protection of the invention.

Claims

1. A ship GNSS / IMU integrated navigation method based on multi-model adaptation, characterized in that, Includes the following steps: S1, Initialization Phase: Acquire GNSS and IMU measurement data from the ship and perform initialization processing on the GNSS position data; S2, Multi-model establishment stage: Establish the large ship uniform motion model, the large ship uniform acceleration motion model, the small ship uniform motion model, and the small ship uniform acceleration motion model; S3, Prediction Phase: At the beginning of each sampling period, the state interaction between models is first performed, the interaction probability is calculated, and then based on the interaction probability, the mixed initial state of each model is calculated. Based on the mixed initial state, each model uses its own dynamic equation to make a prediction. S4, Measurement Update Phase: First, process GNSS measurement data, then process IMU measurement data, and then calculate the measurement information covariance based on the two types of measurement data. Next, calculate the Kalman gain and perform state update. After the state update is completed, calculate the likelihood function of each model. S5, Model Probability Update and State Fusion Stage: Based on the calculated likelihood, update the probabilities of each model, introduce smoothing to avoid abrupt changes in model probabilities, and obtain the final state estimate through weighted fusion of all models; S6, Ship type identification and parameter adaptation stage: Based on the probabilities of each model, calculate the ship type discrimination index, and adjust the adaptive parameters according to the index. First, adjust the process noise matrix. At the same time, evaluate the system performance based on the residual sequence of state estimation. Based on the performance evaluation, dynamically adjust the model transition probability matrix. S7, System Output and Anomaly Handling Stage: Smooth the fusion state, detect anomalies by calculating the normalized state deviation, and take the following measures when an anomaly is detected: increase the noise covariance of the corresponding measurement channel, reduce the probability weight of the anomaly model, or perform cumulative statistics on the duration of the anomaly. S8, Navigation Termination Phase: The operation is terminated when predetermined conditions are met.

2. The ship GNSS / IMU integrated navigation method based on multi-model adaptation according to claim 1, characterized in that, In step S1, the GNSS measurement data includes latitude (Lat), longitude (Lon), and altitude (h). An initial position is selected as a reference point (Lat0, Lon0, h0), and coordinate transformation is performed. The coordinate transformation consists of the following two steps: Step 1: Convert the geodetic coordinates (Lat, Lon, h) to geocentric Earth-fixed coordinates (ECEF). The conversion formula is as follows: X = (N + h)cos(Lat)cos(Lon) Y = (N + h)cos(Lat)sin(Lon) Z = (N(1-w²) + h)sin(Lat) Where: N is the radius of the ramusoidal circle: N=a / sqrt(1-w²sin²(Lat)), a is the semi-major axis of the ellipsoid, and w is the eccentricity of the ellipsoid; Step 2: Convert ECEF coordinates to local ENU coordinates. The conversion formula is as follows: Where (X0,Y0,Z0) are the ECEF coordinates of the reference point; The transformed ENU coordinates will be used as the observation values ​​of the position component in the state vector; Establish a system state vector, which contains nine components: X=[e,n,u,v e ,v n ,v u ,a e ,a n ,a u ] T The position components [e,n,u] are obtained from the GNSS receiver and represent the position coordinates of the carrier in the northeast-northeast coordinate system: e is the eastward position provided by GNSS, n is the northward position provided by GNSS, and u is the celestial position provided by GNSS. velocity component [v] e ,v n ,v u Obtained from GNSS receiver, v e v is the eastward velocity measured by GNSS. n v is the northward velocity measured by GNSS. u The astronomical velocity measured by GNSS; acceleration components [a] e ,a n ,a u ] Obtained through IMU measurement, a e : Eastward acceleration after coordinate transformation of IMU accelerometer measurements, a n : Northward acceleration after coordinate transformation of IMU accelerometer measurements, a u : The axial acceleration after coordinate transformation of the IMU accelerometer measurement.

3. The ship GNSS / IMU integrated navigation method based on multi-model adaptation according to claim 1, characterized in that, In step S2, four models are run simultaneously and interact using the IMM algorithm. Their probabilities are dynamically adjusted based on actual observation data. Initially, the probabilities of the four models are set to be equal, at μ. i(0) =1 / 4, i=1,2,3,4, where μ i This represents the probability of the i-th model; The state transition equation for the uniform motion model of the large ship is: The process noise matrix of the uniform motion model of the large ship is: Where: q l1 Let q be the noise intensity during the ship's position and velocity process. l2 The noise intensity during the acceleration process of a large ship; The state transition equation for the uniformly accelerated motion model of the large ship is: The process noise matrix of the uniformly accelerated motion model of the large ship: Where, q l3 The noise intensity during the acceleration of a large ship; The state transition equation for the uniform motion model of the small boat is the same as that for the uniform motion model of the large boat, and its process noise matrix is: Where: q s1 Let q be the noise intensity during the position and velocity process of the small boat. s2 The noise intensity during the acceleration process of the small boat; The state transition equation for the uniformly accelerated motion model of the small boat is the same as that for the uniformly accelerated motion model of the large boat, and its process noise matrix is: Where, q s3 The noise intensity during the acceleration of the small boat; The inter-model transition probability matrix is ​​as follows: Where: p 11 ,p 22 ,p 33 ,p 44 To maintain the probability of the model itself, p 12 ,p 21 and p 34 ,p 43 1 represents the conversion probability between different motion models of the same type of ship, and the rest represent the conversion probability between different ship types; In each matrix, I is the identity matrix, and τ is the system sampling period, which comes from the system clock.

4. The ship GNSS / IMU integrated navigation method based on multi-model adaptation according to claim 1, characterized in that, In step S3, the formula for calculating the interaction probability is: m ij =(π ij *m i (k-1)) / c j Where: μ ij π represents the probability of switching from model i to model j at time k. ij c represents the model transition probability. j μ is the normalization factor. i (k-1) represents the state probability of model i at time k-1; The formula for calculating the mixed initial state of each model is as follows: X 0j =Σ(X i (k-1)*μ ij ), where X 0j For each model's mixed initial state, X i (k-1) is the state of the i-th model in the (k-1)-th iteration, μ ij It is the transition probability from the i-th model to the j-th model; Each model uses its own dynamic equations for prediction, and the prediction formula is as follows: X i (k|k-1)=F i *X 0i (k-1|k-1), where F i Let X be the state transition matrix. 0i (k-1|k-1) is the mixed initial state of the i-th model in the (k-1)-th iteration; X i (k|k-1) represents the prior predicted state of model i at time k, derived from time k-1. The state transition matrix of the uniform motion model is: Where: I is the identity matrix, and τ is the system sampling period, which comes from the system clock; The state transition matrix of the uniformly accelerated motion model is: ； In each matrix, I is the identity matrix, and τ is the system sampling period, which comes from the system clock.

5. The ship GNSS / IMU integrated navigation method based on multi-model adaptation according to claim 1, characterized in that, In step S4: the formula for processing GNSS measurement data is: v i (k)=Z gnss (k)-H gnss *X i (k|k-1) Where: Z gnss (k) represents GNSS measurements, including position and velocity information, H gnss This is the GNSS measurement matrix, indicating which state variables can be directly observed by GNSS, X i (k|k-1) is the predicted value v of the i-th state vector at the k-th iteration. i (k) is the information sequence, reflecting the difference between the predicted and measured values; The processing of IMU measurement data involves: IMU measurement data is acceleration and angular velocity data in the carrier coordinate system, which undergoes coordinate transformation: a n =C b ^n*a b +g n , Where: a n Let a be the acceleration vector. b C represents the acceleration in the carrier coordinate system measured by the IMU. b ^n is the transformation matrix from the vehicle coordinate system to the navigation coordinate system, obtained from the IMU attitude calculation, g n This is the local gravitational acceleration vector, calculated from latitude and altitude; The calculated measurement information covariance is: S i (k)=H*P i (k|k-1)*H T +R Where Si(k) is the measurement innovation covariance of the i-th model at the current time k, and H is the observation matrix. T It is the transpose of H, P i (k|k-1) is the prediction error covariance matrix of the i-th state in the k-th iteration, and R is the measurement noise covariance matrix, which includes: GNSS position measurement noise, accuracy indicators provided by the GNSS receiver, GNSS velocity measurement noise, accuracy indicators provided by the GNSS receiver, and IMU acceleration measurement noise, indicators provided by the IMU device. The calculated Kalman gain is: K i (k)=P i (k|k-1)*H T *S i (k) -1 Among them, K i (k) represents the Kalman gain of the i-th model at time k, P i (k|k-1) is the prediction error covariance matrix of the i-th state in the k-th iteration, S i (k) -1 S is the prediction error covariance matrix. i The transpose matrix of (k), H T The transpose of the observation matrix H; The state update is performed as follows: X i (k|k)=X i (k|k-1)+K i (k)*v i (k), P i (k|k)=(I-K i (k)*H)*P i (k|k-1), Among them, X i (k|k) is the state estimate of the i-th model at time k, X i (k|k-1) represents the predicted state of the i-th model in the k-th iteration, where K... i (k) represents the Kalman gain of the i-th model at time k, v i (k) represents the information of the i-th model at the current time k, P i (k|k) represents the state covariance of the i-th model at the current time k, where I is the identity matrix and H is the observation matrix; The likelihood function for each model is calculated as follows: L i (k)=(2π)^(-n / 2)*|S i (k)|^(-1 / 2)*exp(-0.5*v i (k) T *S i (k) -1 *v i (k)) Among them, L i (k) represents the likelihood of the i-th model at time k, v i (k) is the information of the i-th model, representing the difference between the observed value and the model prediction; S i (k) is the innovation covariance matrix of the i-th model, representing the uncertainty of the innovation, and n is the dimension of the measurement vector.

6. The ship GNSS / IMU integrated navigation method based on multi-model adaptation according to claim 1, characterized in that, In step S5, the probability of updating each model is: μ i (k)=(L i (k)*μ i (k-1)) / c Where: L i (k) represents the likelihood of the i-th model, indicating the model's explanatory power for the current observations, μ i (k-1) represents the model probability at the previous time step, and c is the normalization factor. To ensure that the sum of all model probabilities is 1, we obtain c = Σ(L) i (k)*μ i (k-1)); The smoothing process is as follows: m ismooth (k)=a*m i (k)+(1-a)*μ i (k-1) Where, μ ismooth (k) represents the smoothed probability of the i-th model at the current time k. μ i (k) represents the probability of the i-th model at time k, μ i (k-1) represents the probability of the i-th model at time k-1 in the previous time step; α is the smoothing coefficient, which is set based on the dynamic characteristics of the system. The final state estimate obtained through the weighted fusion of all models is as follows: X fusion (k)=Σ(μ ismooth (k)*X i (k|k)), where X fusion (k) represents the final state estimate at the current time k, μ ismooth (k) represents the smoothed probability of the i-th model at time k, X i (k|k) is the state estimate of the i-th model at the current time k; The fused state covariance, taking into account the state differences between models, is as follows: P fusion (k)=Σ(μ ismooth (k)*[P i (k|k)+dX i *dX i T ]), Among them, P fusion (k) represents the fused state covariance at the current time k, μ ismooth (k) represents the smoothed probability of the i-th model at time k, P i (k|k) is the state covariance of the i-th model at the current time k, dX i dX is the difference between each model state and the fused state. i =X i (k|k)-X fusion (k).

7. The ship GNSS / IMU integrated navigation method based on multi-model adaptation according to claim 1, characterized in that, In step S6, the ship type discrimination index is calculated based on the probabilities of each model as follows: Ship score =ln((μ lcv +m lca ) / (m scv +m sca )) Where: μ lcv and μ lca Let μ be the probability of the uniform speed model and the uniform acceleration model of the large ship, respectively. scv and μ sca These are the probabilities of the uniform speed model and the uniform acceleration model of the small boat, respectively. The process noise matrix is ​​adjusted as follows: Q new =Q base *(1+β*Ship score ) Among them: Q base The basic process noise matrix is ​​β, which is an adjustment coefficient controlling the adaptive intensity; when Ship score When positive, it favors the characteristics of large ships; when negative, it favors the characteristics of small ships. The performance of the evaluation system is as follows: J(k)=v(k) T *S(k) -1 *v(k) Where: v(k) is the measurement residual of the fused state, S(k) is the residual covariance matrix, and J(k) reflects the reliability of the current state estimate; The model transition probability matrix is ​​dynamically adjusted based on performance evaluation as follows: p new =(1-c)*π base +g*p adapted Where: π base The basic transition probability matrix, π adapted The probability matrix is ​​adjusted based on the ship type identification results, and γ is an adaptive coefficient that varies with J(k).

8. The ship GNSS / IMU integrated navigation method based on multi-model adaptation according to claim 1, characterized in that, In step S7, the system output is to smooth the fused state: X output (k)=X fusion (k)+λ*(X output (k-1)-X fusion (k)) Among them, X output (k) represents the current output state, X fusion (k) represents the fusion state at the current moment, X output (k-1) represents the output state at the previous time step, and λ is the smoothing coefficient, which is determined based on the dynamic characteristics of the system. The anomaly detection is performed by calculating the normalized state deviation. D(k)=|X fusion (k)-X predict (k)| / σ(k) Where D(k) is the normalized state deviation at the current time, and X... fusion (k) represents the fusion state at the current moment, X predict σ(k) represents the predicted state at the current time, and σ(k) represents the prediction uncertainty, which comes from the diagonal elements of the state covariance matrix.

9. The ship GNSS / IMU integrated navigation method based on multi-model adaptation according to claim 1, characterized in that, In step S8, the predetermined conditions are one or more of the following: receiving a system stop command, complete loss of GNSS signal for more than a preset time, abnormal IMU data for more than a threshold, or system reinitialization times exceeding a set value.

10. A ship GNSS / IMU integrated navigation system based on multi-model adaptation, characterized in that, The system includes a memory and a processor, characterized in that the memory stores computer-readable instructions, which, when executed by the processor, implement the function of the ship GNSS / IMU integrated navigation method based on multi-model adaptation according to any one of claims 1 to 9.

Images