An Adaptive Heading Angle Filtering Navigation Method Based on GNSS Track Quality Assessment

Abstract

This invention discloses an adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment, comprising: S1, initialization phase; S2, multi-model establishment phase: assessing the GNSS signal quality, and establishing a first large ship uniform motion model, a first large ship uniform acceleration motion model, a first small ship uniform motion model, and a first small ship uniform acceleration motion model when the assessment value is greater than a preset value; and establishing a second large ship uniform motion model, a second large ship uniform acceleration motion model, a second small ship uniform motion model, and a second small ship uniform acceleration motion model when the assessment value is less than a preset value; 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; and S8, navigation termination phase: terminating operation when predetermined conditions are met. This method adaptively identifies ship types and adjusts navigation parameters by real-time assessment of the credibility of different models.

Description

Technical Field

[0001] This invention relates to an adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment, 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 an adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment, which can effectively solve the above-mentioned problems.

[0004] This invention is implemented as follows:

[0005] An adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment includes 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: The GNSS signal quality is evaluated. When the evaluation value is greater than the preset value, the first large ship uniform motion model, the first large ship uniform acceleration motion model, the first small ship uniform motion model, and the first small ship uniform acceleration motion model are established. When the evaluation value is less than the preset value, the second large ship uniform motion model, the second large ship uniform acceleration motion model, the second small ship uniform motion model, and the second small ship uniform acceleration motion model are established.

[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, then calculate the measurement information covariance based on the two types of measurement data, and then perform state update;

[0010] S5, Model Probability Update and State Fusion Stage: Perform GNSS quality adaptation, update the probabilities of each model, then perform GNSS quality adjustment, 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 GNSS quality score, and make adaptive parameter adjustments according to the index and GNSS quality score, and 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 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.

[0028] 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;

[0029] 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.

[0030] In some embodiments, step S2, evaluating the GNSS signal quality, includes:

[0031] (1) Distance Consistency Assessment:

[0032] Calculate the distance between adjacent GNSS points:

[0033] d i =sqrt((lon) i -lon {i-1} ) 2 +(lat i -lat {i-1} ) 2 )

[0034] Calculate the standard deviation of distance: std d =sqrt(Σ(d i -d mean ) 2 / (n-1))

[0035] Distance Consistency Score d =1-min(1,std d / MAX d ISTANCE STD )

[0036] Where: MAX d ISTANCE STD The distance to the standard deviation threshold is a fixed constant;

[0037] (2) Directional consistency assessment:

[0038] Calculate the direction change between adjacent GNSS points: θ i =atan2(lat i -lat {i-1} ,lon i -lon {i-1} )

[0039] Calculate the standard deviation of directional change: std θ =sqrt(Σ(θ) i -θ mean ) 2 / (n-1))

[0040] Directional consistency score: θ =1-min(1,std θ / MAX d IRECTION STD )

[0041] Where: MAX d IRECTION STD The standard deviation threshold for direction is a fixed constant.

[0042] (3) Speed ​​consistency assessment:

[0043] Calculate the velocity derived from GNSS points: v GNSS =d i / Δt

[0044] Calculate the difference between the velocity and the NMEA velocity: Δv = |v GNSS -v nmea |

[0045] Speed ​​Consistency Score v =1-min(1,Δv / SPEED) d IFF THRESHOLD )

[0046] Among them: SPEED d IFF THRESHOLD The velocity difference threshold is a fixed constant.

[0047] Overall quality score:

[0048] quality score =WEIGHT d ISTANCE*score d +WEIGHT d IRECTION*score θ

[0049] +WEIGHT SPEED *score v

[0050] Among them: WEIGHT d ISTANCE is the distance rating weight, which is a fixed constant. WEIGHT d IRECTION is the directional score weight, which is a fixed constant. WEIGHTSPEED The speed score weight is a fixed constant.

[0051] In some embodiments, in step S2,

[0052] In step S2, eight models are run simultaneously and interact using the IMM algorithm. Their probabilities are dynamically adjusted based on actual observation data. Initially, the probabilities of all eight models are set to be equal, at a value of μ. i (0) = INIT_MODEL_PROB, a fixed constant with a value of 1 / 8, where μ i This represents the probability of the i-th model;

[0053] State transition equation for the uniform motion mode of the first large ship:

[0054]

[0055] The process noise matrix of the first large ship's uniform motion mode:

[0056]

[0057] Where: I is a 3×3 identity matrix, τ is the system sampling period, and Q L1G Let Q be the noise figure of the ship's position process. L2G The noise figure during the acceleration process of a large ship;

[0058] State transition equations for the first uniformly accelerated motion model of the large ship:

[0059]

[0060] The process noise matrix of the first large ship's uniformly accelerated motion model:

[0061]

[0062] Among them: Q L3G The noise figure for the uniform acceleration model of a large ship;

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

[0064] Among them: Q L1P Let Q be the noise figure of the ship's position process. L2P The noise figure during the acceleration process of a large ship;

