A navigation method based on adaptive unscented Kalman filtering of threshold processing, storage medium and equipment

Abstract

The application discloses a navigation method based on threshold processing adaptive unscented Kalman filtering, a storage medium and equipment, comprising obtaining and preprocessing data of a navigation coordinate system and a target coordinate system, constructing a state space system and an observation equation of the navigation coordinate system and the target coordinate system, and obtaining a state distribution relationship of the navigation coordinate system and the target coordinate system; in the state space system, unscented transformation operation is performed on a target point, and a target meeting a selection condition is obtained as a sampling point; weighted calculation is performed on the sampling point, and a predicted value of a state space system state at a specified moment is obtained; based on the predicted value of the state space system, unscented transformation operation is performed on the sampling point, and a sampling point meeting the selection condition is obtained as a new sampling point; the new sampling point is brought into the observation equation, and a predicted value of an observation value at the specified moment is obtained; after updating of measurement noise and a gain matrix, the predicted value of the observation value at the specified moment is output, and navigation is performed.

Description

Technical Field

[0001] This invention relates to a navigation method, storage medium, and device based on threshold processing and adaptive unscented Kalman filtering, belonging to the field of communication and navigation technology. Background Technology

[0002] With the development of positioning and navigation technologies, the requirements for target positioning accuracy are becoming increasingly stringent. While traditional single navigation technologies are simple in design, they all have certain limitations and cannot adapt to complex environments. The emergence of integrated navigation has solved this problem by combining two or more navigation systems to compensate for each other's shortcomings.

[0003] Traditional Kalman filtering prediction systems are only suitable for linear systems. To address nonlinear issues, adaptive unscented Kalman filtering has emerged. With its low computational cost and high accuracy, it has become the mainstream filtering algorithm for integrated navigation.

[0004] In real-world environments, noise can interfere with filters, causing some out-of-range errors. Conventional filters cannot identify these errors and treat them as normal values ​​in calculations, inevitably leading to data deviations. Summary of the Invention

[0005] The technical problem this invention aims to solve is to overcome the shortcomings of existing technologies and provide a navigation method, storage medium, and device based on threshold processing and adaptive unscented Kalman filtering. This method utilizes an adaptive unscented Kalman filter as a combined navigation filter to fuse data from two navigation systems and adaptively estimate external noise. Before data processing, a threshold discrimination mechanism is used to remove severely disturbed outliers, improving the system's robustness. Furthermore, the noise estimation process of the filter is simplified, reducing the system's computational load.

[0006] To achieve the above objectives, in a first aspect, the present invention provides a navigation method based on threshold processing and adaptive unscented Kalman filtering, comprising:

[0007] Acquire and preprocess the data of the navigation coordinate system and the target coordinate system, construct the state space system and observation equations of the navigation coordinate system and the target coordinate system, and obtain the state distribution relationship of the navigation coordinate system and the target coordinate system;

[0008] In a state-space system, an unscented transformation operation is performed on the target point to obtain the target points that meet the selection criteria as sampling points;

[0009] Weighted calculations are performed on the sampling points to obtain the predicted value of the state space system state at a specified time.

[0010] For state-space systems based on predicted values, an unscented transformation operation is performed on the sampling points to obtain sampling points that meet the selection criteria as new sampling points.

[0011] By substituting the new sampling points into the observation equation, the predicted values ​​of the observations at the specified time are obtained.

[0012] After updating the measurement noise and gain matrix, the predicted value of the observation at the specified time is output for navigation.

[0013] Furthermore, the predicted values ​​of the state space system state include:

[0014] The mean and covariance of a state-space system.

[0015] Furthermore, the selection criteria include:

[0016] For a target or sampling point to be selected, are the mean and covariance after the unscented transformation operation equal to the mean and covariance of the state space vector distribution in the state space system to which it belongs?

[0017] In a second aspect, the present invention provides a computer-readable storage medium for storing one or more programs.

[0018] The one or more programs include instructions that, when executed by a computing device, cause the computing device to perform any of the methods described according to the first aspect.

[0019] Thirdly, the present invention provides a computing device comprising:

[0020] One or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs including instructions for performing any of the methods described in accordance with the first aspect.

[0021] The beneficial effects achieved by this invention are as follows:

