A least square covariance dynamic weighted satellite inertial navigation integrated positioning method

Abstract

This invention discloses a least-squares covariance dynamically weighted satellite inertial navigation system (INS) attitude determination method, belonging to the technical field of integrated navigation and mobile vehicle attitude determination. It is used for satellite INS integrated attitude determination, including acquiring and preprocessing raw data; constructing a nonlinear observation model and solving it after linearization using a Gauss-Newton iterative algorithm; updating the error state vector and state covariance over time based on the mechanical arrangement of the inertial navigation system; reconstructing the measurement noise covariance matrix of the extended Kalman filter; constructing the measurement innovation vector; calculating the Kalman gain and updating the state; and outputting the attitude determination result. This invention achieves instantaneous, delay-free adaptive fusion by introducing the least-squares posterior covariance of the current epoch for dynamic weighting, significantly suppressing the short baseline noise amplification effect, and improving the attitude estimation accuracy, statistical consistency, and real-time observation quality self-diagnosis capability under complex sea conditions.

Description

Technical Field

[0001] This invention discloses a least-squares covariance dynamic weighted satellite inertial navigation system attitude determination method, which belongs to the technical field of integrated navigation and mobile vehicle attitude determination. Background Technology

[0002] Currently, the combination of Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) is the standard approach to providing continuous and robust navigation solutions. However, in practical USV applications, especially when equipped with low-cost microelectromechanical systems (MEMS) inertial measurement units (IMUs), existing GNSS / INS integrated navigation technologies suffer from the following significant drawbacks.

[0003] Unobservable heading of single-antenna GNSS under low dynamic conditions: Traditional single-antenna GNSS cannot directly observe the heading of the vehicle. When traveling at low speeds, hovering, or under complex disturbances, the effectiveness of using GNSS velocity vector to assist in estimating the heading is greatly reduced, which can easily lead to the accumulation of heading errors.

[0004] The "short baseline effect" of multi-antenna GNSS on small mobile vehicles: To address heading drift, multi-antenna (e.g., at least three non-collinear antennas) GNSS arrays are often introduced for direct attitude determination. However, on small USVs with limited size, the antennas inevitably exhibit a "short baseline" geometry. This short baseline amplifies the impact of GNSS observation noise on the attitude calculation results, leading to drastic time-varying characteristics in the attitude data quality of multi-antenna GNSS, including a significant amount of high-frequency noise.

[0005] Limitations of traditional Extended Kalman Filter (EKF) fusion strategies: Existing GNSS / INS fusion frameworks typically employ the standard EKF algorithm, where the measurement noise covariance matrix (representing the degree of trust in the GNSS data) is set to a fixed empirical constant. Traditional EKF, with its fixed weighting scheme, is completely incompatible with the drastic real-time fluctuations in noise from multi-antenna GNSS short-baseline observations.

[0006] Existing adaptive filtering (AKF) suffers from lag and instability: To dynamically adjust weights, existing techniques often employ adaptive techniques based on innovation or residual sequences (such as Sage-Husa Adaptive Kalman Filter, SHAKF). These methods rely on a sliding window of historical data for statistical inference, which inevitably introduces "statistical lag" in the dynamically changing ocean environment, leading to suboptimal weight allocation. Furthermore, under complex sea conditions, the coupling between state estimation errors and noise parameter estimations can easily cause filter divergence or instability. Summary of the Invention

[0007] The purpose of this invention is to provide a least-squares covariance dynamic weighted satellite inertial navigation system attitude determination method to solve the problems in the prior art where the measurement noise covariance matrix cannot reflect the GNSS observation quality of the current epoch in real time and accurately, resulting in adaptive weighting with lag, reduced filtering accuracy, and statistical inconsistencies.

[0008] A least-squares covariance dynamically weighted satellite inertial navigation system attitude determination method, characterized by comprising:

[0009] S1. Obtain the raw inertial data output by the inertial measurement unit, obtain the observation data output by the global navigation satellite system receiver array, perform absolute coordinate network adjustment on the observation data, extract the global navigation satellite system observation baseline vector in the navigation coordinate system, and calibrate the lever vector from the center of the inertial measurement unit to the phase center of the global navigation satellite system receiver antenna. The raw inertial data includes angular velocity and acceleration.

[0010] S2. Construct a nonlinear observation model using the attitude rotation matrix and observation noise, linearize the nonlinear observation model using the Gauss-Newton iterative algorithm and solve it to obtain the attitude estimate and posterior covariance matrix of the global navigation satellite system.

[0011] S3. Based on the original inertial data, according to the mechanical arrangement and mechanical arrangement equations of the inertial navigation system, the predicted attitude of the inertial navigation system is calculated, and the error state vector is constructed. At the same time, based on the continuous-time system error dynamics matrix recursively derived by the inertial navigation system, the error state vector and the state covariance are updated over time.

[0012] S4. Introduce a filter into the posterior covariance matrix and combine it with a scalar adjustment factor to reconstruct the measurement noise covariance matrix of the extended Kalman filter.

[0013] S5. Based on the attitude estimation value and posterior covariance matrix of the global navigation satellite system, a measurement innovation vector is constructed. Based on the reconstructed extended Kalman filter measurement noise covariance matrix, the Kalman gain is calculated and the state is updated. The fused attitude information is output and the error state is fed back. The inertial navigation system is corrected using the feedback error state.

[0014] S1 includes, S1.1, acquiring raw inertial data output by the inertial measurement unit under a unified global navigation satellite system time reference, acquiring observation data output by the global navigation satellite system receiver array, wherein the number of global navigation satellite system receiver antennas is greater than or equal to 4;

[0015] S1 includes S1.2, which involves extracting the global navigation satellite system observation baseline vector in the navigation coordinate system based on absolute coordinate network adjustment. Calibrate the lever vector from the center of the inertial measurement unit to the phase center of the global navigation satellite system receiver antenna. , For the navigation coordinate system, For the carrier coordinate system, For the antenna index of the Global Navigation Satellite System receiver, Indicates lever arm, This represents the observed value.

[0016] S2 includes, S2.1, constructing a nonlinear observation model:

[0017] ;

[0018] In the formula, Here is the attitude rotation matrix. The attitude angle vector to be estimated includes the roll angle, pitch angle, and yaw angle. For the first Observation noise of the root antenna.

[0019] S2 includes S2.2, which uses the Gauss-Newton iterative algorithm to linearize the nonlinear observation model:

[0020] ;

[0021] In the formula, This represents the number of iterations for the linearization equation. For the first Linearization error of the next iteration For the first The Jacobian matrix of the next iteration.

[0022] S2 includes S2.3, which, after iterative convergence, outputs the global navigation satellite system attitude estimate for the current epoch. Simultaneously output the posterior covariance matrix. :

[0023] ;

[0024] ;

[0025] In the formula, The Jacobian design matrix reflects the antenna geometry. For the baseline prior weight matrix of the global navigation satellite system, The unit-weighted posterior variance is calculated based on the solution residuals. For the residual vector, For degrees of freedom, The number of observation equations, The number of parameters to be estimated. This is the transpose symbol.

[0026] S3 includes S3.1, which involves calculating the predicted attitude of the inertial navigation system at the current epoch by integrating and recursively applying the inertial navigation mechanics programming equations based on the angular velocity and acceleration measured by the inertial measurement unit, according to the mechanical arrangement of the inertial navigation system. ;

[0027] S3 includes S3.2, constructing the error state vector. , This includes attitude error, velocity error, position error, gyroscope bias error, and accelerometer bias error;

[0028] Based on the continuous-time system error dynamics matrix derived from the inertial navigation system Discretization yields the state transition matrix. :

[0029] ;

[0030] In the formula, It is the identity matrix. The discrete time step;

[0031] according to conduct Time update and state covariance Time update:

[0032] ;

[0033] ;

[0034] In the formula, For the calendar year, For prior error state estimation, For posterior error state estimation, Here is the state transition matrix. To estimate the error covariance matrix a priori, For the posterior estimation of the error covariance matrix, Let be the system process noise covariance matrix.

