Adaptive robust kalman filter integrated navigation method based on mvc

Abstract

The application discloses a kind of adaptive robust Kalman filtering integrated navigation methods based on MVC, mainly include: obtaining sensor real-time data;Carry out Kalman filtering state step prediction;Adaptive robust Kalman filtering estimation parameter matrix and adaptive factor are utilized;Carry out Kalman filtering measurement update;Output integrated navigation result.The application solves the problem that when system process noise is uncertain and system measurement noise is abnormal, the precision of integrated navigation result decreases or even diverges.

Description

Technical Field

[0001] This invention relates to an adaptive robust integrated navigation method, specifically to an adaptive robust Kalman filter integrated navigation method based on MVC. Background Technology

[0002] Strapdown inertial navigation system (SINS) integrated navigation technology is one of the key technologies for normal navigation and positioning. Using an adaptive robust Kalman filter based on MVC for in-flight integrated navigation offers advantages such as high accuracy and reliability. Currently, integrated navigation obtains optimal estimates of attitude, velocity, and position by fusing SINS and GNSS data using Kalman filtering. However, due to the vehicle's intense maneuvers and abnormal velocity and position measurements in the SINS / GNSS integrated navigation system, system process uncertainties and non-Gaussian measurement noise arise, severely compromising the performance of traditional SINS / GNSS integrated navigation systems based on Kalman filtering.

[0003] To overcome the problem that traditional integrated navigation methods are susceptible to non-Gaussian influences from process noise and measurement noise, it is of great significance to study an adaptive robust Kalman filter integrated navigation method based on MVC. Summary of the Invention

[0004] The purpose of this invention is to provide an adaptive robust Kalman filter-based integrated navigation method based on MVC, which solves the problem that the accuracy of integrated navigation results decreases or even diverges when the system process noise is uncertain or the system measurement noise is abnormal, so as to improve the stability and anti-interference of the integrated navigation process.

[0005] The technical solution to achieve the purpose of this invention is as follows:

[0006] An adaptive robust Kalman filter-based integrated navigation method based on MVC includes the following steps:

[0007] Step 1: Acquire real-time sensor data;

[0008] Step 2: Construct an object motion model, update the carrier motion state, and complete the one-step Kalman filter state prediction;

[0009] Step 3: Based on the established parametric equations, the parameter matrix and adaptive factor are estimated using an adaptive robust Kalman filter, including: constructing a minimum cost function for the robust filter based on the maximum snipe criterion to determine a new measurement noise covariance matrix; constructing an adaptive attenuation factor based on the new measurement noise covariance matrix to determine a new one-step prediction covariance matrix for optimal estimation.

[0010] Step 4: Perform Kalman filter measurement updates to obtain integrated navigation results.

[0011] Compared with the prior art, the advantages of the present invention are as follows:

[0012] (1) The present invention uses the scramble tongue function to construct a new cost function, which makes the optimal estimation of the measurement covariance matrix and weakens the influence of measurement abnormal noise.

[0013] (2) The present invention uses an adaptive attenuation factor to make the optimal estimate of the one-step prediction covariance matrix, thereby weakening the influence of abnormal noise caused by the violent movement of the vehicle.

[0014] (3) The present invention improves the navigation accuracy and stability of the system. Attached Figure Description

[0015] Figure 1 This is a flowchart of the integrated navigation process based on MVC, using SINS / GNSS adaptive robust Kalman filtering.

[0016] Figure 2 It is a simulation curve of the carrier's motion.

[0017] Figure 3 This is an attitude error curve.

[0018] Figure 4 This is a speed error curve.

[0019] Figure 5 This is a position error curve. Detailed Implementation

[0020] The present invention will now be described in further detail with reference to the accompanying drawings and examples:

[0021] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0022] This invention is a combined navigation method based on MVC and SINS / GNSS adaptive robust Kalman filtering. It employs adaptive robust filtering to address the divergence problem in combined navigation results when process noise is unknown and measurement noise is uncertain. The process is as follows: Figure 1 As shown, it includes the following steps:

[0023] Step 1: Acquire real-time sensor data, including gyroscope data and accelerometer data;

[0024] Step 2: Construct the object motion model, update the carrier motion state, complete the one-step Kalman filter state prediction, and define the reference coordinate system required for the solution as follows:

[0025] b—Carrier coordinate system, representing the three-axis orthogonal coordinate system of the strapdown inertial navigation system, with its x-axis, y-axis and z-axis pointing to the right-front-up of the carrier, respectively;

[0026] n—Navigation coordinate system, representing the geographic coordinate system of the vehicle's location, with its three axes pointing to the local east, north, and sky directions, respectively;

[0027] e—Earth coordinate system, indicating that the origin is at the Earth's center, the x-axis is the intersection of the Prime Meridian and the equator, the z-axis is the intersection of the Earth's center and the North Pole, and the y-axis, together with the x-axis and z-axis, forms a right-handed coordinate system;

[0028] i — Inertial coordinate system, representing a non-rotating coordinate system in inertial space;

[0029] The SINS attitude, velocity, and position differential equations are expressed as follows:

[0030]

[0031] In the formula: The derivative of the direction cosine matrix; The direction cosine matrix represents the change of the carrier system relative to the navigation system; The figure represents the projection of the rotational angular velocity of the carrier system relative to the navigation system onto the carrier system; × indicates the vector cross product operation;

[0032]

[0033] In the formula: f represents the velocity derivative in the navigation system. n This represents the projection of force onto the navigation system; This represents the projection of the Earth's rotational angular velocity relative to the inertial frame onto the navigation frame; represents the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame; × represents the vector cross product operation; v n Indicates speed in the navigation system; g n This represents the projection of gravitational acceleration onto the navigation frame.

[0034]

[0035] In the formula: Represents the differential of latitude; R represents the northbound velocity in the navigation system. Mh This represents the sum of the radius of curvature and the height of the meridian. Represents the differential of longitude position; Indicates eastward velocity in the navigation system; L represents latitude; R Nh This represents the sum of the radius of curvature and height of the zonal loop; Represents the differential of height position; Indicates the azimuth velocity under the navigation system;

[0036] There is an error between the actual output value of the inertial sensor and the true value, which leads to error propagation during navigation calculations. Under the assumption of small perturbation, the SINS error propagation satisfies the following equation:

[0037]

[0038] In the formula: This represents the differential of the attitude error in the navigation system. This represents the projection of the rotational angular velocity of the navigation frame relative to the inertial frame onto the navigation frame. Indicates the attitude error in the navigation system; This represents the error in the projection of the rotational angular velocity of the navigation frame relative to the inertial frame onto the navigation frame; The direction cosine matrix represents the change of the carrier system relative to the navigation system; It represents the error in the projection of the rotational angular velocity of the carrying system relative to the inertial frame onto the carrying system;

[0039]

[0040] In the formula: f represents the differential of velocity error in the navigation system. n This represents the projection of force onto the navigation system; Indicates the attitude error in the navigation system; This represents the projection of the Earth's rotational angular velocity relative to the inertial frame onto the navigation frame. δv represents the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. n Indicates the speed error in the navigation system; v n Indicates speed in the navigation system; This represents the error in the projection of the Earth's angular velocity relative to the inertial frame onto the navigation frame. This represents the error in the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. The direction cosine matrix δf represents the change of the carrier system relative to the navigation system. b Indicates the specific force error;

[0041]

[0042] In the formula: R represents the differential of latitude position error; Mh This represents the sum of the radius of curvature and the height of the meridian. This indicates the northbound velocity error in the navigation system. δh represents the northward velocity in the navigation system; δh represents the altitude position error. Represents the differential of longitude position error; L represents latitude position; R Nh This represents the sum of the radius of curvature and height of the zonal loop; This indicates the eastward velocity error of the navigation system; Indicates the eastward velocity in the navigation system; This represents the differential of the height position error; This indicates the azimuth velocity error under the navigation system;

