A combined navigation post-processing method and system based on Kalman filtering and graph optimization

Abstract

A combined navigation post-processing method and system combining Kalman filtering with graph optimization. The present invention provides a combined navigation post-processing method and system, the method comprising the following steps: obtaining the specific force and angular velocity output by the inertial navigation system, integrating the specific force and angular velocity using an inertial navigation integral algorithm to obtain the position, velocity and attitude heading angle; performing forward Kalman filtering on the position, velocity and attitude heading angle, and constructing a set of equations according to the numerical values ​​or matrices in the Kalman filtering process; solving the set of equations using a graph optimization method to obtain the state error of the inertial navigation system at all times; performing navigation solution on the inertial navigation system using the state error calculated using the graph optimization method, and outputting navigation data. This method can ensure both navigation accuracy and processing speed.

Description

Technical Field

[0001] The invention belongs to the field of navigation technology, and in particular relates to a combined navigation post-processing method and system combining Kalman filtering with graph optimization. Background Art

[0002] Most of the existing integrated navigation post-processing methods use forward and reverse Kalman filtering. This method is easy to implement, but has its limitations in engineering applications. Figure 1 It is the forward Kalman filter method.

[0003] Since Kalman filtering uses a hidden Markov model, each iteration must solve the edge probability density. This will not only lose a lot of historical measurement information, but also fail to use subsequent information to reversely propagate historical measurements. Therefore, traditional methods often use reverse Kalman filtering plus reverse navigation solution to improve accuracy. See Figure 2 If this method is not used, it is difficult to transmit the subsequent measurement information to the measurement point in front. However, since the Kalman filter uses the method of constantly calculating the edge probability density, due to the Markov property of the model, that is, assuming that the observations at each moment are independent, such as Figure 1 As shown in the figure, the observation of Y1 is independent of Y3, that is, when the observation of Y2 arrives, the marginal probability density of X2 at the second moment will be calculated. In this way, Y3 at the third moment cannot affect X0, X1, and X2. Even if reverse filtering is used, it is difficult for observations that are far away to affect each other. Kalman filtering is only a one-step linear unbiased optimal estimate, and there is no reverse message propagation.

[0004] Graph optimization technology is an emerging optimal estimation technology. It uses a graph model to model and solves large-scale sparse state update equations for calculation. It can process large-scale sensor data. When graph optimization is used to solve problems, it is necessary to remodel and convert the state space model into a graph model. In contrast to Kalman filtering, when graph optimization is used to solve problems, the most desired thing is global optimization, and it is not desired to remove previous observations by solving edge probability density. Therefore, the calculation time will be very long when graph optimization is used for post-processing because the scale of the graph is growing. For combined navigation post-processing, if the probability distribution of all moments is calculated by graph optimization at each moment, and the feedback is obtained by the probability distribution of the latest moment, and the system is fed back, the calculation time will be very long and the software experience will be very bad. In addition, there is a lack of specific solutions in the prior art to use graph optimization to obtain higher accuracy for the combination of inertial navigation systems and other sensors. Most of the prior art adopts the solution of pre-integration of inertial navigation systems, and uses the short-term accuracy of inertial navigation to smooth the information of adjacent frames of vision or laser. There is no specific solution in the prior art to use graph optimization to improve the accuracy of high-precision combined navigation post-processing. Existing solutions for graph-optimized integrated navigation all adopt a state estimation mode without considering a state feedback mode, that is, the estimation uses factor graph optimization to directly estimate the speed, position, and attitude of the navigation system, rather than estimating the errors of speed, position, and attitude.

[0005] Therefore, the Kalman filter and graph optimization combined navigation post-processing solutions in the prior art have their limitations: one is insufficient accuracy, the other is too large in calculation, and no error estimation solution is considered. Summary of the invention

[0006] In view of the defects in the prior art, the present invention provides a combined navigation post-processing method and system of Kalman filtering combined with graph optimization, which can ensure both navigation accuracy and processing speed.

