Self-adaptive course angle filtering navigation method based on GNSS trajectory quality evaluation

Abstract

The invention discloses a self-adaptive course angle filtering navigation method based on GNSS (Global Navigation Satellite System) trajectory quality evaluation, which comprises the following steps: S1, an initialization stage; S2, a multi-model establishment stage: evaluating GNSS signal quality, and when an evaluation value is greater than a preset value, executing the multi-model establishment stage; establishing a first large ship uniform-speed motion model, a first large ship uniform-acceleration motion model, a first small ship uniform-speed motion model and a first small ship uniform-acceleration motion model; when the evaluation value is smaller than the preset value, establishing a second large ship uniform speed motion model, a second large ship uniform acceleration motion model, a second small ship uniform speed motion model and a second small ship uniform acceleration motion model; s3, a prediction stage; S4, a measurement updating stage; S5, a model probability updating and state fusion stage; S6, a ship type identification and parameter self-adaption stage; according to the method, the credibility of different models is evaluated in real time, the ship type is recognized in a self-adaptive mode, and navigation parameters are adjusted.

Description

Technical Field

[0001] The invention relates to an adaptive heading angle filtering navigation method based on GNSS trajectory quality evaluation, belonging to the technical field of ship navigation. Background Art

[0002] In the field of ship navigation, the key to ensuring navigation safety lies in obtaining high-precision position, velocity and attitude information. However, there are many problems with traditional single navigation solutions that may have a negative impact on navigation safety. First, navigation solutions that rely solely on the Global Navigation Satellite System (GNSS) are susceptible to signal shielding and multipath effects. Signal shielding may be caused by tall buildings, bridges or other obstacles, while multipath effects are errors caused by reflection and refraction of signals on different paths. Second, navigation solutions that rely solely on inertial measurement units (IMUs) have the problem of error accumulation. IMUs calculate position and attitude by measuring acceleration and angular velocity, but due to the inherent errors of their internal sensors, these errors will gradually accumulate over time, resulting in reduced navigation accuracy. Third, traditional integrated navigation algorithms often cannot adapt to the motion characteristics of different types of ships. The motion characteristics of each ship are different. For example, there are significant differences in the dynamic characteristics of container ships, tankers and fishing boats when sailing. Existing integrated navigation algorithms often cannot fully consider these differences, which affects navigation accuracy. Finally, existing navigation algorithms lack the ability to adaptively identify ship characteristics. Different ship types have differences in design and structure, which can affect the motion characteristics of ships. However, existing navigation algorithms often cannot automatically identify and adapt to these ship characteristics, resulting in reduced navigation accuracy and reliability. In order 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] The present invention provides an adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment, which can effectively solve the above problems.

[0004] The present invention is achieved in that:

[0005] An adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment comprises the following steps:

[0006] S1, initialization phase: obtaining the ship's GNSS measurement data and IMU measurement data, and initializing the GNSS position data;

[0007] S2, multi-model establishment stage: evaluate the GNSS signal quality, and when the evaluation value is greater than the preset value, establish the first large ship uniform motion model, the first large ship uniform acceleration motion model, the first small boat uniform motion model, and the first small boat uniform acceleration motion model; when the evaluation value is less than the preset value, establish the second large ship uniform motion model, the second large ship uniform acceleration motion model, the second small boat uniform motion model, and the second small boat uniform acceleration motion model;

[0008] S3, prediction stage: at the beginning of each sampling period, the states of the models are interacted with each other, the interaction probability is calculated, and then the mixed initial state of each model is calculated based on the interaction probability. Based on the mixed initial state, each model uses its own dynamic equation for prediction;

[0009] S4, measurement update phase: first process the GNSS measurement data, then process the IMU measurement data, then calculate the measurement information covariance based on the two measurement data, and then perform status update;

[0010] S5, model probability update and state fusion stage: perform GNSS quality adaptation, update the probability of each model, then perform GNSS quality adjustment, introduce smoothing processing to avoid model probability mutation, and obtain the final state estimation through weighted fusion of all models;

[0011] S6, ship type identification and parameter adaptation stage: based on the probability of each model, the ship type identification index and GNSS quality score are calculated, and adaptive parameters are adjusted according to the index and GNSS quality score, and the model transfer probability matrix is ​​dynamically adjusted;

[0012] S7, system output and exception processing stage: smooth the fusion state, perform anomaly detection 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 accumulate statistics on the duration of the anomaly;

[0013] S8, navigation termination phase: terminate the operation when the predetermined conditions are met.

[0014] In some embodiments, in step S1, the GNSS measurement data includes latitude (Lat), longitude (Lon) and altitude (h), and the initial position is selected as the reference point (Lat0, Lon0, h0) to perform coordinate transformation, and the coordinate transformation is divided into the following two steps:

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