[0065] The state transition equations for the second largest ship's uniformly accelerated motion model are the same as those for the first largest ship's uniformly accelerated motion model, and its process noise matrix is:

[0066] Among them: Q L3PThe noise figure for the uniform acceleration model of a large ship;

[0067] The state transition equations for the uniform motion model of the first small boat are the same as those for the uniform motion model of the first large boat.

[0068] Its process noise matrix:

[0069] Among them: Q S1G Q is the noise figure during the boat's position process. S2G The noise figure during the acceleration process of the small boat;

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

[0071] Among them: Q S3G The noise figure for the uniform acceleration model of the small boat;

[0072] The state transition equation for the second small boat's uniform motion model is the same as that for the first small boat's uniform motion model.

[0073]

[0074] Process noise matrix:

[0075] Among them: Q S1P Q is the noise figure during the boat's position process. S2P The noise figure during the acceleration process of the small boat;

[0076] The state transition equation for the second boat's uniformly accelerated motion model is the same as that for the first boat's uniformly accelerated motion model, and its process noise matrix is:

[0077] Among them: Q S3P The noise figure for the uniform acceleration model of the small boat;

[0078] The extended model transition probability matrix is:

[0079]

[0080] Where: P ii To maintain the probability of the model itself, a constant greater than 0.9, P ij (i≠j) represents the transition probability from model i to model j, which satisfies the following for any model i: ΣP ij =1 (j=1,2,...,8).

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

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

[0083] 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;

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

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

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

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

[0088] P i (k|k-1)=F i *P 0i (k-1|k-1)*F i T +Q i

[0089] Where: X i (k-1) represents the state estimate of the i-th model at time k-1.

[0090] P i (k-1|k-1) is the covariance matrix of the i-th model at time k-1.

[0091] F i Let be the state transition matrix of the i-th model.

[0092] Q i Let be the process noise matrix of the i-th model.

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

[0094] v i (k)=Z GNSS (k)-H GNSS *X i (k|k-1)

[0095] 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, v i (k) is the information sequence, reflecting the difference between the predicted and measured values;

[0096] 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 ,

[0097] 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;

[0098] The calculated measurement information covariance is:

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

[0100] Where 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.

[0101] Status Update:

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

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

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

[0105] Where: Z GNSS(k) is the GNSS observation vector at time k.

[0106] H GNSS This is the GNSS observation matrix.

[0107] v i (k) is the information sequence of the i-th model.

[0108] C b ^n is the transformation matrix from the vehicle coordinate system to the navigation coordinate system.

[0109] a b The acceleration of the load system measured by the IMU.

[0110] g n This is the gravitational acceleration vector in the navigation coordinate system.

[0111] a n This refers to the acceleration in the navigation coordinate system.

[0112] In some embodiments, in step S5, GNSS quality is adaptive:

[0113] quality factor = sigmoid(quality) score ,GNSS_SIGMOID_K,GNSS THRESHOLD )

[0114] Where: sigmoid(x,k,t)=1 / (1+exp(-k*(xt))) is a sigmoid function.

[0115] quality score To score the quality of GNSS tracks,

[0116] GNSS_SIGMOID_K is the steepness parameter of the sigmoid function, which is a fixed constant.

[0117] GNSS THRESHOLD The quality judgment threshold is a fixed constant.

[0118] When quality score =GNSS THRESHOLD At that time, quality factor =0.5,

[0119] When quality score >>GNSS THRESHOLD At that time, quality factor Approaching 1

[0120] When quality score < <GNSS THRESHOLDAt that time, quality factor Approaching 0;

[0121] The probability of updating each model is: μ i (k)=(L i (k)*μ i (k-1)) / c,

[0122] 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));

[0123] GNSS quality adjustment:

[0124] For each model i:

[0125] If i is the first model: μ iadjusted (k)=μ i (k)*quality factor

[0126] If i is the second model: μ iadjusted (k)=μ i (k)*(1-quality factor );

[0127] Probability smoothing: μ ismooth (k)=SMOOTH factor *μ iadjusted (k)+(1-SMOOTH factor )*μ ismooth (k-1);

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

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

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

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

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

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

[0134] In some embodiments, in step S6, the ship type is determined:

[0135] Ship score =ln((Σμ large (k)) / (Σμ small (k)))

[0136] Where Σμ large (k) and Σμ small (k) represents the total probability of the large ship and the small ship models, respectively;

[0137] GNSS quality assessment:

[0138] GNSS score =ln((Σμ good (k)) / (Σμ poor (k)))

[0139] Where Σμ good (k) and Σμ poor (k) represent the total probabilities of the good signal and bad signal models, respectively;

[0140] Parameter adaptation:

[0141] Q new =Q base *(1+SHIP_ADAPT factor *Ship score )*(1+GNSS_ADAPT factor *GNSS score );

[0142] Adaptive transition probability:

[0143] π new = (1-ADAPT_RATE)*π base +ADAPT_RATE*π adapted .

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

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

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

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

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

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

[0150] 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.

[0151] The beneficial effects of this invention are:

