Initial alignment method based on mahalanobis distance interference resistance

Abstract

The invention relates to the field of inertial navigation systems, and discloses an anti-interference initial alignment method based on mahalanobis distance, which comprises the following steps: initializing; updating to obtain a first observation vector and a second observation vector at the current moment; calculating a residual vector at the current moment, and further calculating a mahalanobis distance at the current moment; judging whether an abnormal value criterion condition is met or not; if the abnormal value criterion condition is not met, updating a mean value and a covariance matrix of the residual vector at the next moment by adopting the residual vector obtained at the current moment and Mahalanobis distance recursion; if the abnormal value criterion condition is met, pausing to update the mean value and the covariance matrix of the residual vector; and updating the flexible weight and the observation matrix, and continuing to complete the alignment of the movable base by adopting an attitude determination algorithm. According to the method, an improved mahalanobis distance anomaly detection criterion is constructed, the anomaly condition of a residual vector formed by observation vectors is judged, and a weight factor of an observation matrix is introduced, so that anomaly interference in the alignment process is inhibited.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of inertial navigation system, and particularly to an initial alignment method based on Mahalanobis distance anti-interference. BACKGROUND

[0002] In a strapdown inertial navigation system (SINS) and Doppler velocity log (DVL) combined navigation system, initial alignment is a basic step for system startup, and its accuracy has a decisive influence on the stability and accuracy of subsequent navigation calculation. The particularity of the moving base environment, such as no external absolute position update for a long time, causes the initial alignment error to continue to propagate and accumulate, and eventually significantly affects the success or failure of the entire navigation task. Therefore, achieving high-precision and high-robust initial alignment is a prerequisite for reliable navigation of moving vehicles, aircraft, underwater vehicles and other moving carriers.

[0003] However, as a key sensor providing external speed reference, DVL is easily disturbed by complex dynamic environments in actual operation when working underwater. These disturbances often cause outliers or non-Gaussian noise in DVL observation data that deviate from the normal distribution. Similar problems also exist in speed sensors used by other moving base platforms. Traditional initial alignment algorithms based on least squares estimation or fixed linear weighting usually assume that the observation noise is Gaussian distributed and has no significant outliers. When there are outliers in DVL data, these algorithms will deviate significantly from the optimal estimate, causing the attitude calculation error to increase sharply, and causing attitude drift and even system divergence in subsequent navigation calculation. Therefore, in a complex and dynamic moving base environment, improving the robustness of the initial alignment process and enhancing the algorithm's ability to suppress observation anomalies have become one of the key challenges and hotspots in the current moving base navigation system research.

[0004] In a SINS / DVL combined navigation system, the accuracy of initial alignment directly affects the accuracy of subsequent navigation. As a speed reference sensor, DVL is easily disturbed by various factors such as turbulence or multipath effect in a complex underwater environment, often producing outliers. The presence of these outliers can seriously affect the construction accuracy of the observation data, thereby reducing the accuracy of initial alignment. In practical applications, DVL data may introduce abnormal observations due to environmental changes, causing the residuals to deviate, and thus affecting the overall alignment accuracy. Existing robust alignment methods do not fully consider the uncertainty of the statistical properties of the observations, leading to misjudgment and improper handling of outliers, which can further affect the stability of the alignment results. SUMMARY

[0005] The present application aims at overcoming the deficiencies of the prior art, and provides an initial alignment method based on Mahalanobis distance anti-interference.

[0006] The purpose of the present application is achieved by the following technical solutions:

[0007] The first aspect of the present application provides an initial alignment method based on Mahalanobis distance anti-interference, comprising the following steps:

[0008] S1: initialization 、 、 、 = , wherein: is the mean of the residual vector, is the covariance matrix of the residual vector, denotes the nominal covariance matrix of the residual vector, denotes the mean of the calculated Mahalanobis distance, denotes the variance of the calculated Mahalanobis distance, denotes the nominal Mahalanobis distance variance;

[0009] S2: update to obtain the first observation vector and the second observation vector at the current time , wherein the first observation vector and the second observation vector are in the discrete form of the sliding window as follows:

[0010] ;

[0011] ;