[0035] S4 includes, in the current epoch, Introducing a filter, along with a scalar adjustment factor Multiply to reconstruct the measurement noise covariance matrix of the current extended Kalman filter. :

[0036] ;

[0037] In the formula, For the first The specific time corresponding to each epoch;

[0038] Using normalized innovation square Test pair Calibration is performed, including the introduction of... :

[0039] ;

[0040] In the formula, For the first epochs , For the first The new information vector of an epoch, For the first The inverse of the epochal information covariance matrix, Obeying the degree of freedom The chi-square distribution, The theoretical expected value is ;

[0041] Set up a sliding window ,exist Internal computation mean :

[0042] ;

[0043] according to Calibration :

[0044] ;

[0045] In the formula, For the first Scalar adjustment factor of epoch, For the first epochs The mean.

[0046] S5 includes, S5.1, constructing the measurement innovation vector. :

[0047] ;

[0048] In the formula, For generalized subtraction of posture, for The predicted attitude output by the mechanical orchestration of the inertial navigation system at all times. for At any given time, the attitude estimate of the global navigation satellite system.

[0049] S5 includes S5.2, based on Calculate the covariance matrix of measurement information and Kalman gain :

[0050] ;

[0051] ;

[0052] In the formula, For the measurement matrix, The prior estimate is the error covariance matrix;

[0053] State updates include, utilizing right and Update:

[0054] ;

[0055] ;

[0056] In the formula, For posterior error state estimation, This is the posterior estimation error covariance matrix.

[0057] S5 includes, S5.3, utilizing The attitude error in Perform compensation and output the fused pose information;

[0058] Will The zero-bias errors of the gyroscope and accelerometer are fed back to the mechanical arrangement of the inertial navigation system to compensate for the measured values ​​of the inertial navigation system and complete the calibration.

[0059] Compared with existing technologies, this invention has the following advantages: By directly utilizing the posterior covariance matrix inherently generated by the least squares solution of the current epoch, this invention constructs the measurement noise covariance matrix of the extended Kalman filter, abandoning the lag method of traditional adaptive filtering that relies on historical sliding windows. This achieves instantaneous, delay-free adaptive weighting, making the filter more sensitive to multi-antenna geometric changes and environmental noise, and completely avoiding statistical delay and estimation instability problems in dynamic environments. By adaptively reducing the Kalman gain of unreliable observations through posterior covariance expansion, it effectively suppresses the short-baseline noise amplification effect. By introducing a normalized innovation square test to rigorously calibrate the scaling factor, it achieves strict statistical consistency of the filter, avoiding the defects of system "overconfidence" or "insufficient confidence." By utilizing the posterior covariance matrix inherently generated by the multi-antenna least squares solution to form an independent observation quality self-diagnostic index, it achieves real-time quantitative evaluation of the reliability of GNSS observations at the current epoch, providing observation quality early warning and integrity monitoring capabilities for upper-level unmanned systems. Through the above comprehensive methods, a quantitative leap in attitude determination accuracy is achieved. Attached Figure Description

[0060] Figure 1 This is a flowchart of the technology of this invention;

[0061] Figure 2 It is the time series of unit weighted posterior mean error roll angle standard deviation for LS attitude calculation;

[0062] Figure 3 It is the time series of unit-weighted posterior mean error pitch angle standard deviation for LS attitude calculation;

[0063] Figure 4 It is the time series of the unit weighted posterior mean error heading angle standard deviation for LS attitude calculation;

[0064] Figure 5 This is a scatter plot and histogram of filter conformance verification results based on NIS testing.

[0065] Figure 6 This is a time series diagram comparing the heading angle error of the traditional method and the method of this invention;

[0066] Figure 7 yes Figure 6 Enlarged view of region A1 in the middle;

[0067] Figure 8 yes Figure 6 Enlarged view of area A2 in the middle;

[0068] Figure 9 This is a time series graph comparing the roll angle error of the traditional method and the method of this invention;