[0152] This invention dynamically evaluates the reliability levels of various models in real time, adaptively identifies the specific ship type, and flexibly adjusts navigation parameters accordingly to ensure navigation accuracy and safety. The algorithm employs a highly modular design, with functional modules closely interconnected and collaborating through efficient data flow to ultimately 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

[0153] 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.

[0154] Figure 1 This is a flowchart of an adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment, according to an embodiment of the present invention. Detailed Implementation

[0155] 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.

[0156] This invention discloses an adaptive heading angle filtering navigation method based on Global Navigation Satellite System (GNSS) trajectory quality assessment, the specific steps of which are as follows:

[0157] S1, Initialization Phase: First, the system needs to collect the ship's Global Navigation Satellite System (GNSS) measurement data and Inertial Measurement Unit (IMU) measurement data, and perform preliminary initialization processing on these GNSS position data to ensure the smooth progress of subsequent steps.

[0158] S2, Multi-model Establishment Stage: The system will evaluate the quality of the GNSS signal. If the evaluation value exceeds the preset threshold, it will construct multiple motion models, including the first large ship uniform motion model, the first large ship uniform acceleration motion model, the first small ship uniform motion model, and the first small ship uniform acceleration motion model. Conversely, if the evaluation value is lower than the preset threshold, it will construct the second large ship uniform motion model, the second large ship uniform acceleration motion model, the second small ship uniform motion model, and the second small ship uniform acceleration motion model.

[0159] S3, Prediction Phase: At the beginning of each sampling period, the interaction between the states of each model is first performed, the interaction probability is calculated, and then the mixed initial state of each model is calculated based on the probability. Using these mixed initial states, each model makes predictions according to its dynamic equations to ensure the accuracy and reliability of the prediction results.

[0160] S4, Measurement Update Phase: First, the GNSS measurement data is processed, then the IMU measurement data is processed, and the measurement information covariance is calculated based on these two types of measurement data, and then the state is updated to ensure the real-time performance and accuracy of the system.

[0161] S5, Model Probability Update and State Fusion Stage: Perform GNSS quality adaptation, update the probabilities of each model, then perform GNSS quality adjustment, introduce smoothing to avoid abrupt changes in model probabilities, and obtain the final state estimate through weighted fusion of all models to improve the overall performance of the system.

[0162] S6, Ship Type Identification and Parameter Adaptation Stage: Based on the probabilities of each model, calculate the ship type discrimination index and GNSS quality score, and adjust the adaptive parameters according to these indicators and scores. Dynamically adjust the model transition probability matrix to adapt to different ship types and environmental conditions.

[0163] S7, System Output and Anomaly Handling Stage: The fused state is smoothed, and anomalies are detected by calculating the normalized state deviation. Once an anomaly is detected, corresponding measures are taken, such as increasing the noise covariance of the corresponding measurement channel, reducing the probability weight of the anomaly model, or accumulating statistics on the duration of the anomaly, to ensure the stability and reliability of the system.

[0164] S8, Navigation Termination Phase: When predetermined conditions are met, the system will stop operating and end the entire navigation process.

[0165] In some embodiments, step S1 involves using GNSS measurement data, including latitude (Lat), longitude (Lon), and altitude (h). An initial location is selected as a reference point (Lat0, Lon0, h0), and a coordinate transformation is performed. This coordinate transformation consists of two steps:

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

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

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

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

[0170] 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.

[0171] Step 2: Convert the ECEF coordinates to local East-North-Up (ENU) coordinates. The conversion formula is as follows:

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

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

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

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

[0176] The transformed ENU coordinates will be used as the observations of the position components in the state vector. Next, the system state vector is constructed, which contains nine components: X = [e, n, u, v]. e ,v n ,v u ,a e ,a n ,a u ] T 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 a 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 component [a] e ,a n ,a u ] Obtained through inertial measurement unit (IMU), a e a represents the eastward acceleration obtained from the IMU accelerometer measurement after coordinate transformation. n a represents the northward acceleration obtained from the IMU accelerometer measurement after coordinate transformation. u The axial acceleration is the result of coordinate transformation of the IMU accelerometer measurement.

[0177] In some embodiments, step S2, evaluating the GNSS signal quality, includes:

[0178] (1) Distance Consistency Assessment:

[0179] Calculate the distance between adjacent GNSS points:

[0180] d i =sqrt((lon) i -lon {i-1} ) 2 +(lat i -lat {i-1} ) 2 )

[0181] Calculate the standard deviation of distance: std d =sqrt(Σ(d i -d mean ) 2 / (n-1))

[0182] Distance Consistency Score d =1-min(1,std d / MAX d ISTANCE STD )

[0183] Where: MAX d ISTANCE STD The distance to the standard deviation threshold is a fixed constant;

[0184] (2) Directional consistency assessment:

[0185] Calculate the direction change between adjacent GNSS points: θ i =atan2(lat i -lat {i-1} ,lon i -lon {i-1} )

[0186] Calculate the standard deviation of directional change: std θ =sqrt(Σ(θ) i -θ mean ) 2 / (n-1))

[0187] Directional consistency score: θ =1-min(1,std θ / MAX d IRECTION STD )