[0012] In the formula, denotes the attitude transformation matrix of the current time body relative to the initial time body , denotes the attitude transformation matrix of the time body relative to the initial time body , and denote the earth-fixed velocity under the time body and the time body respectively, is the starting time of the current sliding window; is the discrete expression of the first integral term, and the first integral term is: , denotes the specific force of the accelerometer output at the time, This represents the integration time variable in the integral term; The discretized expression corresponding to the second integral term is: , express The projection of the Earth's rotational angular velocity onto the system at any given moment. express Ground speed under the time-carrying system; express Time navigation coordinate system Relative to the initial time vehicle coordinate system The attitude change matrix, Represents the period of discretization. As a unit array, This represents the antisymmetric matrix formed by the rotational angular velocity vectors of the navigation coordinate system relative to the inertial coordinate system. This represents the gravity vector projection in the navigation coordinate system;

[0013] S3: Utilize the first observation vector from step S2 Second observation vector Calculate the current time residual vector :

[0014] ;

[0015] And further calculate the current time. Mahalanobis distance :

[0016] ;

[0017] ;

[0018] ;

[0019] In the formula, Represents the i-th residual scalar;

[0020] S4: Determine whether the outlier criterion is met:

[0021] ;

[0022] in, , ;

[0023] If the outlier criterion is not met, then the residual vector obtained at the current time step is used. and Mahal distance Recursively update the next time step , , and accumulatively update , ; if the outlier criterion condition is satisfied, suspend the update , , directly take the current time , as the observation matrix , of the next time , , and continue to accumulatively update

[0024] S5: update the flexible weight :

[0025] ;

[0026] wherein is a scale factor, is an index control coefficient; then update the observation matrix at the time using the flexible weight , and continue to complete the moving base alignment using the attitude determination algorithm:

[0027] ;

[0028] wherein represents the observation matrix at the time , represents the left multiplication matrix operator of the quaternion composed of the second observation vector ; and represents the right multiplication matrix operator of the quaternion composed of the first observation vector . Further, in step S4, if the outlier criterion condition is not satisfied, the residual vector

[0029] obtained at the current time and the Mahalanobis distance are used to recursively update the , of the next time , including:

[0030] updating the number of data points:

[0031] ;

[0032] represents the total number of non-outlier data points observed from the initial time to the current time , and represents the total number of non-outlier data points observed from the initial time to the last time ;

[0033] update the mean value:

[0034] ;

[0035] represents the mean vector calculated based on the previous data points, represents the mean vector calculated based on the previous data points;

[0036] updating the covariance matrix: ;

[0037] represents the covariance matrix calculated based on the previous data points, represents the covariance matrix calculated based on the previous data points.

[0038] Further, in step S4, if the outlier criterion condition is satisfied, the updating of , is suspended, and the current time , is directly taken as the next time , , including:

[0039] updating the number of data points:

[0040] ;

[0041] updating the mean:

[0042] ;

[0043] updating the covariance matrix:

[0044] .

[0045] The beneficial effects of the present application are:

[0046] In an exemplary embodiment of the present application, by constructing an improved Mahalanobis distance outlier detection criterion, the abnormal situation of the residual vector composed of the observation vector is judged, the weight factor of the observation matrix is introduced, so as to suppress the abnormal interference in the alignment process; a reliable solution is provided for the high-precision initial alignment of the moving base navigation system in complex environment, and good practicability and real-time application potential are possessed. BRIEF DESCRIPTION OF DRAWINGS

[0047] Figure 1 The flowchart of the initial alignment method based on Mahalanobis distance anti-interference provided by an exemplary embodiment of the present application;

[0048] Figure 2 A schematic diagram of a change curve of a posture angle provided for an exemplary embodiment of the present application;

[0049] Figure 3 A schematic diagram of a change curve of a speed provided for an exemplary embodiment of the present application;

[0050] Figure 4 A schematic diagram of a change curve of a trajectory provided for an exemplary embodiment of the present application;

[0051] Figure 5 A schematic diagram of a change curve of a roll angle error provided for an exemplary embodiment of the present application;

[0052] Figure 6 A schematic diagram of a change curve of a pitch angle error provided for an exemplary embodiment of the present application;

[0053] Figure 7 A schematic diagram of a change curve of a yaw angle error provided for an exemplary embodiment of the present application;

[0054] Figure 8 A schematic diagram of a change curve of a posture angle provided for another exemplary embodiment of the present application;

[0055] Figure 9 A schematic diagram of a change curve of a speed provided for another exemplary embodiment of the present application;

[0056] Figure 10 A schematic diagram of a change curve of a trajectory provided for another exemplary embodiment of the present application;