[0007] In a first aspect, a combined navigation post-processing method of Kalman filtering combined with graph optimization comprises the following steps:

[0008] Obtain the specific force and angular velocity output by the inertial navigation system, integrate the specific force and angular velocity using the inertial navigation integration algorithm, and obtain the position, velocity, and attitude heading angle;

[0009] Perform forward Kalman filtering on the position, velocity and attitude heading angle, and construct a set of equations based on the numerical values ​​or matrices in the Kalman filtering process;

[0010] The graph optimization method is used to solve the equations and obtain the state error of the inertial navigation system at all times;

[0011] The state error calculated by the graph optimization method is used to perform navigation solution on the inertial navigation system and output navigation data.

[0012] Preferably, the system of equations is:

[0013]

[0014] Among them, P0 is the prior variance, Q k is the process noise covariance at time k, R k is the measurement covariance at time k, A k is the system matrix used for Kalman filtering at time k, H k is the observation matrix used for Kalman filtering at time k, x k is the error state at time k, is the error state obtained by calculating the Kalman filter at time k-1, z k is the observed value at time k.

[0015] Preferably, after constructing the system of equations, the method further comprises:

[0016] Record the navigation status each time the Kalman filter is updated. The navigation status includes position, velocity, and attitude heading angle:

[0017]

[0018] in, is the position at the kth update, is the speed at the kth update, is the attitude heading angle at the kth update, NAV k The navigation state.

[0019] Preferably, the using the graph optimization method and the state error to perform navigation solution on the inertial navigation system and outputting navigation data specifically includes:

[0020] Using the graph optimization method and the state error to perform navigation solution on the inertial navigation system, and establishing a feedback amount for the next navigation solution according to the navigation solution result;

[0021] The inertial navigation system is used to perform navigation calculation using the feedback obtained after the last navigation calculation, and the navigation data is output.

[0022] Preferably, the feedback amount is in, X gk The state NAV obtained by integrating the Kalman filter at time k using the graph optimization method is k error, is the navigation solution value of the first integration of the Kalman filter to time k, N k is the number of samples from time k-1 to time k.

[0023] In a second aspect, a combined navigation post-processing system of Kalman filtering combined with graph optimization includes:

[0024] Kalman filter unit: used to obtain the specific force and angular velocity output by the inertial navigation system, integrate the specific force and angular velocity using the inertial navigation integration algorithm to obtain the position, velocity and attitude heading angle; perform forward Kalman filtering on the position, velocity and attitude heading angle, and construct a set of equations based on the numerical values ​​or matrices in the Kalman filtering process;

[0025] Graph optimization unit: solves the equation group using the graph optimization method to obtain the state error of the inertial navigation system at all times; performs navigation solution on the inertial navigation system using the state error calculated by the graph optimization method, and outputs navigation data.

[0026] Preferably, the system of equations is:

[0027]

[0028] Among them, P0 is the prior variance, Q k is the process noise covariance at time k, R k k is the measurement covariance, A k is the system matrix used for Kalman filtering at time k, H k is the observation matrix used for Kalman filtering at time k, x k is the error state at time k, is the error state calculated by Kalman filter at time k-1, z k is the observed value at time k.

[0029] Preferably, the Kalman filter unit is further used for:

[0030] Record the navigation status each time the Kalman filter is updated. The navigation status includes position, velocity, and attitude heading angle:

[0031]

[0032] in, is the position at the kth update, is the speed at the kth update, is the attitude heading angle at the kth update, NAV k The navigation state.

[0033] Preferably, the graph optimization unit is specifically used for:

[0034] Using the state error calculated by the graph optimization method to perform navigation solution on the inertial navigation system, and establishing a feedback amount for the next navigation solution according to the navigation solution result;

[0035] The inertial navigation system is used to perform navigation calculation using the feedback obtained after the last navigation calculation, and the navigation data is output.