[0069] Figure 10 yes Figure 9 Enlarged view of region B1 in the middle;

[0070] Figure 11 yes Figure 9 Enlarged view of area B2 in the middle;

[0071] Figure 12 This is a histogram of the roll error distribution of the method of the present invention;

[0072] Figure 13 It is a histogram of roll error distribution using traditional methods. Detailed Implementation

[0073] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention are described clearly and completely below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0074] A least-squares covariance dynamically weighted satellite inertial navigation system attitude determination method, characterized by comprising:

[0075] S1. Obtain the raw inertial data output by the inertial measurement unit, obtain the observation data output by the global navigation satellite system receiver array, perform absolute coordinate network adjustment on the observation data, extract the global navigation satellite system observation baseline vector in the navigation coordinate system, and calibrate the lever vector from the center of the inertial measurement unit to the phase center of the global navigation satellite system receiver antenna. The raw inertial data includes angular velocity and acceleration.

[0076] S2. Construct a nonlinear observation model using the attitude rotation matrix and observation noise, linearize the nonlinear observation model using the Gauss-Newton iterative algorithm and solve it to obtain the attitude estimate and posterior covariance matrix of the global navigation satellite system.

[0077] S3. Based on the original inertial data, according to the mechanical arrangement and mechanical arrangement equations of the inertial navigation system, the predicted attitude of the inertial navigation system is calculated, and the error state vector is constructed. At the same time, based on the continuous-time system error dynamics matrix recursively derived by the inertial navigation system, the error state vector and the state covariance are updated over time.

[0078] S4. Introduce a filter into the posterior covariance matrix and combine it with a scalar adjustment factor to reconstruct the measurement noise covariance matrix of the extended Kalman filter.

[0079] S5. Based on the attitude estimation value and posterior covariance matrix of the global navigation satellite system, a measurement innovation vector is constructed. Based on the reconstructed extended Kalman filter measurement noise covariance matrix, the Kalman gain is calculated and the state is updated. The fused attitude information is output and the error state is fed back. The inertial navigation system is corrected using the feedback error state.

[0080] S1 includes, S1.1, acquiring raw inertial data output by the inertial measurement unit under a unified global navigation satellite system time reference, acquiring observation data output by the global navigation satellite system receiver array, wherein the number of global navigation satellite system receiver antennas is greater than or equal to 4;

[0081] S1 includes S1.2, which involves extracting the global navigation satellite system observation baseline vector in the navigation coordinate system based on absolute coordinate network adjustment. Calibrate the lever vector from the center of the inertial measurement unit to the phase center of the global navigation satellite system receiver antenna. , For the navigation coordinate system, For the carrier coordinate system, For the antenna index of the Global Navigation Satellite System receiver, Indicates lever arm, This represents the observed value.

[0082] S2 includes, S2.1, constructing a nonlinear observation model:

[0083] ;

[0084] In the formula, Here is the attitude rotation matrix. The attitude angle vector to be estimated includes the roll angle, pitch angle, and yaw angle. For the first Observation noise of the root antenna.

[0085] S2 includes S2.2, which uses the Gauss-Newton iterative algorithm to linearize the nonlinear observation model:

[0086] ;

[0087] In the formula, This represents the number of iterations for the linearization equation. For the first Linearization error of the next iteration For the first The Jacobian matrix of the next iteration.

[0088] S2 includes S2.3, which, after iterative convergence, outputs the global navigation satellite system attitude estimate for the current epoch. Simultaneously output the posterior covariance matrix. :

[0089] ;

[0090] ;

[0091] In the formula, The Jacobian design matrix reflects the antenna geometry. For the baseline prior weight matrix of the global navigation satellite system, The unit-weighted posterior variance is calculated based on the solution residuals. For the residual vector, For degrees of freedom, The number of observation equations, The number of parameters to be estimated. This is the transpose symbol.