[0057] Figure 11 A schematic diagram of a change curve of a roll angle error provided for another exemplary embodiment of the present application;

[0058] Figure 12 A schematic diagram of a change curve of a pitch angle error provided for another exemplary embodiment of the present application;

[0059] Figure 13 A schematic diagram of a change curve of a yaw angle error provided for another exemplary embodiment of the present application. DETAILED DESCRIPTION

[0060] The technical solutions of the present application will be described clearly and completely below with reference to the drawings. Obviously, the described embodiments are some but not all of the embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work fall within the scope of the present application. In addition, the technical features involved in the different embodiments of the present application described below can be combined with each other as long as they do not conflict with each other.

[0061] In strapdown inertial navigation systems (SINS) and Doppler log (DVL) integrated navigation systems, initial alignment accuracy is crucial for subsequent navigation performance. Because DVL is susceptible to interference in complex underwater environments, traditional initial alignment methods suffer performance degradation under outlier interference, failing to meet high-precision requirements. Specifically:

[0062] For SINS / DVL integrated navigation systems, the ground velocity provided by DVL can be used to assist SINS in initial alignment. First, the attitude matrix is ​​aligned using a chain rule. Decomposed into:

[0063] ;

[0064] in For the current moment, Let be the attitude transformation matrix of the carrying system relative to the navigation system at the current moment. Navigation system for the current moment Navigation system relative to the initial time The attitude transformation matrix, Indicates the current time of the system. Relative to the initial time of the system The attitude transformation matrix, This is the initial time-loaded system. Navigation system relative to the initial time The attitude matrix is ​​a constant matrix. It contains two time-varying attitude matrices. and It can be updated using the differential equation of the attitude matrix:

[0065] ;

[0066] ;

[0067] In the formula, Represents the attitude matrix The differential, This represents the projection of the rotational angular velocity of the navigation coordinate system relative to the inertial coordinate system onto the navigation coordinate system. express The differential, This represents the angular velocity output by the gyroscope; in the time-varying attitude matrix. and Once obtained, the core of optimized alignment is to determine the constant attitude matrix. .

[0068] Based on the specific force equation, the observation vector equation can be obtained as follows:

[0069] ;

[0070] The first observation vector Second observation vector It can be represented as

[0071] (5);

[0072] ;