[0188] Where: MAX d IRECTION STD The standard deviation threshold for direction is a fixed constant.

[0189] (3) Speed ​​consistency assessment:

[0190] Calculate the velocity derived from GNSS points: v GNSS =di / Δt

[0191] Calculate the difference between the velocity and the NMEA velocity: Δv = |v GNSS -v nmea |

[0192] Speed ​​Consistency Score v =1-min(1,Δv / SPEED) d IFF THRESHOLD )

[0193] Among them: SPEED d IFF THRESHOLD The velocity difference threshold is a fixed constant.

[0194] Overall quality score:

[0195] quality score =WEIGHT d ISTANCE*score d +WEIGHT d IRECTION*score θ +WEIGHT SPEED *score v

[0196] Among them: WEIGHT d ISTANCE is the distance rating weight, which is a fixed constant. WEIGHT d IRECTION is the directional score weight, which is a fixed constant. WEIGHT SPEED The speed score weight is a fixed constant.

[0197] In some embodiments, in step S2, the eight models are executed in parallel, interacting using the IMM algorithm and dynamically adjusting their probabilities based on real-time observation data. In the initial phase of operation, the probabilities of these eight models are set to be equal, i.e., μ. i (0) = INIT_MODEL_PROB, which is a fixed constant with a value of 1 / 8, where μ i This represents the probability of the i-th model.

[0198] The Large-CV model of uniform motion of large ships is constructed based on the motion characteristics of large ships, reflecting the difficulty of steering and speed changes. Once a large ship reaches a stable speed, it tends to maintain a constant speed. When the GNSS signal quality assessment value exceeds a preset threshold, the first uniform motion model of the large ship is constructed, with the following state transition equation:

[0199]

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

[0201] The process noise matrix of the first large ship's uniform motion mode:

[0202]

[0203] Where: I is the identity matrix, τ is the system sampling period, and Q... L1G Let Q be the noise figure of the ship's position process. L2G The noise figure during the acceleration process of a large ship;

[0204] State transition equations for the first uniformly accelerated motion model of the large ship:

[0205]

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

[0207] The Large-Cyclic Acceleration Motion Model (Large-CA) aims to describe the dynamic changes of a large ship during acceleration changes. Considering the large inertia of a large ship, its acceleration changes relatively slowly. When the GNSS signal quality assessment value exceeds a preset threshold, the first large-ship uniform acceleration motion model is constructed, and its process noise matrix is ​​as follows:

[0208]

[0209] Where: I is the identity matrix, τ is the system sampling period, derived from the system clock; Q L3G The noise figure represents the process noise of the large ship's uniform acceleration model.

[0210] When the GNSS signal quality assessment value is lower than a preset threshold, a second large ship uniform motion model is constructed. Its state transition equation is the same as that of the second large ship uniform motion model, and its process noise matrix is:

[0211]

[0212] Where: I is the identity matrix, τ is the system sampling period, derived from the system clock; Q L1P Let Q be the noise figure of the ship's position process. L2P This represents the noise figure during the acceleration process of a large ship.

[0213] When the GNSS signal quality assessment value is lower than a preset threshold, a second large ship uniform acceleration motion model is constructed. Its state transition equation is the same as that of the first large ship uniform acceleration motion model, and its process noise matrix is:

[0214]

[0215] Where: I is the identity matrix, τ is the system sampling period, derived from the system clock; Q L3P The noise figure for the uniform acceleration model of a large ship;

[0216] 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. The state transition equation of the first small boat uniform motion model is the same as that of the first large ship uniform motion model, and its process noise matrix is:

[0217]

[0218] Where: I is the identity matrix, τ is the system sampling period, derived from the system clock; Q S1G Q is the noise figure during the boat's position process. S2G This represents the noise figure during the acceleration process of the small boat.

[0219] The Small-Cyclic Acceleration (Small-CA) model aims to describe the dynamic behavior of a small boat during rapid maneuvers. The state transition equations for the first small-boat uniform-acceleration model are the same as those for the first large-boat uniform-acceleration model, and its process noise matrix is:

[0220]

[0221] Where: I is the identity matrix, τ is the system sampling period, derived from the system clock; Q S3G The noise figure represents the process noise of the small boat's uniform acceleration model.

[0222] The state transition equation for the second boat's uniform motion model is the same as that for the first boat's uniform motion model, and its process noise matrix is:

[0223]

[0224] Where: I is the identity matrix, τ is the system sampling period, derived from the system clock; Q S1P Q is the noise figure during the boat's position process. S2P This represents the noise figure during the acceleration process of the small boat.

[0225] The state transition equation for the second boat's uniformly accelerated motion model is the same as that for the first boat's uniformly accelerated motion model, and its process noise matrix is:

[0226]

[0227] Where: I is the identity matrix, τ is the system sampling period, derived from the system clock; Q S3P The noise figure represents the process noise of the small boat's uniform acceleration model.

[0228] The extended model transition probability matrix is:

[0229]