[0092] S3 includes S3.1, which involves calculating the predicted attitude of the inertial navigation system at the current epoch by integrating and recursively applying the inertial navigation mechanics programming equations based on the angular velocity and acceleration measured by the inertial measurement unit, according to the mechanical arrangement of the inertial navigation system. ;

[0093] S3 includes S3.2, constructing the error state vector. , This includes attitude error, velocity error, position error, gyroscope bias error, and accelerometer bias error;

[0094] Based on the continuous-time system error dynamics matrix derived from the inertial navigation system Discretization yields the state transition matrix. :

[0095] ;

[0096] In the formula, It is the identity matrix. The discrete time step;

[0097] according to conduct Time update and state covariance Time update:

[0098] ;

[0099] ;

[0100] In the formula, For the calendar year, For prior error state estimation, For posterior error state estimation, Here is the state transition matrix. To estimate the error covariance matrix a priori, For the posterior estimation of the error covariance matrix, Let be the system process noise covariance matrix.

[0101] S4 includes, in the current epoch, Introducing a filter, along with a scalar adjustment factor Multiply to reconstruct the measurement noise covariance matrix of the current extended Kalman filter. :

[0102] ;

[0103] In the formula, For the first The specific time corresponding to each epoch;

[0104] Using normalized innovation square Test pair Calibration is performed, including the introduction of... :

[0105] ;

[0106] In the formula, For the first epochs , For the first The new information vector of an epoch, For the first The inverse of the epochal information covariance matrix, Obeying the degree of freedom The chi-square distribution, The theoretical expected value is ;

[0107] Set up a sliding window ,exist Internal computation mean :

[0108] ;

[0109] according to Calibration :

[0110] ;

[0111] In the formula, For the first Scalar adjustment factor of epoch, For the first epochs The mean.

[0112] S5 includes, S5.1, constructing the measurement innovation vector. :

[0113] ;

[0114] In the formula, For generalized subtraction of posture, for The predicted attitude output by the mechanical orchestration of the inertial navigation system at all times. for At any given time, the attitude estimate of the global navigation satellite system.

[0115] S5 includes S5.2, based on Calculate the covariance matrix of measurement information and Kalman gain :

[0116] ;

[0117] ;

[0118] In the formula, For the measurement matrix, The prior estimate is the error covariance matrix;

[0119] State updates include, utilizing right and Update:

[0120] ;

[0121] ;

[0122] In the formula, For posterior error state estimation, This is the posterior estimation error covariance matrix.

[0123] S5 includes, S5.3, utilizing The attitude error in Perform compensation and output the fused pose information;

[0124] Will The zero-bias errors of the gyroscope and accelerometer are fed back to the mechanical arrangement of the inertial navigation system to compensate for the measured values ​​of the inertial navigation system and complete the calibration.

[0125] The extraction of global navigation satellite system (GNSS) observation baseline vectors in the navigation coordinate system based on absolute coordinate network adjustment is as follows: First, the raw RTK (Real-Time Dynamic Differential) absolute coordinates or unprocessed baseline vectors of each receiver antenna are obtained. Then, based on the known geometric prior information of the multi-antenna array (such as the fixed physical distance between antennas), a network adjustment model is constructed. This model is rigorously optimized using the least squares algorithm to eliminate geometric inconsistencies caused by measurement noise, obtaining the optimized absolute coordinates of each antenna. Finally, the optimized antenna absolute coordinates are differentially processed to extract the high-precision observation baseline vectors in the navigation coordinate system (n-system). .