[0022] This invention provides a navigation method based on threshold processing and adaptive unscented Kalman filtering. This invention uses unscented Kalman as the filter of the integrated navigation system. While meeting the requirements of nonlinear systems, it avoids model loss. At the same time, the calculation accuracy is improved to the third order, and the calculation is significantly reduced.

[0023] This invention combines Sage-Husa to adaptively estimate measurement noise, overcoming the shortcoming of ordinary filters that assume noise remains unchanged, and making the predicted value more accurate to the true value.

[0024] This invention preprocesses the values ​​entering the system, filters out outliers that significantly deviate from normal values ​​based on a threshold, and reduces the weight of outliers using a weighted approach, which effectively improves the robustness of the system. Attached Figure Description

[0025] Figure 1 This is an overall flowchart of the present invention;

[0026] Figure 2 This is a trajectory simulation diagram of the present invention;

[0027] Figure 3 This is the error diagram of the east-west and north-bound velocity of this invention;

[0028] Figure 4 This is a comparison chart of position filtering under no abnormal interference according to the present invention;

[0029] Figure 5 This is a comparison chart of position filtering under abnormal interference conditions according to the present invention. Detailed Implementation

[0030] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and should not be used to limit the scope of protection of the present invention. Example

[0031] Embodiments of this invention provide a navigation method based on threshold processing and adaptive unscented Kalman filtering. While meeting the requirements of nonlinear systems, this invention employs unscented Kalman filtering as the filter for the integrated navigation system, effectively avoiding model loss, improving computational accuracy, and reducing computational load. Simultaneously, this invention combines Sage-Husa for adaptive estimation of measurement noise, overcoming the drawback of ordinary filters that assume no change in noise, making the predicted values ​​more accurate to the true values. Furthermore, this invention preprocesses the data entering the system before operation, filtering out outliers that significantly deviate from normal values, and uses a weighted approach to reduce the weight of outliers, effectively improving the system's robustness.

[0032] In its specific design, this invention provides a navigation method based on threshold processing and adaptive unscented Kalman filtering, such as... Figure 1 As shown, it includes the following steps:

[0033] S1: System Initialization:

[0034] Initial calibration of the inertial navigation system is performed by obtaining initial position and initial velocity from the global satellite navigation system, obtaining initial attitude angles from values ​​measured by the inertial measurement unit, and processing quaternions and coordinate transformation matrices. Perform initialization.

[0035] S2: Outlier filtering:

[0036] By setting thresholds to distinguish between normal and abnormal data, a weighted approach is used to reduce the impact of outliers on navigation accuracy.

[0037] In reality, numerous interferences frequently exist, and conventional filters lack error correction steps, potentially incorporating these error values ​​into the calculations. On the other hand, Kalman filters utilize only measurements from the previous moment and estimates from the current moment to improve efficiency. Therefore, the observed values ​​have a significant impact on the filter. This section proposes a threshold filtering mechanism that, by setting a threshold, identifies outliers that clearly exceed normal navigation information, processing these anomalies that can easily cause the filter to diverge.

[0038] First, define the threshold range. ;

[0039] in, It is the average value; It is the standard deviation; This is the target data before weighted averaging. For any given time, the probability of a normal state value falling within this range is 99.7%. Errors exceeding this range are not random errors but rather gross errors and should be discarded. ;

[0040] in, It is the target data after weighted average processing; The target data is before weighted average processing; It is the average value; It is a weighting factor; These are observed values.

[0041] Directly discarding gross errors would make the system discontinuous; therefore, weighting factors should be used. Outliers are processed to reduce the interference of gross errors or disturbed outliers on the filter, thereby improving the filter's robustness.

[0042] S3: Sampling point calculation:

[0043] The sampling points are determined according to the unscented transformation rule. A sampling point sigma is found among the original state distribution points such that the mean and covariance of the sampling point equal the mean and covariance of the original state space vector distribution, thus obtaining the point set of the corresponding nonlinear function values. The accuracy after transformation is at least second-order, and can reach third-order accuracy for distributions conforming to a Gaussian distribution. The weights are then obtained.

[0044] ;

[0045] in It is the value of the first sampling point. , These are the first point weights of the mean and covariance, respectively. It is a scaling factor that describes the range of sampling points around the mean, with a value of .

[0046] ;

[0047] In the formula It is the i-th sampling point. yes The i-th row or column. These are the weights of the i-th point for the mean and covariance, respectively. Substituting the 2n+1 sampling points into the state equation yields the predicted value for the next time step.