[0230] Where: P ii To maintain the probability of the model itself, a constant greater than 0.9, P ij (i≠j) represents the transition probability from model i to model j, which satisfies the following for any model i: ΣP ij =1 (j=1,2,...,8).

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

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

[0233] Where: μ ij π represents the probability of switching from model i to model j at time k. ij μ represents the model transition probability. i (k-1): The probability of model i at time k-1, c j Here, cj is the normalization factor. The normalization factor cj is the sum of the products of all probabilities of switching from different models to model j, i.e., c j =∑ i (π ij ×μ i (k-1)). In the formula, by dividing by the normalization factor c j This ensures that the sum of the probabilities of switching from different models to model j is 1.

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

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

[0236] Among them, X 0j : The mixed initial state of model j, X i (k-1): The state of model i at time k-1, μ ij X is the probability of switching from model i to model j at time k. In the formula, X... i (k-1)×μ ij The mixed initial state X of model j is obtained by summing the product of the state of model i at time k-1 and the probability of switching from model i to model j over all possible models i. 0j This method considers all possible model switching and can more accurately reflect the initial state of the system at time k.

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

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

[0239] P i (k|k-1)=F i *P 0i (k-1|k-1)*F i T +Q i

[0240] Where: X i (k-1) is the state estimate of the i-th model at time k-1, X 0i (k-1|k-1): The initial mixed state of the i-th model at time k-1, containing information about all possible model switching; P i (k-1|k-1) is the covariance matrix of the i-th model at time k-1, P 0i (k-1|k-1) is the initial covariance matrix of the i-th model at time k-1; F i Let Q be the state transition matrix of the i-th model; i Let be the process noise matrix of the i-th model, which describes the random disturbances in the process.

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

[0242] v i (k)=Z GNSS (k)-H GNSS *X i (k|k-1)

[0243] 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, v i (k) is the information sequence, reflecting the difference between the predicted and measured values;

[0244] 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 ,

[0245] Where: a bC 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.

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

[0247] (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.

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

[0249]

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

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

[0252]

[0253] Where Δt is the sampling period.

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

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

[0256]

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

[0258]

[0259] 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.

[0260] The calculated measurement information covariance is:

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

[0262] Where 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.

[0263] The status is updated as follows:

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

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

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

[0267] Among them, K i (k): Kalman gain of the i-th model at time k, P i (k|k-1): The prediction error covariance matrix of the i-th model at time k, H: The observation matrix, H T It is the transpose of the observation matrix, S i (k) -1 X is the inverse matrix of the measurement innovation covariance of the i-th model at time k. i (k|k): The update state of the i-th model at time k, X i (k|k-1): The predicted state of the i-th model at time k, v i (k): The information sequence of the i-th model at time k, P i (k|k): The update error covariance matrix of the i-th model at time k, I: the identity matrix.

[0268] In some embodiments, in step S5, the GNSS quality adaptive method is:

[0269] quality factor = sigmoid(quality) score ,GNSS_SIGMOID_K,GNSS THRESHOLD)

[0270] Where: sigmoid(x,k,t)=1 / (1+exp(-k*(xt))) is a sigmoid function, quality factor For GNSS quality factor, quality score For GNSS trajectory quality scoring, GNSS_SIGMOID_K is the steepness parameter of the sigmoid function, which is a fixed constant. THRESHOLD The quality judgment threshold is a fixed constant.

[0271] When quality score =GNSS THRESHOLD At that time, quality factor =0.5 indicates that the quality score is just around the threshold, and the fitness level is moderate.

[0272] When quality score >>GNSS THRESHOLD At that time, quality factor A score close to 1 indicates a quality score far above the threshold and a high degree of adaptability.

[0273] When quality score < <GNSS THRESHOLD At that time, quality factor A score close to 0 indicates that the quality score is far below the threshold and the degree of adaptation is low.

[0274] The probability of updating each model is: μ i (k)=(L i (k)*μ i (k-1)) / c,

[0275] Where: L i (k) represents the likelihood of the i-th model, indicating the model's explanatory power for the current observations, μ i (k): 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)).

[0276] The GNSS quality was adjusted as follows:

[0277] For each model i:

[0278] If i is the first model: μ iadjusted (k)=μ i (k)*qualityfactor

[0279] If i is the second model: μ iadjusted (k)=μ i (k)*(1-quality factor );

[0280] Where, μ iadjusted (k): The probability of the adjusted i-th model at time k, μ i (k): The probability of the i-th model at time k, quality factor GNSS quality factor. This formula is used to calculate the GNSS quality factor. factor Adjust the probabilities of each model. For the first model, multiply its probability by the quality factor. factor This means that the probability of the first model increases when the GNSS quality is high. For the second model, its probability is multiplied by 1 - quality. factor This indicates that the probability of the second model decreases when the GNSS quality is high. This method can dynamically adjust the model probability to adapt to changes in the quality of the GNSS signal.

[0281] The probability smoothing process is as follows:

[0282] μ ismooth (k)=SMOOTH factor *μ iadjusted (k)+(1-SMOOTH factor )*μ ismooth (k-1);