[0126] The following description, in conjunction with the accompanying drawings, further illustrates the process of this invention. Figure 1As shown, the system comprises four parts: a GNSS attitude calculation module, a system self-evaluation, an LS-CW-based EKF fusion core, and an INS recursion module. The GNSS attitude calculation module performs network adjustment on the raw GNSS observations from four antennas to obtain the optimized navigation coordinate system (n-system) baseline vector. Then, least squares (LS) attitude determination is performed, with the results divided into a posterior covariance matrix and a least squares attitude estimate. The posterior covariance matrix serves two purposes: first, it provides adaptive weights for the EKF fusion core based on LS-CW; second, it is input into the system self-evaluation module for system performance evaluation and diagnosis (evaluating the quality of the least squares solution). The least squares attitude estimate is directly input into the LS-CW-based EKF fusion core. The EKF fusion core based on LS-CW receives the posterior covariance matrix to dynamically construct the measurement noise covariance matrix, receives least-squares attitude estimation to establish the measurement equations, and fuses the two for EKF measurement updates. It then performs error state estimation and outputs error state correction (feedback) to obtain the integrated navigation solution. Finally, it receives the INS recursive state output from the INS recursion for state prediction and feeds it back to the EKF measurement update. The INS recursion involves obtaining the INS recursive state from the raw IMU data based on INS mechanical orchestration and error state correction (feedback).

[0127] This invention specifically conducted field measurements in a real, dynamic sea area with wind and wave-induced motion, using a small catamaran (2.5 meters long and 1.2 meters wide). The lever vector from the center of the inertial measurement unit to the phase center of the global navigation satellite system receiver antenna was calibrated using a total station. The results are as follows... Figures 2 to 13 As shown.

[0128] like Figure 2 As shown in the figure, the time series of the posterior standard deviation of the roll angle calculated by LS attitude determination under different baseline processing strategies are presented. It can be seen from the figure that the fluctuation amplitude of the roll angle standard deviation is reduced and the overall value is the lowest after adopting the absolute coordinate network adjustment strategy. This indicates that network adjustment effectively improves the internal geometric consistency under the short baseline configuration, providing a higher quality covariance input for filtering.

[0129] like Figure 3 As shown, the time series of posterior standard deviations of pitch angles calculated by LS attitude determination under different baseline processing strategies are presented. Similar to the roll angle, the standard deviation of the pitch angle after adjustment using the absolute coordinate network exhibits the best stability. Meanwhile, due to the near-coplanar configuration of the carrier antenna, the overall standard deviation of the pitch angle is significantly smaller than that of the roll angle, accurately reflecting the differences in observation accuracy of the system across different attitude components.

[0130] like Figure 4As shown, the time series of posterior standard deviations of the heading angle calculated by LS attitude calculation under different baseline processing strategies are presented. The low-fluctuation heading angle standard deviation output in real time, affected by the antenna configuration, is smaller than the roll angle standard deviation, just like the pitch angle standard deviation, and can continuously output high-confidence heading observation quality diagnostic indicators.

[0131] like Figure 5 The figure shows the scatter plot and distribution of the filter consistency verification results using the Normalized Innovation Square (NIS) test. The actual NIS time average in the figure is approximately 2.949 in actual parameter tuning, which is highly consistent with the theoretical expected value of 3.0 for three-dimensional attitude measurement, and the vast majority of samples fall within the 95% confidence interval. This indicates that the scalar adjustment factor was accurately calibrated, and the present invention adjusts and determines... Value, making a period of time The statistical expectation or time average value approaches the degrees of freedom of the measurement vector (for three-dimensional attitude measurement). (3), that is By matching this theoretical expected value, the scalar adjustment factor is adjusted. The selection and calibration of filters are necessary to ensure statistical consistency.

[0132] like Figure 6 , Figure 7 and Figure 8 As shown, Figure 7 yes Figure 6 Enlarged view of region A1 in the middle. Figure 8 yes Figure 6 The enlarged view of region A2 shows the time series comparison of heading angle errors between the traditional method and the method of this invention under dynamic sea conditions. The single-antenna EKF exhibits significant heading divergence and drift under this condition; while the present invention employs a multi-antenna adaptive weighted strategy (LS-CW EKF), which not only reduces heading drift but also shows a significant improvement compared to traditional adaptive algorithms (such as SHAKF) that suffer from statistical delay.

[0133] like Figure 9 , Figure 10 and Figure 11 As shown, Figure 10 yes Figure 9 Enlarged view of region B1 in the middle. Figure 11 yes Figure 9 The enlarged view of region B2 shows a time series comparison of roll angle errors between the traditional method and the method of this invention. Due to the short baseline effect, the original multi-antenna GNSS roll angle observations contain severe high-frequency fluctuation noise. Compared to the fixed-weight EKF method, which cannot adapt to noise changes, the method of this invention can respond instantaneously to severe fluctuations in measurement quality, and effectively suppresses noise in a smoother and more stable manner.