[0043] After some manipulation and simplification, the above equation is obtained as follows:

[0044]

[0045] In the formula: This represents the differential of the attitude error in the navigation system. Indicates the attitude error in the navigation system; Represents the velocity error in the navigation system; δp=[δL δλ δh] T Indicates the position error under the navigation system; This indicates that the gyroscope has zero bias under the load system. f represents the differential of velocity error in the navigation system. n This represents the projection of force onto the navigation system; This indicates zero bias of the accelerometer under the load system; This represents the differential of the position error in the navigation system.

[0046] in:

[0047]

[0048] In the formula: f represents the projection of the rotational angular velocity of the navigation frame relative to the inertial frame onto the navigation frame; n This indicates the projection of force onto the navigation system; v n Indicates speed in the navigation system; This represents the projection of the Earth's rotational angular velocity relative to the inertial frame onto the navigation frame. R represents the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. M R represents the radius of curvature of the meridional circle; N ω represents the radius of curvature of the zonal loop; ie L represents the Earth's rotational angular velocity; L represents latitude. Indicates the northbound velocity in the navigation system; β1 represents the eastward velocity in the navigation system; g0 represents the magnitude of gravity at sea level at the equator; β1 = 5.27094 × 10⁻⁶. -3 β3 = 3.086 × 10 -6 ;

[0049] The SINS error propagation model can be expressed as the following state equation:

[0050]

[0051] In the formula: F represents the state vector differential; F represents the one-step state transition matrix; x represents the state vector.

[0052] in:

[0053]

[0054] Kalman filter state prediction in one step:

[0055]

[0056] In the formula: F represents the state prediction at time k; k|k1 This represents the state transition matrix at time k; P represents the state estimate at time k-1; k|k-1 Q represents the state prediction covariance matrix at time k; k-1 This represents the process noise at time k-1;

[0057] Step 3: Based on the established parametric equations, estimate the parameter matrix using an adaptive robust Kalman filter;

[0058] (1) Parameter estimation of measurement noise covariance matrix

[0059] According to the maximum snipe criterion, the cost function minimized by robust filtering is:

[0060]

[0061] In the formula: J L (x k ) represents the cost function; x k This represents the true value of the state at time k; λ represents the state prediction at time k; k Represents the adaptive factor; ρ a (e k，i ) represents the kernel function; e k，i Represents variables; denoted as the state estimate at time k; argmin represents the minimization operation, i represents the i-th residual, and m represents the residual dimension;

[0062] variable e k It can be calculated as follows:

[0063]

[0064] In the formula: R k Indicates measurement noise; z k H represents the observation vector at time k; k Represents the measurement matrix; This represents the state prediction at time k;

[0065] The kernel function can be represented as:

[0066]

[0067] In the formula: ρ a (e k，i ) represents the kernel function; e k，i 'a' represents the variable; 'a' represents the adjustment parameter; 'p' represents the shape parameter.

[0068] Therefore, the weighting function can be obtained as follows:

[0069]

[0070] In the formula: C k，i Represents the weighting function; e k，i 'a' represents the variable; 'a' represents the adjustment parameter; 'p' represents the shape parameter.

[0071] Therefore, the new measurement noise covariance matrix is:

[0072]

[0073] In the formula: R represents the measurement noise covariance matrix at time k for robust estimation; k Let k represent the unrobust measurement noise covariance matrix at time k; The matrix representing the inverse of the weight function;

[0074] (2) One-step prediction of covariance matrix parameter estimation

[0075] Measurement noise can be expressed as:

[0076] v k =x k -H k x k ;

[0077] In the formula: v k Indicates measurement noise; z k H represents the GNSS measurement value at time k; k Represents the measurement matrix; x k Represents the true state at time k;

[0078] According to the principle of orthogonality of information sequences:

[0079]

[0080] In the formula: This represents the one-step prediction covariance matrix at time k in the adaptive estimation. This indicates the transpose of the measurement matrix; V represents the gain matrix at time k; k Represents the information covariance matrix;