[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 circle: N = a / sqrt(1-e 2 sin 2 (Lat)), a is the major semi-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:

[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) is the ECEF coordinate of the reference point;

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

[0026] Create a system state vector, which consists of nine components:

[0027] X=[e,n,u,v e ,v n ,v u ,a e ,a n ,a u ] T , where the position component [e, n, u] is obtained from the GNSS receiver and represents the position coordinates of the carrier in the northeast celestial coordinate system: e is the east position provided by GNSS, n is the north position provided by GNSS, and u is the celestial position provided by GNSS;

[0028] Velocity component [v e ,v n ,v u ] obtained from the GNSS receiver, v e is the eastward velocity measured by GNSS, v n is the north velocity measured by GNSS, vu is the celestial velocity measured by GNSS;

[0029] Acceleration component [a e ,a n ,a u ] obtained through IMU measurement, a e : The eastward acceleration after coordinate conversion of the IMU accelerometer measurement, a n : North acceleration after coordinate conversion of IMU accelerometer measurement, a u : Celestial acceleration measured by the IMU accelerometer after coordinate conversion.

[0030] In some embodiments, in 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 distance standard deviation: std d =sqrt(Σ(d i -d mean ) 2 / (n-1))

[0035] Distance consistency score: score d =1-min(1,std d / MAX d ISTANCE STD )

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

[0037] (2) Direction 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 direction changes: std θ =sqrt(Σ(θ i -θ mean ) 2 / (n-1))

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

[0041] Where: MAX d IRECTION STD is the direction standard deviation threshold, which is a fixed constant;

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

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

[0044] Calculate the difference from NMEA speed: Δv = |v GNSS -v nmea |

[0045] Speed ​​consistency score: score v =1-min(1,Δv / SPEED d IFF THRESHOLD )

[0046] Among them: SPEED d IFF THRESHOLD is the speed difference threshold, which is a fixed constant;

[0047] Overall quality rating:

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

[0049] +WEIGHT SPEED *score v

[0050] Where: WEIGHT d ISTANCE is the distance score weight, which is a fixed constant, WEIGHT d IRECTION is the direction score weight, which is a fixed constant, WEIGHTSPEED is the speed score weight, which is a fixed constant.

[0051] In some embodiments, in step S2,

[0052] In step S2, eight models are run simultaneously, interacting through the IMM algorithm, and their respective probabilities are dynamically adjusted according to the actual observed data. In the initial operation, the probabilities of the eight models are set to be equal, which is μ i (0) = INIT_MODEL_PROB, fixed constant, value is 1 / 8, where μ i represents the probability of the i-th model;

[0053] The state transfer equation of the first large ship uniform motion model:

[0054]

[0055] The process noise matrix of the first large ship uniform motion model:

[0056]

[0057] Where: I is the 3×3 unit matrix, τ is the system sampling period, Q L1G is the noise coefficient of the large ship position process, Q L2G is the noise coefficient of the ship acceleration process;

[0058] The state transfer equation of the first large ship uniform acceleration motion model:

[0059]

[0060] The process noise matrix of the first large ship uniform acceleration motion model:

[0061]

[0062] Where: Q L3G is the noise coefficient of the large ship uniform acceleration model process;

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

[0064] Where: Q L1P is the noise coefficient of the large ship position process, Q L2P is the noise coefficient of the ship acceleration process;

[0065] The state transfer equation of the second large ship uniform acceleration motion model is the same as that of the first large ship uniform acceleration motion model, and its process noise matrix is:

[0066] Where: Q L3Pis the noise coefficient of the large ship uniform acceleration model process;

[0067] The state transfer equation of the first small boat uniform motion model is the same as that of the first large boat uniform motion model.

[0068] Its process noise matrix:

[0069] Where: Q S1G is the noise coefficient of the boat position process, Q S2G is the noise coefficient of the boat acceleration process;

[0070] The state transfer equation of the first small boat uniform acceleration motion model is the same as that of the first large ship uniform acceleration motion model, and its process noise matrix is:

[0071] Where: Q S3G is the noise coefficient of the boat uniform acceleration model process;

[0072] The state transfer equation of the second boat uniform motion model is the same as that of the first boat uniform motion model.

[0073]

[0074] Process noise matrix:

[0075] Where: Q S1P is the noise coefficient of the boat position process, Q S2P is the noise coefficient of the boat acceleration process;

[0076] The state transfer equation of the second boat uniform acceleration motion model is the same as that of the first boat uniform acceleration motion model, and its process noise matrix is:

[0077] Where: Q S3P is the noise coefficient of the boat uniform acceleration model process;

[0078] The extended model conversion probability matrix is:

[0079]

[0080] Where: P ii Keep the probability for the model itself, a fixed constant greater than 0.9, P ij (i≠j) is the transition probability from model i to model j. For any model i, it satisfies: ΣP ij =1(j=1,2,...,8).

[0081] In some embodiments, in step S3, the calculation formula of 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 is the model transition probability, c j is the normalization factor;

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

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

[0086] Each model uses its own kinetic equation for prediction, the prediction formula is:

[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) is 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 is the state transfer matrix of the i-th model,

[0092] Q i is the process noise matrix of the ith 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) is the GNSS measurement value, including position and velocity information, H GNSS is the GNSS measurement matrix, indicating which state quantities can be directly observed by GNSS, v i (k) is the new information sequence, reflecting the difference between the predicted value and the measured value;

[0096] The processing of IMU measurement data is as follows: IMU measurement data is acceleration and angular velocity data in the carrier coordinate system, and coordinate conversion is performed: a n =C b ^n*a b +g n ,

[0097] Among them: a b is the acceleration in the carrier coordinate system measured by IMU, C b ^n is the transformation matrix from the carrier coordinate system to the navigation coordinate system, obtained by IMU attitude solution, g n is the local gravity acceleration vector, calculated from latitude and altitude;

[0098] The calculated measurement innovation covariance is:

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

[0100] Where R is the measurement noise covariance matrix, including: GNSS position measurement noise, which comes from the accuracy index provided by the GNSS receiver, GNSS velocity measurement noise, which comes from the accuracy index provided by the GNSS receiver, and IMU acceleration measurement noise, which comes from the IMU device index;