[0283] Where, μ ismooth (k): The smoothed probability of the i-th model at time k, μ iadjusted (k): The adjusted probability of the i-th model at time k, μ ismooth (k-1): The smoothed probability of the i-th model at the previous time k-1, SMOOTH factor Smoothing coefficient.

[0284] This formula is used to smooth the model probabilities, reducing probability fluctuations and improving system stability. In the formula, SMOOTH... factor ×μ iadjusted (k) represents the contribution of the adjusted probability at the current time, (1-SMOOTH) factor )×μ ismooth (k-1) represents the contribution of the smoothed probability from the previous time step. The smoothing coefficient is SMOOTH. factor This determines the degree to which the adjusted probability at the current moment affects the smoothed probability. When SMOOTH... factorWhen the value of SMOOTH is larger, the impact of the adjusted probability at the current time on the smoothed probability is greater, and the smoothing effect is weaker; conversely, when SMOOTH is smaller... factor The smaller the value, the greater the influence of the smoothed probability from the previous time step on the smoothed probability, and the stronger the smoothing effect. Smoothing coefficient (SMOOTH) factor The settings are usually based on the dynamic characteristics of the system to achieve the best smoothing effect.

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

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

[0287] 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. In the formula, the state estimate X of 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.

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

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

[0290] 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 dX is 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 dX between the model state and the fused state i Rather than 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.

[0291] In some embodiments, in step S6, the ship type is determined:

[0292] Ship score =ln((Σμ large (k)) / (Σμ small (k)))

[0293] Ship score Σμ is a ship type identification index. large (k) and Σμ small (k) represents the total probability of the large ship model and the small ship model, respectively. This formula calculates the ratio of the total probability of the large ship model to the total probability of the small ship model, and then takes the natural logarithm to obtain the ship type discrimination index, Ship. score 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.

[0294] The GNSS quality determination is as follows:

[0295] GNSS score =ln((Σμ good (k)) / (Σμ poor (k)))

[0296] Among them, GNSS score GNSS quality rating, Σμ good (k) and Σμ poor (k) represents the total probabilities of the good signal model and the bad signal model, respectively; this formula obtains the GNSS quality score by calculating the ratio of the total probability of the good signal model to the total probability of the bad signal model, and then taking the natural logarithm. score GNSS score The value can be used to determine the quality of GNSS signals: if GNSS score A positive value indicates good signal quality; if GNSS score A negative value indicates poor signal quality.

[0297] The parameters are adaptively defined as follows:

[0298] Q new =Q base *(1+SHIP_ADAPT factor *Ship score )*(1+GNSS_ADAPT factor *GNSS score ),

[0299] Among them, Q new For the new adaptive parameters, Q base Based on the parameter SHIP_ADAPT factor As a ship adaptation factor, Ship score GNSS_ADAPT is used to rate ships. factor As the adaptation factor for Global Navigation Satellite Systems, GNSS score Scoring of global navigation satellite systems. This formula uses the fundamental parameter Q... base The new adaptive parameter Q is calculated by multiplying the adaptation factors and scores of the ship and the global navigation satellite system. new The product of the adaptation factor and the score plus 1 represents the adjustment magnitude to the baseline parameter.

[0300] The transition probability is adaptively defined as follows:

[0301] π new = (1-ADAPT_RATE)*π base +ADAPT_RATE*π adapted .

[0302] Where, π base It is the basic transition probability, π adapted ADAPT_RATE is the transition probability after adaptation, and ADAPT_RATE is the fitness rate. This formula is derived by applying the base transition probability π. base and the transition probability π after adaptation adapted The new transition probability π is calculated by weighting the values ​​according to the fitness rate ADAPT_RATE. new The higher the fitness rate (ADAPT_RATE), the closer the new transition probability is to the adapted transition probability (π). adapted .

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

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

[0305] Where λ is the smoothing coefficient, determined based on the system's dynamic characteristics; Xoutput(k): the output state at the current moment, X fusion (k): The fusion state at the current moment, X output (k-1): The output state at the previous time step. This formula uses the fused state X at the current time step. fusion (k) and the output state X at the previous time step output The difference between (k-1) is multiplied by the smoothing coefficient λ, and then added to the fused state at the current time step to calculate the output state X at the current time step. output (k). The smoothing coefficient λ determines the degree of influence of the output state at the previous time step on the output state at the current time step. When λ is larger, the output state at the previous time step has a greater influence on the output state at the current time step, and the smoothing effect is more obvious.

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

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