[0134] like Figure 12The figure shows the histogram of the roll angle error distribution of the traditional fixed-weight method (FR-EKF). Due to the severe influence of short-baseline time-varying noise, the traditional fixed-weight filter cannot adaptively adjust the system's confidence in the measurement data, resulting in a distinct bimodal structure in its error distribution. Furthermore, it deviates from the zero mean and exhibits a large standard deviation, indicating its limitations in handling dynamic time-varying noise.

[0135] like Figure 13 The image shows the histogram of the roll error distribution of the method (LS-CW EKF) of this invention. The system's error distribution was successfully corrected to a unimodal structure approaching zero mean. Compared to... Figure 8 The standard deviation is significantly reduced, proving that the present invention effectively alleviates the noise amplification effect caused by geometric configuration and greatly improves the robustness of attitude estimation.

[0136] Compared with the single-antenna EKF, the single-antenna EKF has a heading angle RMSE as high as 2.538° under dynamic sea conditions because the heading is unobservable. The multi-antenna method of this invention solves the heading drift problem.

[0137] Compared to the fixed-weight FR-EKF, the traditional fixed-weight FR-EKF has limitations in handling short-baseline time-varying noise, and its roll error exhibits a distinct bimodal distribution (RMSE of 0.223°). By employing the present invention (LS-CW EKF, adaptive weighting), the dynamic weighting strategy sensitively responds to geometric changes and noise fluctuations, correcting the roll error distribution to a near-zero-mean unimodal structure. The roll angle RMSE is reduced to 0.181° (an improvement of 18.8%), and the heading angle RMSE is reduced to 0.136° (an improvement of 10.5%), demonstrating higher accuracy and robustness.

[0138] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A least-squares covariance dynamically weighted satellite inertial navigation system attitude determination method, characterized in that, include: S1. Obtain the raw inertial data output by the inertial measurement unit, obtain the observation data output by the global navigation satellite system receiver array, perform absolute coordinate network adjustment on the observation data, extract the global navigation satellite system observation baseline vector in the navigation coordinate system, and calibrate the lever vector from the center of the inertial measurement unit to the phase center of the global navigation satellite system receiver antenna. The raw inertial data includes angular velocity and acceleration. S2. Construct a nonlinear observation model using the attitude rotation matrix and observation noise, linearize the nonlinear observation model using the Gauss-Newton iterative algorithm and solve it to obtain the attitude estimate and posterior covariance matrix of the global navigation satellite system. S3. Based on the original inertial data, according to the mechanical arrangement and mechanical arrangement equations of the inertial navigation system, the predicted attitude of the inertial navigation system is calculated, and the error state vector is constructed. At the same time, based on the continuous-time system error dynamics matrix recursively derived by the inertial navigation system, the error state vector and the state covariance are updated over time. S4. Introduce a filter into the posterior covariance matrix and combine it with a scalar adjustment factor to reconstruct the measurement noise covariance matrix of the extended Kalman filter. S5. Based on the attitude estimation value and posterior covariance matrix of the global navigation satellite system, a measurement innovation vector is constructed. Based on the reconstructed extended Kalman filter measurement noise covariance matrix, the Kalman gain is calculated and the state is updated. The fused attitude information is output and the error state is fed back. The inertial navigation system is corrected using the feedback error state. S1 includes, S1.1, acquiring raw inertial data output by the inertial measurement unit under a unified global navigation satellite system time reference, acquiring observation data output by the global navigation satellite system receiver array, wherein the number of global navigation satellite system receiver antennas is greater than or equal to 4; S1 includes S1.2, which involves extracting the global navigation satellite system observation baseline vector in the navigation coordinate system based on absolute coordinate network adjustment. Calibrate the lever vector from the center of the inertial measurement unit to the phase center of the global navigation satellite system receiver antenna. , For the navigation coordinate system, For the carrier coordinate system, For the antenna index of the Global Navigation Satellite System receiver, Indicates lever arm, Represents the observed value; S2 includes, S2.1, constructing a nonlinear observation model: ； In the formula, Here is the attitude rotation matrix. The attitude angle vector to be estimated includes the roll angle, pitch angle, and yaw angle. For the first Observation noise of the root antenna; S2 includes S2.2, which uses the Gauss-Newton iterative algorithm to linearize the nonlinear observation model: ； In the formula, This represents the number of iterations for the linearization equation. For the first Linearization error of the next iteration For the first The Jacobian matrix of the next iteration; S2 includes S2.3, which, after iterative convergence, outputs the global navigation satellite system attitude estimate for the current epoch. Simultaneously output the posterior covariance matrix. : ； ； In the formula, The Jacobian design matrix reflects the antenna geometry. For the baseline prior weight matrix of the global navigation satellite system, The unit-weighted posterior variance is calculated based on the solution residuals. For the residual vector, For degrees of freedom, The number of observation equations, The number of parameters to be estimated. It is the transpose symbol; S3 includes S3.1, which involves calculating the predicted attitude of the inertial navigation system at the current epoch by integrating and recursively applying the inertial navigation mechanics programming equations based on the angular velocity and acceleration measured by the inertial measurement unit, according to the mechanical arrangement of the inertial navigation system. ; S3 includes S3.2, constructing the error state vector. , This includes attitude error, velocity error, position error, gyroscope bias error, and accelerometer bias error; Based on the continuous-time system error dynamics matrix derived from the inertial navigation system Discretization yields the state transition matrix. : ； In the formula, It is the identity matrix. The discrete time step; according to conduct Time update and state covariance Time update: ； ； In the formula, For the calendar year, For prior error state estimation, For posterior error state estimation, Here is the state transition matrix. To estimate the error covariance matrix a priori, For the posterior estimation of the error covariance matrix, Let be the system process noise covariance matrix.