[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 is the GNSS observation matrix,

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

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

[0109] a b is the load system acceleration measured by IMU,

[0110] g n is the gravity acceleration vector in the navigation coordinate system,

[0111] a n is the acceleration in the navigation coordinate system.

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

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

[0114] Among them: sigmoid(x,k,t)=1 / (1+exp(-k*(xt))) is the S-type function,

[0115] quality score Score the quality of GNSS trajectories,

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

[0117] GNSS THRESHOLD is the quality judgment threshold, is a fixed constant,

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

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

[0120] When quality score < <GNSS THRESHOLDWhen, 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) is the likelihood of the i-th model, which indicates the model’s ability to explain the current observation, μ i (k-1) is the model probability at the previous moment, c is the normalization factor, ensuring that the sum of all model probabilities is 1, and by summing we get: 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 estimation obtained by weighted fusion of all models is:

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

[0131] The fused state covariance takes into account the state differences between models:

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

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

[0134] In some embodiments, in step S6, ship type determination:

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

[0136] where Σμ large (k) and Σμ small (k) are the total probabilities of the large ship and small ship models respectively;

[0137] GNSS quality determination:

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

[0139] where Σμ good (k) and Σμ poor (k) are the total probabilities of 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] Transition probability adaptation:

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

[0144] In some embodiments, in step S7, the system output is smoothing the fusion state:

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

[0146] Among them, λ 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 condition is one or more of: receiving a system stop command, complete loss of GNSS signal exceeding a preset time, IMU data anomaly continuing to exceed a threshold, or system reinitialization times exceeding a set value.

[0151] The beneficial effects of the present invention are:

[0152] The present invention dynamically evaluates the credibility level of each model in real time, adaptively identifies the specific model of the ship, and flexibly adjusts the navigation parameters accordingly to ensure the accuracy and safety of navigation. The algorithm adopts a highly modular design concept. The various functional modules are closely interconnected and interoperable through efficient data flows, and work together to eventually build a complete and efficient adaptive filtering system that can respond to changes in the external environment in real time and optimize navigation performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0153] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0154] Figure 1 The present invention 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 DESCRIPTION

[0155] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work 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 drawings is not intended to limit the scope of the invention claimed for protection, but merely represents the selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0156] The embodiment of the present invention discloses an adaptive heading angle filtering navigation method based on global navigation satellite system (GNSS) trajectory quality assessment, and the specific steps 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, multiple motion models including the first large ship uniform motion model, the first large ship uniform acceleration motion model, the first small boat uniform motion model and the first small boat uniform acceleration motion model are constructed; conversely, if the evaluation value is lower than the preset threshold, the second large ship uniform motion model, the second large ship uniform acceleration motion model, the second small boat uniform motion model and the second small boat uniform acceleration motion model are constructed.

[0159] S3, prediction stage: At the beginning of each sampling period, the states of each model are first interacted, 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 is predicted according to its dynamic equation to ensure the accuracy and reliability of the prediction results.

[0160] S4, measurement update phase: First, the GNSS measurement data is processed, and then the IMU measurement data is processed, and the measurement information covariance is calculated based on these two measurement data, and then the status is updated to ensure the real-time and accuracy of the system.

[0161] S5, model probability update and state fusion stage: perform GNSS quality adaptation, update the probability of each model, then perform GNSS quality adjustment, introduce smoothing processing to avoid sudden changes in model probability, 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 probability of each model, the ship type discrimination index and GNSS quality score are calculated, and adaptive parameters are adjusted according to these indicators and scores, and the model transfer probability matrix is ​​dynamically adjusted to adapt to different ship types and environmental conditions.

[0163] S7, system output and exception handling stage: smooth the fusion state and perform anomaly detection by calculating the normalized state deviation. Once an anomaly is detected, take corresponding measures, 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 the predetermined conditions are met, the system will stop running 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 position is selected as a reference point (Lat0, Lon0, h0) and a coordinate transformation is performed. The coordinate transformation is divided into two steps:

[0166] Step 1: Convert geodetic coordinates (Lat, Lon, h) to Earth-centered Earth-fixed coordinates (ECEF). 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 circle, and the calculation formula is: N = a / sqrt(1-e 2 sin 2 (Lat)), a is the major semi-axis of the ellipsoid, and e is the eccentricity of the ellipsoid.

[0171] Step 2: Convert 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] Among them, (X0, Y0, Z0) is the ECEF coordinate of the reference point.

[0176] The converted ENU coordinates will be used as the observed values ​​of the position component in the state vector. Next, the system state vector is established, which contains nine components: X = [e, n, u, v e ,v n ,v u ,a e ,a n ,a u ] T Among them, the position component [e, n, u] is obtained from the GNSS receiver and represents the position coordinates of the carrier in the northeast celestial coordinate system: e is the east position provided by GNSS, n is the north position provided by GNSS, and u is the celestial position provided by GNSS. The velocity component [v e ,v n ,v u ] is also obtained from the GNSS receiver, v e is the eastward velocity measured by GNSS, v n is the north velocity measured by GNSS, v u is the celestial velocity measured by GNSS. e ,a n ,a u ] is obtained by measuring the inertial measurement unit (IMU), a e is the eastward acceleration after coordinate conversion of the IMU accelerometer measurement value, a n is the northward acceleration after coordinate conversion of the IMU accelerometer measurement value, a u It is the celestial acceleration after coordinate conversion of the IMU accelerometer measurement value.