[0036] Preferably, the feedback amount is in, X gk The state NAV obtained by integrating the Kalman filter at time k using the graph optimization method is k error, is the navigation solution value of the first integration of the Kalman filter to time k, N k is the number of samples from time k-1 to time k.

[0037] It can be seen from the above technical solution that the combined navigation post-processing method and system of Kalman filtering combined with graph optimization provided by the present invention uses Kalman filtering for initialization to obtain feedback as an observation of the system prediction factor, and inputs it into the graph optimization together with other observations for global optimization to improve the accuracy of error estimation. The state estimation of the graph optimization is initialized by Kalman filtering, so that the entire operation process only needs to perform graph optimization once, which greatly reduces the amount of calculation and ensures the processing speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to the drawings required for the specific embodiments or the description of the prior art. In all the drawings, similar elements or parts are generally identified by similar reference numerals. In the drawings, the elements or parts are not necessarily drawn according to the actual scale.

[0039] Figure 1 A schematic diagram of forward filtering provided as background technology.

[0040] Figure 2 A schematic diagram of reverse filtering provided as background technology.

[0041] Figure 3 This is a flow chart of the method provided in Embodiment 1 of the present invention. DETAILED DESCRIPTION

[0042] The embodiments of the technical solution of the present invention will be described in detail below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and are therefore only used as examples, and the protection scope of the present invention cannot be limited thereto. It should be noted that, unless otherwise specified, the technical terms or scientific terms used in this application should be the common meanings understood by those skilled in the art to which the present invention belongs.

[0043] It should be understood that when used in this specification and the appended claims, the terms "include" and "comprises" indicate the presence of described features, integers, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or combinations thereof.

[0044] It should also be understood that the terms used in this specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the specification of the present invention and the appended claims, unless the context clearly indicates otherwise, the singular forms "a", "an" and "the" are intended to include plural forms.

[0045] As used in this specification and the appended claims, the term "if" may be interpreted as "when" or "upon" or "in response to determining" or "in response to detecting," depending on the context. Similarly, the phrases "if it is determined" or "if [described condition or event] is detected" may be interpreted as meaning "upon determination" or "in response to determining" or "upon detection of [described condition or event]" or "in response to detecting [described condition or event]," depending on the context.

[0046] Embodiment 1:

[0047] A combined navigation post-processing method of Kalman filtering and graph optimization, see Figure 3 , including the following steps:

[0048] S1: Obtain the specific force and angular velocity output by the inertial navigation system, integrate the specific force and angular velocity using the inertial navigation integration algorithm, and obtain the position, velocity, and attitude heading angle;

[0049] S2: Perform forward Kalman filtering on the position, velocity and attitude heading angle, and construct a set of equations based on the values ​​or matrices (including prior values, posterior probability distribution, observation values ​​and feedback values) in the Kalman filtering process;

[0050] S3: Solve the equations using the graph optimization method to obtain the state error of the inertial navigation system at all times;

[0051] S4: Using the state error calculated by the graph optimization method to perform navigation solution on the inertial navigation system, and output navigation data.

[0052] The combined navigation post-processing method and system of Kalman filter combined with graph optimization uses Kalman filter for initialization to obtain feedback as the observation of system prediction factor, and inputs it into graph optimization together with other observations for global optimization to improve the accuracy of error estimation. Kalman filter is used to initialize the state estimation of graph optimization, so that the whole operation process only needs to perform graph optimization once, which greatly reduces the amount of calculation and ensures the processing speed. The method specifically includes the following three steps:

[0053] 1. Forward Kalman filter.

[0054] The system of equations is:

[0055]

[0056] Among them, P0 is the prior variance, Q k is the process noise covariance at time k, R k is the measurement covariance at time k, A k is the system matrix used for Kalman filtering at time k, H k is the observation matrix used for Kalman filtering at time k, x k is the error state at time k, is the error state calculated by Kalman filter at time k-1, z k is the observed value at time k.