[0073] In the above formula, Here, represents the lower bound of integration, and represents the start time of the current sliding window. This represents the current moment, i.e., the maximum score. When... hour( (where the sliding window has a fixed length), the above integral is the result of the time taken within the window length. The sliding integral within. This represents the projection of the current velocity relative to the ground in the carrier coordinate system b. express Time-based system Relative to the initial time of the system The attitude transformation matrix, express Time-based system Relative to the initial time of the system The attitude transformation matrix, This indicates the specific force output by the accelerometer. This represents the time variable for integration within the integration interval. The projection of the Earth's rotational angular velocity onto the system of reference. express The projection of the velocity relative to the ground at any given moment onto the vehicle's coordinate system; express The attitude transformation matrix of the navigation coordinate system at time t relative to the initial navigation coordinate system. This represents the projection of the gravitational acceleration vector onto the navigation coordinate system.

[0074] Observation vector and In the sliding window The discretized form is as follows:

[0075] ;

[0076] ;

[0077] in This is the time series at the current moment. The start time of the current sliding window Time series, The length of the sliding window. for the solution period, meet . The discrete expression corresponding to the first integral term in formula (5) is The discrete expression corresponding to the second integral term in formula (5) is represents the attitude transformation matrix of the navigation coordinate system at the moment relative to the navigation coordinate system at the initial moment, represents a 3x3 unit matrix, represents the rotation angular velocity of the navigation coordinate system at the moment relative to the carrier coordinate system.

[0078] In the SINS / DVL integrated navigation system, the accuracy of the initial alignment directly affects the accuracy of the subsequent navigation. As a velocity reference sensor, the DVL is easily disturbed by various factors in complex underwater environments, such as turbulence or multipath effects, often producing outliers. The presence of these outliers can seriously affect and the construction accuracy, thereby reducing the accuracy of the initial alignment. The construction of the first observation vector and the second observation vector involves multiple parameters, where the second observation vector is updated by and and is not easily disturbed by sensor output. The first observation vector is composed of the outputs of the IMU and the DVL. In actual applications, DVL data may introduce abnormal observations due to environmental changes, causing the deviation of residuals and further affecting the overall alignment accuracy. Existing robust alignment methods do not fully consider the uncertainty of the statistical properties of the observations, leading to misjudgment and improper handling of outliers, which can further affect the stability of the alignment result. Therefore, in the following exemplary embodiments, the outlier discrimination method based on Mahalanobis distance can effectively handle this problem by recursively calculating the discrimination threshold in real time by recursively calculating the discrimination threshold based on all historical Mahalanobis distances, thereby dynamically detecting and suppressing the influence of outliers on the alignment accuracy.

[0079] In order to solve the problems of outlier misjudgment during detection and suboptimal weighting during weighting, the following exemplary embodiments provide a new detection method. Mahalanobis distance is a distance measurement method that considers the correlation and scale difference between data features, and can effectively reflect the dispersion degree between data outliers and sample mean. However, when using Mahalanobis distance, sample data needs to follow a normal distribution, and the data obtained in the initial alignment process does not meet this condition. Based on the first observation vector and the second observation vector the following exemplary embodiments construct a residual vector As the data vector in Mahalanobis distance:

[0080] ;

[0081] Let be the th observation residual scalar , be the time series at the current time. Therefore, the square of the Mahalanobis distance based on can be expressed as:

[0082] ;

[0083] ;

[0084] ;

[0085] In the above formula, is the mean of the residual vector, is the covariance matrix of the residual vector. In actual use, when the observation vector and are outliers, if the next time and are directly updated using formulas (11) and (12), the update accuracy of the subsequent Mahalanobis distance will be affected.

[0086] Therefore, in the following exemplary embodiments, an outlier criterion based on cumulative statistics is established as follows:

[0087] ;

[0088] ;

[0089] ;

[0090] If the updated Mahalanobis distance at the current time satisfies formula (13), it is considered that the sample point is an outlier relative to the entire sample point, and the sample does not participate in the update of the sample mean and covariance, preventing the statistics , from being contaminated, but in order to improve the stability in the detection process, the , are still accumulated and updated.

[0091] Based on the outlier criterion of formula (13), the following exemplary embodiments construct a flexible weight update formula for the observation vector:

[0092] ;

[0093] wherein is an exponential control coefficient, and the experiment finds that the optimal value is . is a scale factor, which controls the mapping relationship between the residual size and the weight attenuation, and for the unity of the implementation of the Mahalanobis distance statistical criterion, the optimal setting is , which is the critical value of the chi-square distribution with the degree of freedom of at the confidence level of 0.975.

[0094] Based on the abnormal value criterion of formula (13), the weight coefficient of formula (16) obtained under different conditions acts on the attitude observation matrix in the construction stage, and the weight of each new information item is set to , so that the robustness of the optimization alignment model to abnormal observation can be further enhanced.

[0095] The updating formula of the observation matrix can be obtained as follows:

[0096] ;

[0097] According to formula (4), (7)-(17), the initial alignment method based on the Mahalanobis distance anti-interference provided by the example embodiment of the application is obtained, and the method is shown in formula (18) as follows: Figure 1 , which includes the following steps:

[0098] S1: initialization , , , = , wherein: is the mean of the residual vector, is the covariance matrix of the residual vector, denotes the nominal covariance matrix of the residual vector, denotes the mean of the calculated Mahalanobis distance, denotes the variance of the calculated Mahalanobis distance, denotes the nominal Mahalanobis distance variance;

[0099] S2: according to formula (7) and formula (8), the first observation vector and the second observation vector of the current time are updated and obtained, wherein the first observation vector and the second observation vector are in the discrete form of the sliding window as follows:

[0100] ;

[0101] ;​

[0102] wherein, denotes the current time instant body frame denotes the initial time instant body frame attitude transformation matrix, denotes the current time instant body frame denotes the initial time instant body frame attitude transformation matrix, and denote the ground velocity in the current time instant body frame and in the initial time instant body frame respectively, is the start time instant of the current sliding window; is the discretized expression of the first integral term, which is: , denotes the specific force of the accelerometer output at the current time instant, denotes the integration time variable in the integral term; is the discretized expression of the second integral term, which is: , denotes the projection of the earth rotation angular velocity in the current time instant body frame, denotes the ground velocity in the current time instant body frame; denotes the attitude transformation matrix of the navigation coordinate system with respect to the initial time instant body coordinate system , denotes the discretized period, is the identity matrix, denotes the skew-symmetric matrix composed of the rotation angular velocity vector of the navigation coordinate system with respect to the inertial coordinate system, denotes the projection of the gravity vector in the navigation coordinate system; S3: according to formula (9), the residual vector at the current time instant is calculated using the first observation vector and the second observation vector

[0103] in step S2:

[0104] ;

[0105] and further according to formula (10), the Mahalanobis distance at the current time instant is calculated:

[0106] ;​​​​

[0107] ;

[0108] ;

[0109] In the formula, Represents the i-th residual scalar;

[0110] S4: Based on formula (13), determine whether the outlier criterion is met:

[0111] ;

[0112] in, , ;

[0113] If the outlier criterion is not met, then the residual vector obtained at the current time step is used according to formulas (11) and (12). and Mahal distance Recursively update the next time step , and cumulative updates , If the outlier criterion is met, updates will be paused. , Directly set the current time , As the next moment , But continue to accumulate updates , ;

[0114] S5: Update the flexible weights according to formula (16). :

[0115] ;

[0116] In the formula, As a scale factor, The index control coefficient is used; then, according to formula (17), flexible weights are applied. renew Observation matrix at time The attitude determination algorithm is then used to continue aligning the moving base.

[0117] ;

[0118] In the formula, express The observation matrix at time, Represents the second observation vector The left-multiplication matrix operator of the quaternions formed; Represents the first observation vector The right-multiplication matrix operator of the quaternions formed.

[0119] In summary, the initial alignment method based on Mahalanobis distance for anti-interference proposed in this exemplary embodiment determines the anomalies in the residual vector formed by the observation vectors by constructing an improved Mahalanobis distance anomaly detection criterion and introduces a weighting factor for the observation matrix, thereby suppressing abnormal interference in the alignment process.

[0120] More preferably, in an exemplary embodiment, in order to improve computational efficiency, in step S4, formulas (11) and (12) can be calculated using a recursive update method. In step S4, if the outlier criterion is not met, the residual vector obtained at the current time is used. and Mahal distance Recursively update the next time step , ,include:

[0121] Update data points:

[0122] ;

[0123] Represents the time from the initial moment to the current moment. The total number of non-outlier data points observed. This represents the time from the initial moment to the previous moment. The total number of non-outlier data points observed;

[0124] Update the mean:

[0125] ;

[0126] Indicates based on the previous The mean vector calculated from each data point Indicates based on the previous The mean vector calculated from each data point;

[0127] Update the covariance matrix: ;

[0128] Indicates based on the previous The covariance matrix calculated from each data point Indicates based on the previous The covariance matrix is ​​calculated from each data point.

[0129] Specifically, in the present exemplary embodiment, with the above formula, on the premise that there is no abnormal observation point, the mean and covariance can be updated according to the above recursive formula.

[0130] More preferably, in an exemplary embodiment, in step S4, if the abnormal value criterion condition is met, the updating of the mean and covariance is suspended 、 , and the current time 、 is directly taken as the 、 of the next time

[0131] Update the number of data points:

[0132] ;

[0133] Update the mean:

[0134] ;

[0135] Update the covariance matrix:

[0136] .

[0137] Specifically, in the present exemplary embodiment, with the above formula, when there is an abnormal observation point, the mean and variance are taken as the values of the last time to avoid being contaminated by the abnormal observation point and affecting the judgment of the next time.

[0138] More preferably, in an exemplary embodiment, for step S4, the observation matrix of the current time is updated according to formula (17) using the flexible weight , and the pose determination algorithm is used to continue to complete the moving base alignment, and the specific implementation mode can be:

[0139] Since the attitude matrix and the quaternion can be converted to each other, the corresponding observation formula can be expressed as a quaternion as follows:

[0140] ;

[0141] is a quaternion multiplication operation, and the above formula can be rearranged to obtain

[0142] ;

[0143] According to the new observation vector equation, the loss function of the observation equation can be constructed as follows:

[0144] ;​

[0145] Where the matrix for:

[0146] ;

[0147] The attitude quaternion can be determined The problem is transformed into a constrained optimization problem:

[0148] ;

[0149] By updating the observation matrix Determine the optimal pose quaternion Thus, the attitude matrix is ​​obtained. .

[0150] The following content will use experiments to demonstrate its effectiveness:

[0151] Designing simulation experiments in a dynamic marine environment: ( Figure 2 Attitude angle change curve; Figure 3 Velocity change curve; Figure 4 (Trajectory Change Curve) The vehicle executes a pre-generated, 300-second high-maneuver trajectory. This trajectory simulates typical motion in a dynamic marine environment, and its characteristics include... Figure 2 and Figure 3 As shown, the trajectory includes acceleration, turning, and pitch and roll maneuvers with an amplitude of approximately ±30°. The initial position of the trajectory is latitude 34.5°, longitude 118.8°, and altitude 50m. The sensor configuration is shown in Table 1, including an IMU (100 Hz, gyroscope bias 0.01° / h, accelerometer bias 100μg) and a DVL (5 Hz, velocity noise standard deviation 0.05m / s, randomly injected outliers with a probability of 2% and a noise level of 200 times). The comparison methods include conventional optimized alignment (OBA), OBA without additional outliers (OBAr), existing robust alignment (Robust OBA), and the Mahalanobis distance-based OBA method (MOBA) proposed in the above exemplary embodiment.

[0152] Table 1 Sensor Configuration

[0153]

[0154] Figure 5 –7 shows the root mean square error (RMSE) curve for the attitude angle. Figures 5-7The roll angle error curve, the pitch angle error curve, and the yaw angle error curve are shown in FIG. 6, respectively. When the DVL outliers appear, the outliers reduce the performance of the OBA method, and the error fluctuation is large. However, the OBAM and ROBA methods are not greatly disturbed. The average RMSE of the last 50 seconds is shown in Table 2. The root mean square error of the heading angle error of the OBAM is 0.8890°, which is better than all the comparison methods. This is because the OBAM calculates the Mahalanobis distance threshold in real time through the sliding window. When the outliers appear, the Mahalanobis distance D exceeds the threshold and triggers the statistical freezing mechanism, which can effectively block the error propagation and pollution. Under strong maneuvering and abnormal data interference, the MOBA blocks the error propagation by virtue of the strong mechanism, and suppresses the heading angle fluctuation amplitude within 1°. In addition, the parameter configuration is simple, and the calculation efficiency is equivalent to that of the classical optimization alignment method. The MOBA provides a reliable solution with milliradian-level precision and strong anti-interference capability for high-precision initial alignment of unmanned carriers in complex environments.

[0155] Table 2 Comparison of average RMSE of attitude angles

[0156]

[0157] To verify the actual performance of the initial alignment method based on the Mahalanobis distance anti-interference, a sea trial experiment data is used for initial alignment, and the data time length is 300 seconds. The attitude angle change curve, the velocity change curve, and the motion trajectory change curve are shown in FIG. 7, respectively. Figures 8-10 In the experiment, a fiber-optic strapdown inertial navigation system (FSINS), a DVL, and a high-precision integrated navigation system are used for data acquisition. The specific configuration is shown in Table 3. The FSINS is composed of a gyroscope (zero bias stability , angular random walk ) and an accelerometer (zero bias stability is , velocity random walk ). The output frequency is 98 Hz. The output frequency of the Doppler velocity log (DVL) is 1 Hz, and the velocity measurement noise standard deviation is 0.1 m / s. The output of the high-precision integrated navigation system is used as the data reference. To simulate the occasional abnormal observation in the actual system, the outliers are randomly injected in the DVL output at a probability of 2%, so as to verify the robustness of the algorithm under non-ideal observation conditions. The alignment results of the MOBA are compared with those of the OBAr, OBA, and ROBA. The experimental results are shown in FIG. 8 and Table 3. Figures 11-13

[0158] Table 3 Sensor configuration

[0159] Figure 11 ​- 13 shows the roll, pitch and yaw error curves, and Table 4 shows the RMSE error statistics after 50 seconds of alignment. It can be seen that both robust methods (ROBA and MOBA) show good suppression of outliers, and the MOBA method proposed in the above example embodiment has the highest accuracy in yaw estimation, with an error of 0.4225°, which is significantly better than other methods. In terms of horizontal attitude estimation, the errors of each method remain at a low level, indicating that the method of the above example embodiment does not sacrifice the estimation performance of the horizontal attitude while ensuring the accuracy of the heading. The results show that the robust optimization alignment method based on Mahalanobis distance proposed in the above example embodiment exhibits significant superiority in the dynamic base marine environment, and by recursively calculating the adaptive threshold of Mahalanobis distance, it accurately isolates the sudden outliers and continuous interference in the DVL velocity measurement, and the heading angle alignment accuracy is optimal compared with existing methods, and the pitch and roll angle errors are also small.

[0160] Table 4 Comparison of average RMSE of attitude angles

[0161]

[0162] The initial alignment method based on Mahalanobis distance anti-interference proposed in the above example embodiment has verified its effectiveness and superiority in the dynamic base marine environment. By adjusting the adaptive Mahalanobis distance threshold based on cumulative statistics, this method can identify and suppress sudden outliers and interference in DVL measurement in real time, and ensure stable convergence of attitude error. Experimental results show that compared with traditional static methods and other robust alignment methods, the method of the above example embodiment has significant performance improvement in terms of heading angle accuracy, pitch angle and roll angle error. Therefore, this method provides a reliable solution for high-precision initial alignment of dynamic base navigation systems in complex environments, and has good practicality and real-time application potential.

[0163] Obviously, the above embodiments are only examples for clarity, and are not limiting of the embodiments. For those skilled in the art, other different forms of changes or variations can be made on the basis of the above description. All embodiments do not need to be exhausted. The obvious changes or variations derived therefrom are still within the protection scope of the present application.

Claims

1. An initial alignment method based on Mahalanobis distance for interference resistance, characterized in that: Includes the following steps: S1: Initialization , , , = ,in: The mean of the residual vector. Let covariance be the residual vector. This represents the nominal covariance matrix of the residual vector. This represents the mean of the calculated Mahalanobis distance. This represents the variance of the calculated Mahalanobis distance. This represents the nominal Mahalanobis distance variance; S2: Update to get the current time. First observation vector Second observation vector The first observation vector Second observation vector In the sliding window The discretized form is as follows: ； ； In the formula, Indicates the current time of the system. Relative to the initial time system The attitude transformation matrix, express Time-based system Relative to the initial time of the system The attitude transformation matrix, and They represent Time and Ground speed under the time-carrying system This indicates the start time of the current sliding window; Here is the discretized expression for the first integral term, which is: , express The ratio of the accelerometer output at any given time. This represents the integration time variable in the integral term; The discretized expression corresponding to the second integral term is: , express The projection of the Earth's rotational angular velocity onto the system at any given moment. express Ground speed under the time-carrying system; express Time navigation coordinate system Relative to the initial time vehicle coordinate system The attitude change matrix, Represents the period of discretization. As a unit array, This represents the antisymmetric matrix formed by the rotational angular velocity vectors of the navigation coordinate system relative to the inertial coordinate system. This represents the gravity vector projection in the navigation coordinate system; S3: Utilize the first observation vector from step S2 Second observation vector Calculate the current time residual vector : ； And further calculate the current time. Mahalanobis distance : ； ； ； In the formula, Represents the i-th residual scalar; S4: Determine whether the outlier criterion is met: ； in, , ; If the outlier criterion is not met, then the residual vector obtained at the current time step is used. and Mahal distance Recursively update the next time step , and cumulative updates , If the outlier criterion is met, updates will be paused. , Directly set the current time , As the next moment , But continue to accumulate updates , ; S5: Update flexible weights : ； In the formula, As a scale factor, The index control coefficient is then used; then flexible weights are applied. renew Observation matrix at time The attitude determination algorithm is then used to continue aligning the moving base. ； In the formula, express The observation matrix at time, Represents the second observation vector The left-multiplication matrix operator of the quaternions formed; Represents the first observation vector The right-multiplication matrix operator of the quaternions formed.

2. The initial alignment method based on Mahalanobis distance for interference resistance according to claim 1, characterized in that: In step S4, if the outlier criterion is not met, the residual vector obtained at the current time is used. and Mahal distance Recursively update the next time step , ,include: Update data points: ； Represents the time from the initial moment to the current moment. The total number of non-outlier data points observed. This represents the time from the initial moment to the previous moment. The total number of non-outlier data points observed; Update the mean: ； Indicates based on the previous The mean vector calculated from each data point Indicates based on the previous The mean vector calculated from each data point; Update the covariance matrix: ； Indicates based on the previous The covariance matrix calculated from each data point Indicates based on the previous The covariance matrix is ​​calculated from each data point.

3. The initial alignment method based on Mahalanobis distance for interference resistance according to claim 2, characterized in that: In step S4, if the outlier criterion is met, then the update is paused. , Directly set the current time , As the next moment , ,include: Update data points: ； Update the mean: ； Update the covariance matrix: 。

Images