[0177] In some embodiments, in 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 distance standard deviation: std d =sqrt(Σ(d i -d mean ) 2 / (n-1))

[0182] Distance consistency score: score d =1-min(1,std d / MAX d ISTANCE STD )

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

[0184] (2) Direction 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 direction changes: std θ =sqrt(Σ(θ i -θ mean ) 2 / (n-1))

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

[0188] Where: MAX d IRECTION STD is the direction standard deviation threshold, which is a fixed constant;

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

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

[0191] Calculate the difference from NMEA speed: Δv = |v GNSS -v nmea |

[0192] Speed ​​consistency score: score v =1-min(1,Δv / SPEED d IFF THRESHOLD )

[0193] Among them: SPEED d IFF THRESHOLD is the speed difference threshold, which is a fixed constant;

[0194] Overall quality rating:

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

[0196] Where: WEIGHT d ISTANCE is the distance score weight, which is a fixed constant, WEIGHT d IRECTION is the direction score weight, which is a fixed constant, WEIGHT SPEED is the speed score weight, which is a fixed constant.

[0197] In some embodiments, in step S2, the eight models are executed in parallel, interact with each other using the IMM algorithm, and dynamically adjust their respective probabilities according to real-time observation data. In the initial stage of operation, the probabilities of the eight models are set to be equal, that is, μ i (0) = INIT_MODEL_PROB, which is a fixed constant with a value of 1 / 8, where μ i Represents the probability of the ith model.

[0198] The large-ship uniform motion model (Large-CV) is constructed based on the motion characteristics of large ships, which reflects the difficulty of turning and changing speed of large ships. Once a large ship reaches a stable speed, it tends to maintain a constant speed and continue to sail. When the evaluation value of the GNSS signal quality exceeds the preset threshold, the first large-ship uniform motion model is constructed, and its state transfer equation is:

[0199]

[0200] Among them, 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 uniform motion model:

[0202]

[0203] Where: I is the unit matrix, τ is the system sampling period, Q L1G is the noise coefficient of the large ship position process, Q L2G is the noise coefficient of the ship acceleration process;

[0204] The state transfer equation of the first large ship uniform acceleration motion model:

[0205]

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

[0207] The large-ship uniform acceleration motion model (Large-CA) is designed to describe the dynamic changes of large ships during speed change. Considering that large ships have large inertia, their acceleration changes are usually slow. When the evaluation value of the GNSS signal quality exceeds the preset threshold, the first large-ship uniform acceleration motion model is constructed, and its process noise matrix is:

[0208]

[0209] Where: I is the unit matrix, τ is the system sampling period, which comes from the system clock; Q L3G is the noise coefficient of the large ship uniform acceleration model process.

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

[0211]

[0212] Where: I is the unit matrix, τ is the system sampling period, which comes from the system clock; Q L1P is the noise coefficient of the large ship position process, Q L2P is the noise coefficient of the large ship acceleration process.

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

[0214]

[0215] Where: I is the unit matrix, τ is the system sampling period, which comes from the system clock; Q L3P is the noise coefficient of the large ship uniform acceleration model process;

[0216] The small-boat uniform motion model (Small-CV) takes into account that small boats are easily affected by external factors and will fluctuate even during uniform navigation. The state transfer equation of the first small-boat uniform motion model is the same as that of the first large-boat uniform motion model, and its process noise matrix is:

[0217]

[0218] Where: I is the unit matrix, τ is the system sampling period, which comes from the system clock; Q S1G is the noise coefficient of the boat position process, Q S2G is the noise coefficient of the boat acceleration process.

[0219] The Small-CA model is designed to describe the dynamic behavior of a small boat during rapid maneuvers. The state transfer equation of the first Small-CA model is the same as that of the first Large-CA model, and its process noise matrix is:

[0220]

[0221] Where: I is the unit matrix, τ is the system sampling period, which comes from the system clock; Q S3G is the noise coefficient of the boat uniform acceleration model process.

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

[0223]

[0224] Where: I is the unit matrix, τ is the system sampling period, which comes from the system clock; Q S1P is the noise coefficient of the boat position process, Q S2P is the noise coefficient of the boat acceleration process.

[0225] The state transfer equation of the second boat uniform acceleration motion model is the same as that of the first boat uniform acceleration motion model, and its process noise matrix is:

[0226]

[0227] Where: I is the unit matrix, τ is the system sampling period, which comes from the system clock; Q S3P is the noise coefficient of the boat uniform acceleration model process.

[0228] The extended model conversion probability matrix is:

[0229]

[0230] Where: P ii Keep the probability for the model itself, a fixed constant greater than 0.9, P ij (i≠j) is the transition probability from model i to model j. For any model i, it satisfies: ΣP ij =1(j=1,2,...,8).

[0231] In some embodiments, in step S3, the calculation formula of 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 is the model transition probability, μ i (k-1): the probability of model i at time k-1, c j is the normalization factor. The normalization factor cj is the sum of the probabilities of switching from different models to model j, that is, c j =∑ i (π ij ×μ i (k-1)). In the formula, by dividing by the normalization factor c j , ensuring that the sum of all probabilities of switching from different models to model j is 1.

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

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