[0057] Specifically, the method records the prior value, the posterior probability distribution, the observation value and the feedback value in the Kalman filtering process, constructs the above equation group, and solves the above equation group through the following equation:

[0058] System Model: x k =A k x k-1 +B k u k +q k (2);

[0059] where u k represents the known control input, B k represents the control input matrix; process noise q k Obey Gaussian distribution: q k ~N(0,Q k ), noise can also be modeled in other forms;

[0060] Observation equation: z k =H k x k +r k , (3);

[0061] Among them, H krepresents the observation matrix; the measurement noise r k Obey Gaussian distribution: r k ~N(0,R k );

[0062] Initial condition: expected E of x0 <x0>=x0(+), the variance of x0 E<(x0-x0(+))(x0-x0(+)) T >=P0(+), assuming process noise and measurement noise are uncorrelated

[0063] Status prediction: x k (-)=A k x k-1 (+)+B k u k (4);

[0064] Prior error covariance matrix update:

[0065] P k (-) represents the prior covariance at this moment, P k-1 (+) indicates the posterior covariance of the previous moment;

[0066] Kalman gain calculation:

[0067] Posterior error covariance update: P k (+)=(IK k H k ) k (-) (7);

[0068] State estimation observation update: x k (+) = x k (-)+K k (z k -H k x k (-)) (8);

[0069] Assume that the initial state follows the distribution: x0~N(x0(+),P0); According to formula (2), we get The noise of this equation obeys N(0,Q1); According to formula (3), we get H1x1=z1, and the noise of this equation obeys N(0,R1); According to the time series, we can calculate Arranged in time series, all values ​​of equation (1) can be solved. A time series is a set of random variables ordered by time. It is usually the result of observing a potential process at a given sampling rate in equally spaced time periods. Time series data essentially reflects the trend of one or more random variables changing over time.

[0070] Preferably, after constructing the system of equations, the method further comprises:

[0071] Record the navigation status each time the Kalman filter is updated. The navigation status includes position, velocity, and attitude heading angle:

[0072]

[0073] in, is the position at the kth update, is the speed at the kth update, is the attitude heading angle at the kth update, NAV k The navigation state.

[0074] Specifically, the method also records the navigation state at each step of Kalman filter state update, namely, the position, velocity, and attitude heading angle in three directions.

[0075] 2. Graph optimization solves the error state at all times.

[0076] Write formula (1) as:

[0077] A g X g =b g (10);

[0078] Assume the prior variance P0, process noise covariance Q i , measure covariance R i All are diagonal matrices. The weight matrix needs to be considered when solving equation (10). A diagonal matrix is ​​a matrix in which all elements outside the main diagonal are zero.

[0079] Let P g =diag[P0 Q0 R1 … Q N-1 R N ] T (11);

[0080] Wherein, diag means generating a diagonal matrix by using the right-hand side vector as the diagonal elements.

[0081] Let X g =[x0 x1 … x k-1 x k ] T (12);

[0082] Then, equation (9) can be obtained by the least square method according to the graph optimization technology, and the error state is:

[0083]

[0084] The solution of equation (12) can be obtained by using graph optimization algorithm.

[0085] 3. Use graph optimization solution to recalculate navigation and obtain high-frequency navigation and high-precision output.

[0086] The state error calculated by the graph optimization method is used to perform navigation solution on the inertial navigation system, and the output of navigation data specifically includes:

[0087] Using the state error calculated by the graph optimization method to perform navigation solution on the inertial navigation system, and establishing a feedback amount for the next navigation solution according to the navigation solution result;

[0088] The inertial navigation system is used to perform navigation calculation using the feedback obtained after the last navigation calculation, and the navigation data is output.

[0089] Preferably, the feedback amount is in, X gk The state NAV obtained by integrating the Kalman filter at time k using the graph optimization method is k The error, is the navigation solution value of the first integration of the Kalman filter to time k, N k is the number of samples from time k-1 to time k.