[0308] Where D(k): the normalized state deviation at the current time, X fusion (k): The fusion state at the current moment, X predict (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 fused state X at the current time. fusion (k) and predicted state X predict The normalized state deviation D(k) is obtained by dividing the absolute value of the difference between σ(k) and σ(k) by the prediction uncertainty. 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.

[0309] If the normalized state deviation D(k) exceeds a preset threshold, the system will trigger anomaly handling mechanisms, such as recalibrating the sensors or activating a backup navigation system to ensure navigation accuracy and reliability. Furthermore, the anomaly detection module records anomaly events, including the time of occurrence, the type of anomaly, and the system state at the time of the anomaly, for subsequent fault analysis and system optimization. In this way, the method not only improves the robustness of the navigation system but also provides valuable data support for system maintenance and optimization.

[0310] In some specific embodiments, the predetermined conditions set for step S8 cover the following aspects: First, the execution of this step will be triggered when the system receives an explicit stop command; second, if the Global Navigation Satellite System (GNSS) signal is completely lost, and this loss continues for more than a preset duration standard, step S8 will also be triggered; third, if the data from the Inertial Measurement Unit (IMU) is abnormal, and this abnormal state lasts for more than a preset threshold range, this step will also be triggered; finally, the execution of step S8 will also be triggered when the cumulative number of times the system performs reinitialization exceeds a preset maximum allowable value. When any one or more of the above conditions are met simultaneously, step S8 will be activated and the corresponding operation will be performed.

[0311] 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. An adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment, 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: The GNSS signal quality is evaluated. When the evaluation value is greater than the preset value, the first large ship uniform motion model, the first large ship uniform acceleration motion model, the first small ship uniform motion model, and the first small ship uniform acceleration motion model are established. When the evaluation value is less than the preset value, the second large ship uniform motion model, the second large ship uniform acceleration motion model, the second small ship uniform motion model, and the second small ship uniform acceleration motion model are established. 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, then calculate the measurement information covariance based on the two types of measurement data, and then perform state update; S5, Model Probability Update and State Fusion Stage: Perform GNSS quality adaptation, update the probabilities of each model, then perform GNSS quality adjustment, 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 GNSS quality score, and make adaptive parameter adjustments according to the index and GNSS quality score, and 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 adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment 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 adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment according to claim 1, characterized in that, Step S2, evaluating the GNSS signal quality, includes: (1) Distance Consistency Assessment: Calculate the distance between adjacent GNSS points: d i =sqrt((lon i -lon {i-1} )²+(years i -years {i-1} )²) Calculate the standard deviation of distance: std d =sqrt(Σ(d i -d mean )² / (n-1)) Distance Consistency Score d =1-min(1,std d / MAX d ISTANCE STD ) Where: MAX d ISTANCE STD The distance to the standard deviation threshold is a fixed constant; (2) Assessment of directional consistency: Calculate the direction change between adjacent GNSS points: θ i =atan2(lat i -lat {i-1} ,lon i -lon {i-1} ) Calculate the standard deviation of directional change: std θ =sqrt(Σ(θ i -θ mean )² / (n-1)) Directional consistency score: θ =1-min(1,std θ / MAX d IRECTION STD ) Where: MAX d IRECTION STD The standard deviation threshold for direction is a fixed constant. (3) Speed ​​consistency assessment: Calculate the velocity derived from GNSS points: v GNSS =d i / Δt Calculate the difference between the velocity and the NMEA velocity: Δv = |v GNSS -v nmea | Speed ​​Consistency Score v =1-min(1,Δv / SPEED d IFF THRESHOLD ) Among them: SPEED d IFF THRESHOLD The velocity difference threshold is a fixed constant. Overall quality score: quality score =WEIGHT d ISTANCE*score d +WEIGHT d IRECTION*score θ +WEIGHT SPEED *score v Among them: WEIGHT d ISTANCE is the distance rating weight, which is a fixed constant. WEIGHT d IRECTION is the directional score weight, which is a fixed constant. WEIGHT SPEED The speed score weight is a fixed constant.

4. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment according to claim 1, characterized in that, In step S2, In step S2, eight models are run simultaneously and interact using the IMM algorithm. Their probabilities are dynamically adjusted based on actual observation data. Initially, the probabilities of all eight models are set to be equal, at a value of μ. i (0) = INIT_MODEL_PROB, a fixed constant with a value of 1 / 8, where μ i This represents the probability of the i-th model; State transition equation for the uniform motion mode of the first large ship: The process noise matrix of the first large ship's uniform motion mode: Where: I is the identity matrix, τ is the system sampling period, and Q... L1G Let Q be the noise figure of the ship's position process. L2G The noise figure during the acceleration process of a large ship; State transition equations for the first uniformly accelerated motion model of the large ship: The process noise matrix of the first large ship's uniformly accelerated motion model: Among them: Q L3G The noise figure for the uniform acceleration model of a large ship; The state transition equation for the uniform motion model of the second large ship is the same as that for the uniform motion model of the second large ship, and its process noise matrix is: Among them: Q L1P Let Q be the noise figure of the ship's position process. L2P The noise figure during the acceleration process of a large ship; The state transition equations for the second largest ship's uniformly accelerated motion model are the same as those for the first largest ship's uniformly accelerated motion model, and its process noise matrix is: Among them: Q L3P The noise figure for the uniform acceleration model of a large ship; The state transition equations for the uniform motion model of the first small boat are the same as those for the uniform motion model of the first large boat. Its process noise matrix: Among them: Q S1G Q is the noise figure during the boat's position process. S2G The noise figure during the acceleration process of the small boat; The state transition equation for the uniformly accelerated motion model of the first small boat is the same as that of the uniformly accelerated motion model of the first large boat, and its process noise matrix is: Among them: Q S3G The noise figure for the uniform acceleration model of the small boat; The state transition equation for the second boat's uniform motion model is the same as that for the first boat's uniform motion model, and its process noise matrix is: Among them: Q S1P Q is the noise figure during the boat's position process. S2P The noise figure during the acceleration process of the small boat; The state transition equation for the second boat's uniformly accelerated motion model is the same as that for the first boat's uniformly accelerated motion model, and its process noise matrix is: Among them: Q S3P The noise figure for the uniform acceleration model of the small boat; The extended model transition probability matrix is: Where: P ii To maintain the probability of the model itself, a constant greater than 0.9, P ij Let ΣP be the transition probability from model i to model j. For any model i, ΣP satisfies: ij =1, i≠j, j=1,2,...,8.

5. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment 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 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)*m ij ); 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) P i (k|k-1)=F i *P 0i (k-1|k-1)*F i T +Q i Where: X i (k-1) is the state estimate of the i-th model at time k-1, X 0i (k-1|k-1) represents the initial mixed state of the i-th model at time k-1, P 0i (k-1|k-1) is the initial covariance matrix of the i-th model at time k-1, P i (k-1|k-1) is the covariance matrix of the i-th model at time k-1. F i Let be the state transition matrix of the i-th model. Q i Let be the process noise matrix of the i-th model.

6. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment 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, v 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 in the navigation coordinate system. 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 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. Status Update: K i (k)=P i (k|k-1)*H T *S i (k) -1 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) Where: 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 model at time k, H is the observation matrix, and X i (k|k) represents the update state of the i-th model at time k, X i (k|k-1) represents the predicted state of the i-th model at time k, v i (k) is the information sequence of the i-th model at time k, P i (k|k) is the update error covariance matrix of the i-th model at time k, and I is the identity matrix.

7. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment according to claim 1, characterized in that, In step S5, GNSS quality is adaptively adjusted: quality factor =sigmoid(quality score ,GNSS_SIGMOID_K,GNSS THRESHOLD ) Where: sigmoid(x,k,t)=1 / (1+exp(-k*(xt))) is a sigmoid function. quality score To score the quality of GNSS tracks, GNSS_SIGMOID_K is the steepness parameter of the sigmoid function, which is a fixed constant. GNSS THRESHOLD The quality judgment threshold is a fixed constant. When quality score = GNSS THRESHOLD then quality factor = 0.5, When quality score >>GNSS THRESHOLD At that time, quality factor Approaching 1 When quality score < <GNSS THRESHOLD At that time, quality factor Approaching 0; 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)); GNSS quality adjustment: For each model i: If i is the first model: μ iadjusted (k)=μ i (k)*quality factor If i is the second model: μ iadjusted (k)=μ i (k)*(1-quality factor ); Probability smoothing: μ ismooth (k)=SMOOTH factor *μ iadjusted (k)+(1-SMOOTH factor )*μ ismooth (k-1); Among them, SMOOTH factor The smoothing coefficient is set based on the system's dynamic characteristics; The final state estimate obtained through the weighted fusion of all models is as follows: X fusion (k)=Σ(μ ismooth (k)*X i (k|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, X i (k|k) is the state estimate of the i-th model at time k, dX i dX is the difference between each model state and the fused state. i =X i (k|k)-X fusion (k), where Pi(k|k) is the state covariance of the i-th model at the current time k.

8. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment according to claim 1, characterized in that, In step S6, ship type determination: Ship score =ln((Sm large (k)) / (Sm small (k))) Where Σμ large (k) and Σμ small (k) represents the total probability of the large ship and small ship models, respectively; GNSS quality assessment: GNSS score =ln((Sm good (k)) / (Sm poor (k))) Where Σμ good (k) and Σμ poor (k) represent the total probabilities of the good signal and bad signal models, respectively; parameter Adaptive: Q new =Q base *(1+SHIP_ADAPT factor *Ship score )*(1+GNSS_ADAPT factor *GNSS score ); where Q new For the new adaptive parameters, Q base Based on the parameter SHIP_ADAPT factor As a ship adaptation factor, Ship score GNSS_ADAPT is used to rate ships. factor As the adaptation factor for Global Navigation Satellite Systems, GNSS score Rate the global navigation satellite system; Adaptive transition probability: π new =(1-ADAPT_RATE)*π base +ADAPT_RATE*π adapted , where π base It is the basic transition probability, π adapted ADAPT_RATE is the transition probability after adaptation, and ADAPT_RATE is the fitness rate.

9. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment 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)) Where λ is the smoothing coefficient, determined based on the system's dynamic characteristics, 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; The anomaly detection is performed by calculating the normalized state deviation. D(k)=|X fusion (k)-X predict (k)| / σ(k) Among them, X fusion (k) represents the fusion state at the current moment, X predict σ(k) represents the predicted state at the previous time step, and σ(k) represents the prediction uncertainty, which comes from the diagonal elements of the state covariance matrix.

10. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment 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.

Images