[0236] Among them, X 0j : The initial state of the mixture of model j, X i (k-1): the state of model i at time k-1, μ ij : The probability of switching from model i to model j at time k. In the formula, X i (k-1)×μ ij represents the product of the state of model i at time k-1 and the probability of switching from model i to model j. The sum is taken for all possible models i to obtain the mixed initial state X of model j. 0j This method takes into account all possible model switches and can more accurately reflect the initial state of the system at time k.

[0237] Each model uses its own kinetic equation for prediction, the prediction formula is:

[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 mixed initial state of the i-th model at time k-1, which contains the information of 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 mixture of the i-th model at time k-1; F i is the state transfer matrix of the i-th model; Q i is the process noise matrix of the ith model, describing 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) is the GNSS measurement value, including position and velocity information, H GNSS is the GNSS measurement matrix, indicating which state quantities can be directly observed by GNSS, v i (k) is the new information sequence, reflecting the difference between the predicted value and the measured value;

[0244] The processing of IMU measurement data is as follows: IMU measurement data is acceleration and angular velocity data in the carrier coordinate system, and coordinate conversion is performed: a n =C b ^n*a b +g n ,

[0245] Among them: a bis the acceleration in the carrier coordinate system measured by IMU, C b ^n is the transformation matrix from the carrier coordinate system to the navigation coordinate system, obtained by IMU attitude solution, g n is the local gravity 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 [ω x ,ω y ,ω z ], which represents the angular velocity of rotation around the three axes in the carrier coordinate system.

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

[0249]

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

[0251] (3) The Euler angle is obtained by 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) In order to avoid the gimbal lock problem, when the pitch angle θ is close to ±90°, the quaternion method is required to update the attitude:

[0258]

[0259] Where: q is the attitude quaternion, Ω(ω) is the antisymmetric matrix of angular velocity, is the time derivative of the quaternion, which represents the rate of change of the attitude.

[0260] The calculated measurement innovation 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, which comes from the accuracy index provided by the GNSS receiver, GNSS velocity measurement noise, which comes from the accuracy index provided by the GNSS receiver, and IMU acceleration measurement noise, which comes from the IMU device index.

[0263] The status update is:

[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): prediction error covariance matrix of the i-th model at time k, H: observation matrix, H T is the transpose of the observation matrix, S i (k) -1 : The inverse matrix of the measurement innovation covariance of the i-th model at time k, X i (k|k): The updated 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 new information sequence of the i-th model at time k, P i (k|k): Updated error covariance matrix of the i-th model at time k, I: Identity matrix.

[0268] In some embodiments, in step S5, GNSS quality adaptation is:

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

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

[0271] When quality score =GNSS THRESHOLD When, quality factor =0.5, indicating that the quality score is just near the threshold and the degree of adaptation is medium.

[0272] When quality score >>GNSS THRESHOLD When, quality factor When it approaches 1, it means that the quality score is much higher than the threshold and the degree of adaptation is high.

[0273] When quality score < <GNSS THRESHOLD When, quality factor When it approaches 0, it means 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) is the likelihood of the i-th model, which indicates the model’s ability to explain the current observation, μ i (k): The probability of the i-th model at the current time k, μ i (k-1) is the model probability of the previous moment k-1, c is the normalization factor, ensuring that the sum of all model probabilities is 1, and by summing we get: c = Σ(L i (k)*μ i (k-1)).

[0276] The GNSS quality is 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] Among them, μ iadjusted (k): The probability of the adjusted i-th model at the current time k, μ i (k): the probability of the i-th model at the current time k, quality factor :GNSS quality factor. This formula is used to calculate the GNSS quality factor based on the GNSS quality factor. factor Adjust the probability of each model. For the first model, its probability is multiplied by quality factor , which means that when the GNSS quality is high, the probability of the first model increases. For the second model, its probability is multiplied by 1-quality factor , which means that when the GNSS quality is high, the probability of the second model will decrease. This method can dynamically adjust the model probability to adapt to the quality changes of GNSS signals.

[0281] The probability smoothing process is:

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

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

[0284] This formula is used to smooth the model probability to reduce the probability fluctuation and improve the stability of the system. factor ×μ iadjusted (k) represents the contribution of the adjusted probability at the current moment, (1-SMOOTH factor )×μ ismooth (k-1) represents the contribution of the smoothed probability at the previous moment. Smoothing coefficient SMOOTH factor Determines the degree of influence of the current adjusted probability on the smoothed probability. factorWhen SMOOTH is larger, the influence of the adjusted probability at the current moment on the smoothed probability is greater, and the smoothing effect is weaker; on the contrary, when SMOOTH factor The smaller the value, the greater the influence of the smoothed probability at the previous moment on the smoothed probability, and the stronger the smoothing effect. factor Usually set based on the dynamic characteristics of the system to achieve the best smoothing effect.

[0285] The final state estimation obtained by weighted fusion of all models is:

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

[0287] Among them, X fusion (k): Final state estimate at the current time k, μ ismooth (k): The smoothed probability of the i-th model at the current time k, X i (k|k): The state estimate of the i-th model at the current time k. In the formula, the state estimate X of each model i (k|k) multiplied by its corresponding smoothed probability μ ismooth (k), and then summed over all models. The smoothed probability μ ismooth (k) is used as a weight to indicate the contribution of each model to the final state estimation. Through weighted fusion, the estimation results of all models can be comprehensively considered to improve the accuracy and reliability of the final state estimation.

[0288] The fused state covariance takes into account the state differences between models:

[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 the current time k, μ ismooth (k): The smoothed probability 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, dX i is the difference between each model state and the fusion state, dX 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 fusion state i Instead of transposing dX i T The product of and then multiply by the smoothed probability μ of the model ismooth (k). Sum all models to get 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, ship type determination:

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

[0293] Among them, Ship score is the ship type identification index, Σμ large (k) and Σμ small (k) are the total probabilities of large ship and small ship models respectively. This formula calculates the ratio of the total probability of large ship model to the total probability of small ship model, and then takes the natural logarithm to obtain the ship type discrimination index Ship score .Ship score The value of can be used to determine the ship type: if Ship score is positive, indicating that it is more likely to be a large ship; if Ship score A negative value means it is more likely to be a small boat.

[0294] The GNSS quality determination is:

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

[0296] Among them, GNSS score Score the GNSS quality, Σμ good (k) and Σμ poor (k) are the total probabilities of the good signal model and the bad signal model respectively; the formula calculates the ratio of the total probability of the good signal model to the total probability of the bad signal model, and then takes the natural logarithm to obtain the GNSS quality score GNSS score GNSS score The value of can be used to judge the quality of GNSS signal: if GNSS score If it is positive, it means the signal quality is good; if GNSS score A negative value indicates poor signal quality.

[0297] The parameter adaptation is:

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

[0299] Among them, Q new is the new adaptive parameter, Q base As the basic parameter, SHIP_ADAPT factor Ship adaptation factor, score Score vessel, GNSS_ADAPT factor is the global navigation satellite system adaptation factor, GNSS score The formula is calculated by dividing the basic parameter Q base Multiply with the adaptation factors and scores of the ship and GNSS to calculate the new adaptation parameter Q new The product of the adaptation factor and the score plus 1 indicates the adjustment of the basic parameters.

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

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

[0302] Among them, π base is the basic transition probability, π adapted is the transition probability after adaptation, ADAPT_RATE: is the adaptation rate, which is calculated by subtracting the basic transition probability π base and the adapted transition probability π adapted According to the weighted average of the adaptation rate ADAPT_RATE, the new transition probability π is calculated new The higher the adaptation 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 smoothing the fusion state:

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

[0305] Where λ is the smoothing coefficient, which is determined based on the dynamic characteristics of the system; 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 of the previous moment. This formula is obtained by taking the fusion state X fusion (k) and the output state X at the previous moment output The difference between (k-1) is multiplied by the smoothing coefficient λ and then added to the current fusion state to calculate the output state X at the current moment output (k). The smoothing coefficient λ determines the degree of influence of the output state at the previous moment on the output state at the current moment. When λ is larger, the output state at the previous moment has a greater influence on the output state at the current moment, 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): normalized state deviation at the current moment, X fusion (k): The fusion state at the current moment, X predict (k): the predicted state at the current moment; σ(k) is the prediction uncertainty, which comes from the diagonal elements of the state covariance matrix. This formula is calculated by calculating the fusion state X at the current moment fusion (k) and the predicted state X predict (k), and then divide it by the prediction uncertainty σ(k) to get the normalized state deviation D(k). The normalized state deviation D(k) can be used to determine whether the system is abnormal. If D(k) exceeds a preset threshold, it may indicate that the system is abnormal.

[0309] If the normalized state deviation D(k) is higher than the preset threshold, the system will trigger an exception handling mechanism, such as recalibrating the sensor or enabling the backup navigation system to ensure the accuracy and reliability of navigation. In addition, the anomaly detection module will record the abnormal event, including the time when the abnormality occurred, the type of abnormality, and the system status at the time of the abnormality, so as to facilitate subsequent fault analysis and system optimization. In this way, the method can not only improve the robustness of the navigation system, but also provide valuable data support for system maintenance and optimization.

[0310] In some specific embodiments, the predetermined conditions set in step S8 cover the following aspects: first, when the system receives a clear stop instruction, the execution of this step will be triggered; second, if the global navigation satellite system (GNSS) signal is completely lost, and this loss state lasts for more than a preset time length standard, step S8 will also be triggered; third, if the data of the inertial measurement unit (IMU) is abnormal, and the duration of this abnormal state exceeds the preset threshold range, this step will also be triggered; finally, when the number of times the system reinitializes exceeds the maximum allowable value set in advance, the execution of step S8 will also be triggered. When any one or more of the above conditions are met at the same time, step S8 will be activated and the corresponding operation will be performed.

[0311] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment, characterized in that: The following steps are involved: S1, initialization phase: obtaining the ship's GNSS measurement data and IMU measurement data, and initializing the GNSS position data; S2, multi-model establishment stage: evaluate the GNSS signal quality, and when the evaluation value is greater than the preset value, establish the first large ship uniform motion model, the first large ship uniform acceleration motion model, the first small boat uniform motion model, and the first small boat uniform acceleration motion model; when the evaluation value is less than the preset value, establish the second large ship uniform motion model, the second large ship uniform acceleration motion model, the second small boat uniform motion model, and the second small boat uniform acceleration motion model; S3, prediction stage: at the beginning of each sampling period, the states of the models are interacted with each other, the interaction probability is calculated, and then the mixed initial state of each model is calculated based on the interaction probability. Based on the mixed initial state, each model uses its own dynamic equation for prediction; S4, measurement update phase: first process the GNSS measurement data, then process the IMU measurement data, then calculate the measurement information covariance based on the two measurement data, and then perform status update; S5, model probability update and state fusion stage: perform GNSS quality adaptation, update the probability of each model, then perform GNSS quality adjustment, introduce smoothing processing to avoid model probability mutation, and obtain the final state estimation through weighted fusion of all models; S6, ship type identification and parameter adaptation stage: based on the probability of each model, the ship type identification index and GNSS quality score are calculated, and adaptive parameters are adjusted according to the index and GNSS quality score, and the model transfer probability matrix is ​​dynamically adjusted; S7, system output and exception processing stage: smooth the fusion state, perform anomaly detection 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 accumulate statistics on the duration of the anomaly; S8, Navigation termination phase: The operation is terminated when the predetermined conditions are met.

2. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment according to claim 1 is characterized in that: In step S1, the GNSS measurement data includes latitude (Lat), longitude (Lon) and altitude (h). The initial position is selected as the reference point (Lat0, Lon0, h0) to perform coordinate transformation. The coordinate transformation is divided into the following two steps: Step 1: Convert the geodetic coordinates (Lat, Lon, h) to Earth-centered Earth-fixed coordinates (ECEF). The conversion formula is: X=(N+h)*cos(Lat)*cos(Lon) Y=(N+h)*cos(Lat)*sin(Lon) Z=(N*(1-e 2 )+h)*sin(Lat) Where: N is the radius of the circle: N = a / sqrt(1-e 2 sin 2 (Lat)), a is the major semi-axis of the ellipsoid, and e is the eccentricity of the ellipsoid; Step 2: Convert ECEF coordinates to local ENU coordinates. The conversion formula is: [e]=[-sin(Lon0)cos(Lon0)0][X-X0] [n]=[-sin(Lat0)cos(Lon0)-sin(Lat0)sin(Lon0)cos(Lat0)][Y-Y0] [u]=[cos(Lat0)cos(Lon0)cos(Lat0)sin(Lon0)sin(Lat0)][Z-Z0] Where (X0, Y0, Z0) is the ECEF coordinate of the reference point; The converted ENU coordinates will be used as the observed values ​​of the position component in the state vector; Create a system state vector, which consists of nine components: X=[e,n,u,v e ,v n ,v u ,a e ,a n ,a u ] T , where the position component [e, n, u] is obtained from the GNSS receiver and represents the position coordinates of the carrier in the northeast celestial coordinate system: e is the east position provided by GNSS, n is the north position provided by GNSS, and u is the celestial position provided by GNSS; Velocity component [v e ,v n ,v u ] obtained from the GNSS receiver, v e is the eastward velocity measured by GNSS, v n is the north velocity measured by GNSS, v u is the celestial velocity measured by GNSS; Acceleration component [a e ,a n ,a u ] obtained through IMU measurement, a e : The eastward acceleration after coordinate conversion of the IMU accelerometer measurement, a n : North acceleration after coordinate conversion of IMU accelerometer measurement, a u : Celestial acceleration measured by the IMU accelerometer after coordinate conversion.