[0081] in:

[0082] V k =(z k -H k x k|k1 (z) k -H k x k|k1 ) T

[0083]

[0084] In the formula: z k x represents the GNSS measurement value at time k; k|k-1 This indicates a one-step prediction of the state at time k; λ represents the measurement noise covariance matrix at time k for robust estimation. k P represents the adaptive factor; k|k-1 This represents the predicted covariance matrix at time k;

[0085] Simplifying the above formula, we can obtain the formula for calculating the adaptive factor as follows:

[0086]

[0087] In the formula: λ i，k P represents the adaptive factor for the predicted covariance matrix; k|k-1 This represents the predicted covariance matrix at time k; V represents the transpose of the measurement matrix; [i, j] represents the i-th row and j-th column of the matrix; k Represents the information covariance matrix; This represents the measurement noise covariance matrix of the robust estimate at time k;

[0088] Step 4: Kalman filter measurement update

[0089] Using the reconstructed measurement covariance matrix and adaptive factor obtained from the above adaptive robust Kalman filter equation, the measurement update of the Kalman filter can be obtained as follows:

[0090]

[0091] In the formula: K k Represents the gain matrix; This represents the prediction covariance matrix of the adaptive estimation at time k; This indicates the transpose of the measurement matrix; This represents the measurement noise covariance matrix of the robust estimate at time k; This represents the state estimate at time k; z represents the state at time k that can be predicted in one step; k P represents the GNSS measurement value at time k; k|k Let I represent the state covariance matrix at time k; I represents the identity matrix.

[0092] This embodiment demonstrates the robustness of the integrated navigation process by using Matlab simulation software to verify the proposed MVC-based SINS / GNSS adaptive robust Kalman filter integrated navigation method.

[0093] The MATLAB simulation experiment used an Intel(R) Core(TM) i5-6300HQ CPU 2.30GHz, 8GB RAM, and Windows 10 operating system. The simulation experiment was conducted under the following parameters:

[0094] The constant drift error of the gyroscope measurement is The random walk error of the gyroscope measurement is The output frequency is 100Hz; the constant drift error of the accelerometer is... The random walk error of the gyroscope measurement is The output frequency is 100Hz. The adjustment parameter is set to a=1, the shape parameter is set to p=2, and the initial value of the Kalman filter parameter is... The covariance of measurement noise and process noise is set to R. j，|k = [0.1m / s, 2.5m] 2 , like Figure 2 The figure shown is a motion curve of the vehicle during the in-flight integrated navigation process; Figure 3 , Figure 4 and Figure 5 The figure shows the combined navigation error of SINS / GNSS adaptive robust Kalman filtering based on MVC. As can be seen from the figure, after adopting the adaptive robust technology, the combined navigation results effectively suppress the interference of measurement abnormal noise, while the traditional method is affected by external abnormal noise, resulting in navigation instability.

Claims

1. An adaptive robust Kalman filter-based integrated navigation method based on MVC, characterized in that, Includes the following steps: Step 1: Acquire real-time sensor data; Step 2: Construct an object motion model, update the carrier motion state, and complete the one-step Kalman filter state prediction; Step 3: Based on the established parametric equations, the parameter matrix and adaptive factors are estimated using an adaptive robust Kalman filter, including: constructing a minimum cost function for the robust filter based on the maximum snipe criterion, and determining a new measurement noise covariance matrix; An adaptive attenuation factor is constructed based on the new measurement noise covariance matrix, and a new step prediction covariance matrix is ​​determined for optimal estimation. Step 4: Perform Kalman filtering measurement updates to obtain integrated navigation results; The cost function is: In the formula, Represents the cost function; This represents the true value of the state at time k; This represents the state prediction at time k; Indicates the adaptive decay factor; Represents the kernel function; Represents variables; denoted as the state estimate at time k; argmin represents the minimization operation, i represents the i-th residual, and m represents the residual dimension; The adaptive attenuation factor is: In the formula, This represents the adaptive factor for predicting the covariance matrix; This represents the predicted covariance matrix at time k; This indicates the transpose of the measurement matrix; This represents the i-th row and j-th column of the matrix; Represents the information covariance matrix; Let represent the measurement noise covariance matrix of the robust estimation at time k.