2. The least squares covariance dynamically weighted satellite inertial navigation system attitude determination method according to claim 1, characterized in that, S4 includes, in the current epoch, Introducing a filter, along with a scalar adjustment factor Multiply to reconstruct the measurement noise covariance matrix of the current extended Kalman filter. : ； In the formula, For the first The specific time corresponding to each epoch; Using normalized innovation square Test pair Calibration is performed, including the introduction of... : ； In the formula, For the first epochs , For the first The new information vector of an epoch, For the first The inverse of the epochal information covariance matrix, Obeying the degree of freedom The chi-square distribution, The theoretical expected value is ; Set up a sliding window ,exist Internal computation mean : ； according to Calibration : ； In the formula, For the first Scalar adjustment factor of epoch, For the first epochs The mean.

3. The least squares covariance dynamically weighted satellite inertial navigation system attitude determination method according to claim 2, characterized in that, S5 includes, S5.1, constructing the measurement innovation vector. : ； In the formula, For generalized subtraction of posture, for The predicted attitude output by the mechanical orchestration of the inertial navigation system at all times. for At any given time, the attitude estimate of the global navigation satellite system.

4. The least squares covariance dynamically weighted satellite inertial navigation system attitude determination method according to claim 3, characterized in that, S5 includes S5.2, based on Calculate the covariance matrix of measurement information and Kalman gain : ； ； In the formula, For the measurement matrix, The prior estimate is the error covariance matrix; State updates include, utilizing right and Update: ； ； In the formula, For posterior error state estimation, This is the posterior estimation error covariance matrix.

5. The least-squares covariance dynamically weighted satellite inertial navigation system attitude determination method according to claim 4, characterized in that, S5 includes, S5.3, utilizing The attitude error in Perform compensation and output the fused pose information; Will The zero-bias errors of the gyroscope and accelerometer are fed back to the mechanical arrangement of the inertial navigation system to compensate for the measured values ​​of the inertial navigation system and complete the calibration.

Images