3. The adaptive heading angle filtering navigation method based on GNSS trajectory quality assessment according to claim 1 is characterized in that: In 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} ) 2 +(years i -years {i-1} ) 2 ) Calculate the distance standard deviation: std d =sqrt(Σ(d i -d mean ) 2 / (n-1)) Distance consistency score: score d =1-min(1,std d / MAX d ISTANCE STD ) Where: MAX d ISTANCE STD is the distance standard deviation threshold, which is a fixed constant; (2) Direction consistency assessment: 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 direction changes: std θ =sqrt(Σ(θ i -θ mean ) 2 / (n-1)) Direction consistency score: score θ =1-min(1,std θ / MAX d IRECTION STD ) Where: MAX d IRECTION STD is the direction standard deviation threshold, which is a fixed constant; (3) Speed ​​consistency assessment: Calculate the velocity derived from the GNSS point: v GNSS =d i / Δt Calculate the difference from NMEA speed: Δv = |v GNSS -v nmea | Speed ​​consistency score: score v =1-min(1,Δv / SPEED d IFF THRESHOLD ) Among them: SPEED d IFF THRESHOLD is the speed difference threshold, which is a fixed constant; Overall quality rating: quality score =WEIGHT d ISTANCE*score d +WEIGHT d IRECTION*score θ +WEIGHT SPEED *score v Where: WEIGHT d ISTANCE is the distance score weight, which is a fixed constant, WEIGHT d IRECTION is the direction score weight, which is a fixed constant, WEIGHT SPEED is the speed score weight, which 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, interacting through the IMM algorithm, and their respective probabilities are dynamically adjusted according to the actual observed data. In the initial operation, the probabilities of the eight models are set to be equal, which is μ i (0) = INIT_MODEL_PROB, fixed constant, value is 1 / 8, where μ i represents the probability of the i-th model; The state transfer equation of the first large ship uniform motion model: The process noise matrix of the first large ship uniform motion model: Where: I is the 3×3 unit matrix, τ is the system sampling period, Q L1G is the noise coefficient of the large ship position process, Q L2G is the noise coefficient of the ship acceleration process; The state transfer equation of the first large ship uniform acceleration motion model: The process noise matrix of the first large ship uniform acceleration motion model: Where: Q L3G is the noise coefficient of the large ship uniform acceleration model process; The state transfer equation of the uniform motion model of the second largest ship is the same as that of the uniform motion model of the second largest ship, and its process noise matrix is: Where: Q L1P is the noise coefficient of the large ship position process, Q L2P is the noise coefficient of the ship acceleration process; The state transfer equation of the second large ship uniform acceleration motion model is the same as that of the first large ship uniform acceleration motion model, and its process noise matrix is: Where: Q L3P is the noise coefficient of the large ship uniform acceleration model process; The state transfer equation of the first small boat uniform motion model is the same as that of the first large boat uniform motion model. Its process noise matrix: Where: Q S1G is the noise coefficient of the boat position process, Q S2G is the noise coefficient of the boat acceleration process; The state transfer equation of the first small boat uniform acceleration motion model is the same as that of the first large ship uniform acceleration motion model, and its process noise matrix is: Where: Q S3G is the noise coefficient of the boat uniform acceleration model process; The state transfer equation of the second boat uniform motion model is the same as that of the first boat uniform motion model. Process noise matrix: Where: Q S1P is the noise coefficient of the boat position process, Q S2P is the noise coefficient of the boat acceleration process; The state transfer equation of the second boat uniform acceleration motion model is the same as that of the first boat uniform acceleration motion model, and its process noise matrix is: Where: Q S3P is the noise coefficient of the boat uniform acceleration model process; The extended model conversion probability matrix is: Where: P ii Keep the probability for the model itself, a fixed constant greater than 0.9, P ij (i≠j) is the transition probability from model i to model j. For any model i, it satisfies: ΣP ij =1(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 calculation formula of the interaction probability is: m ij =(π i j*m i (k-1)) / c j Where: μ i j represents the probability of switching from model i to model j at time k, π i j is the model transition probability, c j is the normalization factor; The formula for calculating the mixed initial state for each model is as follows: X 0j =Σ(X i (k-1)*m ij ); Each model uses its own kinetic equation for prediction, the prediction formula is: 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, P i (k-1|k-1) is the covariance matrix of the i-th model at time k-1, F i is the state transfer matrix of the i-th model, Q i is the process noise matrix of the ith 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) is the GNSS measurement value, including position and velocity information, H GNSS is the GNSS measurement matrix, indicating which state quantities can be directly observed by GNSS, v i (k) is the new information sequence, reflecting the difference between the predicted value and the measured value; The processing of IMU measurement data is as follows: IMU measurement data is acceleration and angular velocity data in the carrier coordinate system, and coordinate conversion is performed: a n =C b ^n*a b +g n , Among them: a b is the acceleration in the carrier coordinate system measured by IMU, C b ^n is the transformation matrix from the carrier coordinate system to the navigation coordinate system, obtained by IMU attitude solution, g n is the local gravity acceleration vector, calculated from latitude and altitude; The calculated measurement innovation covariance is: S i (k)=H*P i (k|k-1)*H T +R Where R is the measurement noise covariance matrix, including: GNSS position measurement noise, which comes from the accuracy index provided by the GNSS receiver, GNSS velocity measurement noise, which comes from the accuracy index provided by the GNSS receiver, and IMU acceleration measurement noise, which comes from the IMU device index; 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: Z GNSS (k) is the GNSS observation vector at time k, H GNSS is the GNSS observation matrix, v i (k) is the new information sequence of the i-th model, C b ^n is the transformation matrix from the carrier coordinate system to the navigation coordinate system, a b is the load system acceleration measured by IMU, g n is the gravity acceleration vector in the navigation coordinate system, a n is the acceleration in the navigation coordinate system.

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 adaptation: quality factor =sigmoid(quality score ,GNSS_SIGMOID_K,GNSS THRESHOLD ) Among them: sigmoid(x,k,t)=1 / (1+exp(-k*(xt))) is the S-type function, quality score Score the quality of GNSS trajectories, GNSS_SIGMOID_K is the steepness parameter of the sigmoid function, which is a fixed constant. GNSS THRESHOLD is the quality judgment threshold, is a fixed constant, When quality score = GNSS THRESHOLD , quality factor = 0.5, When quality score >>GNSS THRESHOLD When, quality factor Approaching 1 When quality score < <GNSS THRESHOLD When, quality factor Approaching 0; The probability of updating each model is: i (k)=(L i (k)*μ i (k-1)) / c, Where: L i (k) is the likelihood of the i-th model, which indicates the model’s ability to explain the current observation, μ i (k-1) is the model probability at the previous moment, c is the normalization factor, ensuring that the sum of all model probabilities is 1, and by summing we get: 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); Where α is the smoothing coefficient, which is set based on the dynamic characteristics of the system; The final state estimation obtained by weighted fusion of all models is: X fusion (k)=Σ(μ ismooth (k)*X i (k|k)); The fused state covariance takes into account the state differences between models: P fusion (k)=Σ(μ ismooth (k)*[P i (k|k)+dX i *dX i T ]), Among them, dX i is the difference between each model state and the fusion state, dX i =X i (k|k)-X fusion (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 identification: Ship score =ln((Sm large (k)) / (Sm small (k))) where Σμ large (k) and Σμ small (k) are the total probabilities of the large ship and small ship models respectively; GNSS quality determination: GNSS score =ln((Sm good (k)) / (Sm poor (k))) where Σμ good (k) and Σμ poor (k) are the total probabilities of 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 ); Transition probability adaptation: p new =(1-ADAPT_RATE)*π base +ADAPT_RATE*π adapted 。 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 fusion state: X output (k)=X fusion (k)+λ*(X output (k-1)-X fusion (k)) Among them, λ 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 σ(k) is 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 condition is one or more of: receiving a system stop command, complete loss of GNSS signal exceeding a preset time, IMU data anomaly continuing to exceed a threshold, or system reinitialization times exceeding a set value.

Images