2. The adaptive robust Kalman filter integrated navigation method based on MVC according to claim 1, characterized in that, Constructing an object motion model, updating the carrier's motion state, and completing one-step Kalman filter state prediction include: Error models for attitude, velocity, and position in a navigation system: In the formula: This represents the differential of the attitude error in the navigation system. Indicates the attitude error in the navigation system; Indicates the speed error under the navigation system; Indicates the position error under the navigation system; This indicates that the gyroscope has zero bias under the load system. This represents the differential of the velocity error in the navigation system. This represents the projection of force onto the navigation system; This indicates zero bias of the accelerometer under the load system; This represents the differential of the position error in the navigation system. in: In the formula: This represents the projection of the rotational angular velocity of the navigation frame relative to the inertial frame onto the navigation frame. This represents the projection of force onto the navigation system; Indicates speed in the navigation system; This represents the projection of the Earth's rotational angular velocity relative to the inertial frame onto the navigation frame. This represents the projection of the angular velocity of the navigation frame relative to the Earth frame onto the navigation frame. Indicates the radius of curvature of the meridian; Indicates the radius of curvature of the zonal loop; This represents the Earth's angular velocity of rotation; Indicates latitude; Indicates the northbound velocity in the navigation system; Indicates the eastward velocity in the navigation system; This indicates the magnitude of gravity at sea level near the equator; ; ; The error propagation model for the carrier's motion state is as follows: In the formula: Represents the differential of the state vector; Represents the state transition matrix in one step; Represents the state vector; in: The Kalman filter state prediction in one step is: In the formula: This represents the state prediction at time k; This represents the state transition matrix at time k; This represents the state estimate at time k-1; This represents the state prediction covariance matrix at time k; This represents the process noise at time k-1.

3. The adaptive robust Kalman filter integrated navigation method based on MVC according to claim 1, characterized in that, The variable for: In the formula: Indicates measurement noise; Represents the observation vector at time k; Represents the measurement matrix; This represents the state prediction at time k.

4. The adaptive robust Kalman filter integrated navigation method based on MVC according to claim 3, characterized in that, The kernel function is: In the formula, Represents the kernel function; 'a' represents the variable; 'a' represents the adjustment parameter; 'p' represents the shape parameter.

5. The adaptive robust Kalman filter integrated navigation method based on MVC according to claim 3, characterized in that, The new measurement noise covariance matrix is: In the formula, Let represent the measurement noise covariance matrix at time k for robust estimation; Let k represent the unrobust measurement noise covariance matrix at time k; This represents the inverse matrix of the weight function; the weight function is: In the formula, Represents the weighting function; 'a' represents the variable; 'a' represents the adjustment parameter; 'p' represents the shape parameter.

6. The adaptive robust Kalman filter integrated navigation method based on MVC according to claim 1, characterized in that, The new step-prediction covariance matrix is: In the formula, This represents the one-step prediction covariance matrix at time k in the adaptive estimation. Indicates the adaptive factor. Let k represent the one-step prediction covariance matrix.

7. The adaptive robust Kalman filter integrated navigation method based on MVC according to claim 6, characterized in that, In step 4, the Kalman filter measurement is updated as follows: In the formula, Represents the gain matrix; Let k represent the prediction covariance matrix of the adaptive estimation at time k; This indicates the transpose of the measurement matrix; This represents the measurement noise covariance matrix of the robust estimate at time k; This represents the state estimate at time k; This indicates a one-step prediction of the state at time k; express GNSS measurement at any given time; Represents the state covariance matrix at time k; Represents a unit array.

8. The adaptive robust Kalman filter integrated navigation method based on MVC according to claim 1, characterized in that, The real-time data from the sensors includes gyroscope data and accelerometer data.

Images