[0048] S4: System Status Update

[0049] Calculate the next predicted value for the sigma point set, and calculate the weighted predicted value of the state value at time k. Then, generate a new sigma point set using unscented transformation, substitute it into the observation equation, and obtain the predicted value of the measurement.

[0050] ;

[0051] in These are the mean and covariance matrices of the system state prediction at time k, respectively. The sampling points are then updated based on the predicted values. Substituting the new point into the observation equation, we obtain the predicted value of the observation at time k.

[0052] ;

[0053] In the formula These are the predicted mean and covariance matrix of the observations at time k, respectively. It is the covariance of the measured noise.

[0054] S5: Noise Update:

[0055] Using simplified Sage-Husa dynamics to estimate measurement noise This feature allows for the elimination of the predicted noise Q, reducing the number of adaptive loops and lowering the burden of filter computation loops and gain calculations.

[0056] ;

[0057] In the formula Measure the residuals for the filter. These are the adaptive factor and the fading factor, respectively.

[0058] S6: Gain matrix update:

[0059] ;

[0060] After completing one state prediction and noise prediction, the filter gain needs to be updated, as shown in the equation. It is the filter gain.

[0061] This invention utilizes MATLAB for navigation simulation, and the experiment was simulated using MATLAB, such as... Figure 2 As shown. Starting at 108.91°E and 34.24°N, a 260-second driving trajectory was simulated, including acceleration, deceleration, and turning. The gyroscope bias was set to... Random walk is The accelerometer bias is set to 50. Random walk is . Figure 3 The threshold processing performance for eastward and northward directions under interference conditions is demonstrated. This invention also compares the position filtering performance of four algorithms under conditions of no external interference and with external interference, specifically as follows: Figure 4 and Figure 5 As shown. Example

[0062] Based on the same inventive concept as Embodiment 1, Embodiment 2 of the present invention provides a computer-readable storage medium for storing one or more programs, the one or more programs including instructions that, when executed by a computing device, cause the computing device to perform any of the methods described according to Embodiment 1. Example

[0063] Based on the same inventive concept as Embodiment 1, Embodiment 3 of the present invention provides a computing device including one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include instructions for performing any of the methods described in Embodiment 1.

[0064] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0065] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0066] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0067] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0068] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the technical principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A navigation method based on threshold processing and adaptive unscented Kalman filtering, characterized in that: Includes the following steps: Acquire and preprocess the data of the navigation coordinate system and the target coordinate system, construct the state space system and observation equations of the navigation coordinate system and the target coordinate system, and obtain the state distribution relationship of the navigation coordinate system and the target coordinate system; In a state-space system, an unscented transformation operation is performed on the target point to obtain the target points that meet the selection criteria as sampling points; Weighted calculations are performed on the sampling points to obtain the predicted value of the state space system state at a specified time. For state-space systems based on predicted values, an unscented transformation operation is performed on the sampling points to obtain sampling points that meet the selection criteria as new sampling points. By substituting the new sampling points into the observation equation, the predicted values ​​of the observations at the specified time are obtained. After updating the measurement noise and gain matrix, the predicted value of the observation at a specified time is output for navigation. Threshold judgment is performed on the target data to identify abnormal target data and perform anomaly handling, including: Define the threshold range: ; in, It is the average value; It is the standard deviation; The target data is before weighted average processing; Extract target data that exceeds the threshold range and perform weighted averaging: ; in, It is the target data after weighted average processing; It is a weighting factor; yes The observed value at that moment.

2. The navigation method based on threshold processing and adaptive unscented Kalman filtering according to claim 1, characterized in that: The predicted values ​​of the state of the state-space system include: The mean and covariance of a state-space system.

3. The navigation method based on threshold processing and adaptive unscented Kalman filtering according to claim 2, characterized in that: The selection criteria include: For a target or sampling point to be selected, are the mean and covariance after the unscented transformation operation equal to the mean and covariance of the state space vector distribution in the state space system to which it belongs? 4. A computer-readable storage medium for storing one or more programs, characterized in that, The one or more programs include instructions that, when executed by a computing device, cause the computing device to perform any of the methods according to claims 1 to 3.

5. A computing device, characterized in that, include, One or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs including instructions for performing any of the methods according to claims 1 to 3.

Images