[0090] Specifically, graph optimization is usually for inertial navigation, using an integral factor, that is, the integral equation is:

[0091]

[0092] For external sensors, the measurement factor is used, that is, the measurement equation is:

[0093] z k =H k x k +r k (15)

[0094] This step requires integrating the inertial navigation system input from beginning to end, and ensuring that the gyro and accelerometer measurements obtained when measuring the inertial sensors installed in each inertial navigation system satisfy the following results:

[0095]

[0096] Assume first Start the inertial navigation integration and integrate it according to equation (14) to the first measurement update of the Kalman filter to obtain by is the feedback quantity, where N1 is the inertial navigation input value from the Kalman filter from 0 to 1, and is integrated again from 0 to 1 to ensure

[0097] In this way, two navigation solutions are implemented. The first feedback value is used as the basis for the second feedback. The second integral is recalculated from the starting point according to the feedback to ensure that the integral result obtained when each sensor is measured is the same as the graph optimization result, and the entire recursive solution is completed in this way. In this way, the integral error can be reduced through the feedback amount, ensuring that the high-frequency output results meet the optimal solution of graph optimization.

[0098] This method breaks through the limitation of traditional Kalman filtering that cannot carry out large-scale two-way information transmission. The feedback of Kalman filtering is used as the observation basis and constraint, and is input into the global optimization system, making the post-processing calculation more accurate.

[0099] Embodiment 2:

[0100] A combined navigation post-processing system combining Kalman filtering and graph optimization, comprising:

[0101] Kalman filter unit: used to obtain the specific force and angular velocity output by the inertial navigation system, integrate the specific force and angular velocity using the inertial navigation integration algorithm to obtain the position, velocity and attitude heading angle; perform forward Kalman filtering on the position, velocity and attitude heading angle, and construct a set of equations based on the numerical values ​​or matrices in the Kalman filtering process;

[0102] Graph optimization unit: solves the equation group using the graph optimization method to obtain the state error of the inertial navigation system at all times; performs navigation solution on the inertial navigation system using the state error calculated by the graph optimization method, and outputs navigation data.

[0103] Preferably, the system of equations is:

[0104]

[0105] Among them, P0 is the prior variance, Q k is the process noise covariance at time k, R k k is the measurement covariance, A k is the system matrix used for Kalman filtering at time k, H k is the observation matrix used for Kalman filtering at time k, x k is the error state at time k, is the error state calculated by Kalman filter at time k-1, z k is the observed value at time k.

[0106] Preferably, the Kalman filter unit is further used for:

[0107] Record the navigation status each time the Kalman filter is updated. The navigation status includes position, velocity, and attitude heading angle:

[0108]

[0109] in, is the position at the kth update, is the speed at the kth update, is the attitude heading angle at the kth update, NAV k The navigation state.

[0110] Preferably, the graph optimization unit is specifically used for:

[0111] Using the state error calculated by the graph optimization method to perform navigation solution on the inertial navigation system, and establishing a feedback amount for the next navigation solution according to the navigation solution result;

[0112] The inertial navigation system is used to perform navigation calculation using the feedback obtained after the last navigation calculation, and the navigation data is output.

[0113] Preferably, the feedback amount is in, X gk The state NAV obtained by integrating the Kalman filter at time k using the graph optimization method is k error, is the navigation solution value of the first integration of the Kalman filter to time k, N k is the number of samples from time k-1 to time k.

[0114] The system provided in the embodiment of the present invention is briefly described. For matters not mentioned in the embodiment part, reference may be made to the corresponding contents in the aforementioned embodiment.

[0115] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. These modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention, and they should all be included in the scope of the claims and specification of the present invention.

Claims

1. A combined navigation post-processing method of Kalman filtering combined with graph optimization, characterized in that: The following steps are involved: Obtain the specific force and angular velocity output by the inertial navigation system, integrate the specific force and angular velocity using the inertial navigation integration algorithm, and obtain the position, velocity, and attitude heading angle; Perform forward Kalman filtering on the position, velocity and attitude heading angle, and construct a set of equations based on the numerical values ​​or matrices in the Kalman filtering process; The graph optimization method is used to solve the equations and obtain the state error of the inertial navigation system at all times; The state error calculated by the graph optimization method is used to perform navigation solution on the inertial navigation system and output navigation data; The system of equations is: Among them, P0 is the prior variance, Q k is the process noise covariance at time k, R k is the measurement covariance, A k is the system matrix used for Kalman filtering at time k, H k is the observation matrix used for Kalman filtering at time k, x k is the error state at time k, is the error state obtained by calculating the Kalman filter at time k-1, z k is the observed value at time k.

2. The combined navigation post-processing method of Kalman filtering combined with graph optimization according to claim 1 is characterized in that: After constructing the equation group, it also includes: Record the navigation status each time the Kalman filter is updated. The navigation status includes position, velocity, and attitude heading angle: in, is the position at the kth update, is the speed at the kth update, is the attitude heading angle at the kth update, NAV k The navigation state.

3. The combined navigation post-processing method of Kalman filtering combined with graph optimization according to claim 2 is characterized in that: The using of the graph optimization method and the state error to perform navigation solution on the inertial navigation system and output navigation data specifically includes: Using the graph optimization method and the state error to perform navigation solution on the inertial navigation system, and establishing a feedback amount for the next navigation solution according to the navigation solution result; The inertial navigation system is used to perform navigation calculation using the feedback obtained after the last navigation calculation, and the navigation data is output.

4. The combined navigation post-processing method of Kalman filtering combined with graph optimization according to claim 3 is characterized in that: The feedback amount is in, X gk The state NAV obtained by integrating the Kalman filter at time k using the graph optimization method is k error, is the navigation solution value of the first integration of the Kalman filter to time k, N k is the number of samples from time k-1 to time k.

5. A Kalman filter combined with graph optimization combined navigation post-processing system, characterized in that: include: Kalman filter unit: used to obtain the specific force and angular velocity output by the inertial navigation system, integrate the specific force and angular velocity using the inertial navigation integration algorithm to obtain the position, velocity and attitude heading angle; perform forward Kalman filtering on the position, velocity and attitude heading angle, and construct a set of equations based on the numerical values ​​or matrices in the Kalman filtering process; A graph optimization unit: using a graph optimization method to solve the equation group, and obtain the state error of the inertial navigation system at all times; using the graph optimization method and the state error to perform navigation solution on the inertial navigation system, and outputting navigation data; The system of equations is: Among them, P0 is the prior variance, Q k is the process noise covariance at time k, R k is the measurement covariance, A k is the system matrix used for Kalman filtering at time k, H k is the observation matrix used for Kalman filtering at time k, x k is the error state at time k, is the error state obtained by calculating the Kalman filter at time k-1, z k is the observed value at time k.

6. The combined navigation post-processing system of Kalman filtering combined with graph optimization according to claim 5, characterized in that: The Kalman filter unit is also used for: in, is the position at the kth update, is the speed at the kth update, is the attitude heading angle at the kth update, NAV k The navigation state.

7. The combined navigation post-processing system of Kalman filtering combined with graph optimization according to claim 6, characterized in that: The graph optimization unit is specifically used for: Using the state error calculated by the graph optimization method to perform navigation solution on the inertial navigation system, and establishing a feedback amount for the next navigation solution according to the navigation solution result; 8. The combined navigation post-processing system of Kalman filtering combined with graph optimization according to claim 7, characterized in that: The feedback amount is in, X gk The state NAV obtained by integrating the Kalman filter at time k using the graph optimization method is k error, is the navigation solution value of the first integration of the Kalman filter to time k, N k is the number of samples from time k-1 to time k.

Images