A method and system for optimal estimation

Abstract

This invention provides an optimal estimation method and system. The method includes the following steps: modeling the system to be estimated using a continuous state-space modeling method to obtain a continuous state space; discretizing the continuous state space into a graphical state space using the characteristics of the system to be estimated; and analyzing the graphical state space using a preset graphical optimization algorithm to obtain an estimate of the system to be estimated. This method can handle large-scale sensor data, and because it uses a graphical optimization algorithm, it can accommodate larger-scale data. It can update the state of each subsystem according to the frequency of different subsystems, eliminating the need to update the system state every moment according to the Kalman filter model. It can utilize the system's constants, constraints, and different update frequencies to construct large-scale overdetermined equation systems, improving computational accuracy.

Description

Technical Field

[0001] This invention belongs to the field of data processing technology, specifically relating to an optimal estimation method and system. Background Technology

[0002] For existing optimal estimation methods, see [link to relevant documentation]. Figure 1 First, the system is modeled using a continuous state space. Then, the continuous state space is discretized into a discrete state-space time series. Finally, Kalman filtering is used for computation, assuming the system state x∈R. n x is an n-dimensional vector located in R n (n-dimensional real space).

[0003] A nonlinear system subjected to random noise perturbation can be described as follows:

[0004]

[0005] The observation equation is as follows:

[0006] z = h(x) + v (2)

[0007] where x∈R N Let f(x) represent the system state, and f(x) represent the pattern of change in the system state. It is a nonlinear function, u∈R. N Represents affine input, B∈R N×N This is the control input matrix, where q ~ N(0,Q) represents the random process noise (which can also be described using a non-Gaussian distribution). In engineering practice, it is often difficult to obtain an accurate description of nonlinear dynamic systems. Therefore, the linearized equations of the system near the origin are usually studied thoroughly. This is especially true in inertial navigation. For this type of nonlinear system, the nonlinear truncated part can be described as:

[0008] g(x)=f(x)-Ax (3)

[0009] in Let f(x) be the linearized Jacobian matrix of f(x) near the origin. Assume g(x) is a nonlinear function satisfying the Lipschitz conditions:

[0010] g(x)≤γ||x|| (4)

[0011] Thus (1) can be rearranged as follows:

[0012]

[0013] The observation equation is as follows:

[0014] z = Hx + v (6)

[0015] The measurement noise r ~ N(0,R) (which can also be described using a non-Gaussian distribution). For such a problem, a discrete state-space model is typically used for discretization, and an extended Kalman filter is used to calculate the estimate.

[0016] System model: x k =A k x k-1 +B k u k +q k (7)

[0017] Process noise: q k ~N(0,Q) k Noise can also be modeled in other ways.

[0018] Observation equation: z k =H k x k +r k Noise measurement: r k ~N(0,R k (8)

[0019] Initial conditions: E <x0>=x0(+), E<(x0-x0(+))(x0-x0(+)) T >=P0(+), assuming process noise and measurement noise are uncorrelated, i.e.

[0020] State prediction: x k (-)=A k x k-1 (+)+B k u k (9)

[0021] Prior error covariance matrix update:

[0022] Kalman gain calculation:

[0023] Posterior error covariance update: P k (+)=(IK k H k )P k (-) (12)

[0024] State estimation observation update: x k (+)=x k (-)+K k (z k -H k x k (-)) (13)

[0025] This method has the following problems:

[0026] First, it cannot handle large-scale sensor data. When multiple systems are measuring data, it is difficult to achieve high accuracy. Although federated Kalman filtering is implemented using multiple filter banks, engineering applications often use a single Kalman filter or nonlinear filter for ease of implementation. The implementation method of Kalman filtering or nonlinear filtering determines that it can only handle the optimal measurement at the current moment, which is a one-step linear unbiased optimal estimate.

[0027] Second, it cannot properly handle constraints and constants in the system. When there are constants and constraints in the system, since Kalman filtering uses a time-series state update mode, from the perspective of the entire state update equation, constants and constraints are actually affected by measurement noise at every moment, and it cannot truly use measurements to reduce the estimation error of constants and constraints.

[0028] Third, all time series must be predicted and updated at the same frequency. When the system is complex and has multiple different subsystems that are updated at different frequencies, Kalman filtering or nonlinear filtering must update the system state at the same frequency. This will increase the number of equations to be calculated and reduce the accuracy of the solution and the robustness of the system.

[0029] Graph optimization is an emerging optimal estimation technique. It uses graphical models for modeling and solves large-scale sparse state update equations for computation, enabling it to handle massive sensor data. However, graph optimization requires remodeling when addressing problems, converting the state-space model into a graphical model. Traditional conversion methods are discussed in [link to traditional methods]. Figure 2 First, the system is modeled using a continuous state space. Then, the continuous state space is discretized into a discrete state space time series. Finally, a graph optimization algorithm is used for calculation. That is, it is assumed that the discrete time series described by equations (7) to (8) is the discretization of system (1) to (2).

[0030] x1, x2, ..., x N ∈R n Let u1, u2, ... u be the state variables to be solved. N ∈R n For control inputs, q1, q2, ... q N ∈R n For process noise, q k ~N(0,Q) k ), z1, z2, ... z N ∈R n For the observables, r1, r2, ... r N ∈R n For process noise, r k ~N(0,R k Then the equation for a large sparse matrix is ​​as follows:

[0031]

[0032] The graphical representation of this equation is as follows: Figure 3 As shown, this model can be considered to possess the hidden Markov property, that is, the state at the current moment is only related to the input at the current moment, the state at the previous moment, and the observation at the current moment. Figure 2 In this context, P, Q, and R serve as weight coefficients for each block of equations. They are typically written as diagonal matrices, allowing each row of equations to be multiplied by a coefficient to balance the weight ratio of that equation within the larger sparse equation.

[0033] This method is merely a simple discretization, inheriting the discrete state approach of Kalman physics. It is a graphical model representation of the Kalman discrete state space, without utilizing the internal characteristics of the state space model. Summary of the Invention

[0034] To address the shortcomings of existing technologies, this invention provides an optimal estimation method and system that can both inherit the advantages of traditional state-space modeling methods and improve the solution accuracy.

[0035] Firstly, an optimal estimation method includes the following steps:

[0036] A continuous state space modeling method is used to model the system to be estimated, resulting in a continuous state space;

[0037] The continuous state space is discretized into a graphical state space by utilizing the characteristics of the system to be estimated.

[0038] The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimator of the system to be estimated.

[0039] Preferably, the features of the system to be estimated include system constants, time series with different update frequencies, and / or dynamic constraints.

[0040] Preferably, the graph state space is obtained by linking the nodes and edges of the graph to contain the states corresponding to the features.

[0041] Secondly, an optimal estimation system.

[0042] The system to be estimated is a combination of GNSS navigation and inertial navigation, which is a loose combination; wherein, GNSS navigation provides position to inertial navigation to suppress the divergence of inertial navigation error;

[0043] This optimal estimation system is used for:

[0044] The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space;

[0045] Set multiple time series and a constant term R. b The time series includes the inertial navigation state time series δNAV. i accelerometer error state time series and gyroscope error state time series Accelerometer Error State Time Series and gyroscope error state time series The update frequency is lower than that of the inertial navigation state time series δNAV i Update frequency; constant term R b For lever arm;

[0046] Based on the time series and constant term R b Discretize the continuous state space into a graphical state space;

[0047] The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimator of the system to be estimated.

[0048] Preferably, the step of basing the time series and the constant term R b Discretizing a continuous state space into a graphical state space specifically includes:

[0049] Assuming that the continuous state space does not update the accelerometer error and gyroscope error within a certain time period, the variable matrix of the continuous state space is:

[0050] X = [δa] b δω b R b δNAV0 δNAV1 … δNAV N-1 δNAV N ] T ;

[0051] The continuous state space is discretized into the graphical state space based on the variable matrix.

[0052] Thirdly, an optimal estimation system.

[0053] The system to be estimated is a combination of inertial navigation and RTK navigation, which is a compact combination. RTK navigation continuously observes satellites through a satellite navigation receiver at a known location and transmits the observed data to a rover receiver at the user end. The rover receiver is used to obtain the user's coordinates based on the observed data.

[0054] This optimal estimation system is used for:

[0055] The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space;

[0056] Set the phase offset without cycle slip as a constant term;

[0057] Discretize the continuous state space into a graphical state space based on the constant term;

[0058] The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimator of the system to be estimated.

[0059] Preferably, the discretization of the continuous state space into a graphical state space based on the constant term specifically includes:

[0060] Assuming the system to be estimated has no carrier phase cycle slips, the variable matrix in the continuous state space is:

[0061] The continuous state space is discretized into the graphical state space based on the variable matrix.

[0062] Fourthly, an optimal estimation system.

[0063] The system to be estimated is a combination of inertial navigation and odometry; wherein, the odometry uses its linear divergence characteristics to suppress the second divergence of system errors in inertial navigation;

[0064] This optimal estimation system is used for:

[0065] The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space;

[0066] Establish a time series using the systematic error of the odometer;

[0067] Based on this time series, the continuous state space is discretized into a graphical state space;

[0068] The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimator of the system to be estimated.

[0069] Preferably, the step of establishing a time series using the systematic error of the odometer specifically includes:

[0070] Calculate the difference between the position obtained by the odometry and the position obtained by the inertial navigation.

[0071] Set this difference as the observation;

[0072] Assuming that the continuous state space does not update accelerometer error, gyroscope error, installation error, and odometer coefficients within a certain time period, then the variable matrix of the continuous state space is:

[0073]

[0074] The continuous state space is discretized into the graphical state space based on the variable matrix.

[0075] As can be seen from the above technical solutions, the optimal estimation method and system provided by the present invention have the following advantages:

[0076] 1. It can process large-scale sensor data, and due to the use of graph optimization algorithms, it can accommodate even larger-scale data.

[0077] 2. The state of each subsystem can be updated according to the frequency of different subsystems, without having to update the system state every moment according to the Kalman filter pattern.

[0078] 3. Large-scale overdetermined equation systems can be constructed by utilizing the system's constants, constraints, and different update frequencies, thereby improving computational accuracy. Attached Figure Description

[0079] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the accompanying drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. 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 to scale.

[0080] Figure 1 A flowchart of the existing best estimation method provided for the background technology.

[0081] Figure 2 The background technology provides a graphical model of the traditional discrete state space.

[0082] Figure 3 A flowchart of a traditional graph optimization method provided for the background technology.

[0083] Figure 4 A flowchart of the optimal estimation method provided in Example 1.

[0084] Figure 5 This is a schematic diagram of the graph state-space model provided in Example 1.

[0085] Figure 6 This is a schematic diagram of the state space of the GNSS / INS integrated navigation map provided in Embodiment 2.

[0086] Figure 7 This is a schematic diagram of the state space of the inertial navigation / RTK navigation graph provided in Embodiment 3.

[0087] Figure 8 This is a schematic diagram of the state space of the inertial navigation / odometer map provided in Embodiment 4. Detailed Implementation

[0088] The embodiments of the technical solution of the present invention will now be described in detail with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and are therefore merely examples and should not be used to limit the scope of protection of the present invention. It should be noted that, unless otherwise stated, the technical or scientific terms used in this application should have the ordinary meaning understood by those skilled in the art to which this invention pertains.

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

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

[0091] As used in this specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if [described condition or event] is detected" may be interpreted, depending on the context, as "once determined," "in response to determination," "once [described condition or event] is detected," or "in response to detection of [described condition or event]."

[0092] Example 1:

[0093] An optimal estimation method, see [link to relevant documentation] Figure 4 This includes the following steps:

[0094] A continuous state space modeling method is used to model the system to be estimated, resulting in a continuous state space;

[0095] The continuous state space is discretized into a graphical state space by utilizing the characteristics of the system to be estimated.

[0096] The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimator of the system to be estimated.

[0097] Preferably, the features of the system to be estimated include system constants, time series with different update frequencies, and / or dynamic constraints.

[0098] Preferably, the graph state space is obtained by linking the nodes and edges of the graph to contain the states corresponding to the features.

[0099] Specifically, a time series is a set of random variables ordered by time. It is typically the result of observing a potential process at equal intervals according to a given sampling rate. Time series data essentially reflects the trend of one or more random variables changing over time. This method connects states with their own characteristics in the form of graph nodes and edges, establishing a graph model, i.e., a graph state space. This graph state space inherits the advantages of the state space model and, unlike the Kalman discrete state space, is a new discretization form of the continuous state space. While compressing the number of equations in large-scale sparse equation systems, the graph state space also establishes large-scale overdetermined equation systems. The state characteristics of the graph state space differ from those of the implicit Markov model, effectively improving the accuracy of state estimation.

[0100] This optimal estimation method utilizes the characteristics of constants in the system, time series with different update frequencies, and dynamic constraints to discretize the continuous state space into a graphical state space model, thus decomposing the implicit Markov properties of the equations. It has the following advantages:

[0101] 1. It can process large-scale sensor data, and due to the use of graph optimization algorithms, it can accommodate even larger-scale data.

[0102] 2. The state of each subsystem can be updated according to the frequency of different subsystems, without having to update the system state every moment according to the Kalman filter pattern.

[0103] 3. Large-scale overdetermined equation systems can be constructed by utilizing the system's constants, constraints, and different update frequencies, thereby improving computational accuracy.

[0104] See Figure 5 Assume that the discrete time series described by equations (7) to (8) are discretizations of system (1) to (2). This method utilizes the characteristics of the system to be estimated to discretize the continuous state space into time series with different update frequencies. Where 0 to S represent the time series numbers with different update frequencies, and the system to be estimated has t constants C1, C2, ... C t Given the observation sequence Z1, Z2, ..., Z M , constrain FX k +D=0, these equations, together with the process noise q and r, form a large overdetermined system of equations. Although this method has more equations per solution than Kalman filtering, the overall number of solution equations and variables is less than that of Kalman filtering.

[0105] Example 2:

[0106] Example 2 provides an estimation system for the system to be estimated, based on the above examples.

[0107] The system to be estimated is a combination of GNSS navigation and inertial navigation, which is a loose combination; wherein, GNSS navigation provides position to inertial navigation to suppress the divergence of inertial navigation error.

[0108] Specifically, in the system to be estimated, GNSS navigation and inertial navigation operate independently. If the traditional Kalman filter method is used to estimate the system, then:

[0109] The system state variable x consists of inertial navigation velocity error, inertial navigation attitude error, inertial navigation position error, accelerometer offset error, gyroscope offset error, and the lever arm (the distance between the phase center point of the GNSS receiver antenna and the center point of the inertial navigation system), i.e., x = [δv n δφ n δP n δa b δω b R b ] T ,δv n δφ represents the 3D velocity error in the navigation coordinate system n. n δP represents the projection of the angular errors in the three directions onto the navigation coordinate system. n Indicates the error in longitude, latitude, and altitude, δa b The accelerometer offset error, δω, is defined under the load system b. b R represents the accelerometer offset error. b Represents the lever arm. A continuous state space is defined as:

[0110]

[0111]

[0112] In the above formula, This represents the 3×3 direction cosine matrix from the carrier system (b) to the navigation system (n), where 0 3×3 This represents a 3×3 matrix of all zeros.

[0113]

[0114] Where lat represents the local latitude, V E V N V U They represent the northeast-to-sky velocity, h represents altitude, and ω represents the vertical velocity. ie R represents the angular velocity of Earth's rotation. normal R represents the radius of curvature of the meridional circle. meridian This represents the radius of curvature of the zonal loop.

[0115]

[0116] f E ,f N ,f U This indicates the relative increase in force in the northeast direction.

[0117]

[0118]

[0119]

[0120]

[0121]

[0122] The observation equation is: y = Hx + r;

[0123]

[0124] Then equations (15) and (23) can be discretized into Kalman state space in the manner of equations (7) and (8), calculated using Kalman filtering, and the estimated value at this moment can be obtained. As feedback quantity u k calculate.

[0125] This optimal estimation system is used for:

[0126] The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space;

[0127] Set multiple time series and a constant term R. b The time series includes the inertial navigation state time series δNAV. i accelerometer error state time series and gyroscope error state time series Accelerometer Error State Time Series and gyroscope error state time series The update frequency is lower than that of the inertial navigation state time series δNAV i Update frequency; constant term R b For lever arm;

[0128] Based on the time series and constant term R b Discretize the continuous state space into a graphical state space;

[0129] The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimator of the system to be estimated.

[0130] Preferably, the step of basing the time series and the constant term R b Discretizing a continuous state space into a graphical state space specifically includes:

[0131] Assuming that the continuous state space does not update the accelerometer error and gyroscope error within a certain time period, the variable matrix of the continuous state space is:

[0132] X = [δa] b δω b R b δNAV0 δNAV1 … δNAV N-1 δNAV N ] T ;

[0133] The continuous state space is discretized into the graphical state space based on the variable matrix.

[0134] Specifically, the system discretizes system equations (15) and (23) into three time series and one constant term, that is, the graph state-space form of the discretized equation (23) is:

[0135]

[0136]

[0137]

[0138] constant R b (27)

[0139] General settings

[0140] make

[0141] The inertial navigation state time series is δNAV i (i=1,...,N1), the accelerometer error state time series is as follows The gyroscope error state time series is The constant is R b The update frequencies of the accelerometer error state time series and gyroscope error state time series are inconsistent with those of the inertial navigation system (INS) state time series, and can be lower than those of the INS state time series. Assuming that the system to be estimated does not update the accelerometer and gyroscope offsets during a certain time period, the overall variable matrix to be determined is:

[0142] X = [δa] b δω b R b δNAV0 δNAV1 … δNAV N-1 δNAV N ] T

[0143] The equation to be solved is transformed into:

[0144]

[0145] in

[0146] HI 3×9 =[0 3×3 0 3×3 I 3×3 (33)

[0147] b = [0 9×1 u0 z1 u1 z2 … u N-1 z N ] T (34)

[0148] Equation (30) is denoted as A g X g =b g (35)

[0149] Assume prior variance P0 and process noise covariance Q. i Measurement covariance R i Since both are diagonal matrices, solving equation (30) also requires considering the weighting matrix:

[0150] P g =diag[P0Q0R1…Q N-1 R N ] T (36)

[0151] `diag` represents a diagonal matrix generated by using the vector on the right as its diagonal elements. The diagonal matrix is ​​a matrix where all elements outside the main diagonal are 0. Therefore, the least squares solution to equation (30) is:

[0152]

[0153] Therefore, it can be seen that the system completes the steps of solving the problem using a graph state-space model and graph optimization method through (30) to (37). Figure 6 As shown, this model is obviously the same as... Figure 2 In contrast, it lacks the implicit Markov property. However, if Kalman filtering is used to solve this problem, the form of the equation system becomes:

[0154]

[0155] b g =[0 1×9 0 1×3 0 1×3 0 1×3 u0 u a (0) u ω (0) u R (0) z1 … u0 u a (N-1) u ω (N-1) u R (N-1) z N ] T (39)

[0156]

[0157] There is a system of equations A N X = b (41)

[0158] If the Kalman filter method is used, then equations (38) to (41) are solved recursively in time order. The model is as follows: Figure 2 As shown in equations (30) to (41), the number of equations in this system is much less than that in Kalman filtering, especially in the lever problem. This system only needs to solve for one lever value, while Kalman filtering requires solving for many variables.

[0159] When this system is applied to the combination of GNSS and inertial navigation systems, the view arm is constant, the gyroscope offset and accelerometer offset are time series with a relatively slow frequency, and the discrete continuous state space model is a graphical state space model.

[0160] The system provided in this embodiment of the invention is described in a brief manner. For any parts not mentioned in the embodiment section, please refer to the corresponding content in the foregoing embodiment.

[0161] Example 3:

[0162] Example 3 provides another estimation system for the system to be estimated, based on the above examples.

[0163] The system to be estimated is a combination of inertial navigation and RTK navigation, which is a compact combination. RTK navigation continuously observes satellites through a satellite navigation receiver at a known location and transmits the observed data to a rover receiver at the user end. The rover receiver is used to obtain the user's coordinates based on the observed data.

[0164] This optimal estimation system is used for:

[0165] The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space;

[0166] Set the phase offset without cycle slip as a constant term;

[0167] Discretize the continuous state space into a graphical state space based on the constant term;

[0168] The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimator of the system to be estimated.

[0169] Preferably, the discretization of the continuous state space into a graphical state space based on the constant term specifically includes:

[0170] Assuming the system to be estimated has no carrier phase cycle slips, the variable matrix in the continuous state space is:

[0171] The continuous state space is discretized into the graphical state space based on the variable matrix.

[0172] Specifically, carrier phase measurement determines the phase difference between the satellite carrier signal received by the GPS receiver and the reference carrier signal generated by the receiver oscillator. Cycle slip refers to a jump or interruption in the integer cycle count caused by loss of lock on the satellite signal during carrier phase measurement in Global Navigation Satellite System (GNSS) technology.

[0173] Inertial navigation / RTK integrated navigation is a commonly used integrated navigation method in high-precision positioning applications such as autonomous driving and surveying. A common combination method involves inertial navigation assisting RTK positioning, with RTK positioning information fed back to inertial navigation as a position reference to dampen inertial navigation error divergence. Tight integration typically refers to treating the system errors of inertial navigation and RTK as states in the same state space for estimation. RTK usually involves placing a satellite navigation receiver at a base station with a known precise location to continuously observe satellites and transmit the observation data and coordinate information from the base station to the rover receiver at the user end via radio transmission equipment in real time. The rover receives satellite signals and information from the base station, processes the data in real time according to the principle of relative positioning, and provides the user station with three-dimensional coordinates with centimeter-level accuracy.

[0174] RTK (Real-Time Kinematics) often employs the difference method, which eliminates redundant parameters by subtracting observations, thereby reducing the order of the equations, the workload of solving the problem, and improving accuracy. The most commonly used difference method is double-difference observation, which uses the rover and base station receivers to simultaneously measure the observations from two satellites and then performs difference analysis on the single-difference results from the two different satellites to further eliminate receiver clock bias in the measurements.

[0175] If n+1 satellites are observed within an observation epoch, numbered 0, 1, 2, ..., n, and satellite 0 is selected as the reference satellite, then the observation equation for one epoch can be formed as follows:

[0176]

[0177] It is written as V = LδX u +DB+r (43)

[0178] The coordinate error of the rover station is expressed by the following formula:

[0179] δX u =[δx u δy u δz u ] T (44)

[0180] B. Floating-point single-difference carrier phase offset is expressed by the following formula:

[0181] B = [B 0 B 1 B 2 … B n ] T (45)

[0182]

[0183] x k ,y k ,z k This represents the coordinates of the k-th satellite. This represents the distance calculated between the rover receiver and the k-th satellite. The coordinates calculated by the rover are:

[0184]

[0185] The implicit calculation error in the above equation is represented by (44). The observation equation for the difference between the position error of inertial navigation and the position error of RTK is:

[0186]

[0187] Let the system error state be:

[0188] X = [δNAV] e δa b δω b δX u B] T (49)

[0189] δNAV e This represents the projection of the inertial navigation error in equation (24) onto the e-frame. Therefore, the continuous state equation of the system is:

[0190]

[0191]

[0192] In the above formula, FF e This is the projection of equation (29) onto the e-frame (geocentric coordinate system). The above equation assumes no carrier phase cycle slips during operation; if cycle slips exist, B is reinitialized. The continuous observation equation for the system is:

[0193] Z = Hx + r

[0194]

[0195] Discretize equations (50) to (51) according to equations (7), (8) and (14). By solving the Kalman filter update equation, we can obtain the estimated values ​​of the rover antenna position, velocity, floating-point single-difference carrier phase deviation, inertial navigation position error and boom arm status.

[0196] Suppose that during a certain time interval, the graph optimization state space does not update the accelerometer or gyroscope offsets, and there are no cycle slips within this update interval. Then the overall variables to be solved are:

[0197]

[0198]

[0199]

[0200] A t X t =b t (55)

[0201] Solving equation (55) also requires considering the weighting matrix:

[0202] P t =diag[P0 Q0 R1 … Q N-1 R N ] T (56)

[0203] diag represents generating a diagonal matrix by using the right-hand vector as diagonal elements. Then the least squares solution to equation (55) is:

[0204]

[0205] Solving equations (30) to (37) using a graph optimization algorithm completes the steps of solving this problem using a graph state-space model and graph optimization method. The graph state-space model represented by equations (52) to (57) is as follows: Figure 7 As shown.

[0206] When applied to inertial navigation / RTK integrated navigation, this system uses the phase offset at the end of the cycle slip as a constant to discretize the continuous state-space model into a graphical state-space model.

[0207] The system provided in this embodiment of the invention is described in a brief manner. For any parts not mentioned in the embodiment section, please refer to the corresponding content in the foregoing embodiment.

[0208] Example 4:

[0209] Example 4 provides another estimation system for the system to be estimated, based on the above examples.

[0210] The system to be estimated is a combination of inertial navigation and odometry; wherein, the odometry uses its linear divergence characteristics to suppress the second divergence of system errors in inertial navigation;

[0211] This optimal estimation system is used for:

[0212] The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space;

[0213] Establish a time series using the systematic error of the odometer;

[0214] Based on this time series, the continuous state space is discretized into a graphical state space;

[0215] The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimator of the system to be estimated.

[0216] Preferably, the step of establishing a time series using the systematic error of the odometer specifically includes:

[0217] Calculate the difference between the position obtained by the odometry and the position obtained by the inertial navigation.

[0218] Set this difference as the observation;

[0219] Assuming that the continuous state space does not update accelerometer error, gyroscope error, installation error, and odometer coefficients within a certain time period, then the variable matrix of the continuous state space is:

[0220]

[0221] The continuous state space is discretized into the graphical state space based on the variable matrix.

[0222] Specifically, the combined navigation of inertial navigation and odometer generally refers to the combination of land-based inertial navigation and odometer, which utilizes the linear divergence characteristics of the odometer to suppress the secondary divergence of inertial navigation errors.

[0223] If Kalman filtering is used to estimate the system, the system state variable x consists of inertial navigation velocity error, inertial navigation attitude error, inertial navigation position error, accelerometer offset error, gyroscope offset error, odometer installation error angle (pitch installation error and heading installation error), odometer coefficient error, and odometer dead reckoning position error.

[0224]

[0225] Where δγ b The odometer installation error angle is represented by δK, and the odometer coefficient error is represented by δK. This represents the position error calculated from the odometry dead reckoning. The continuously linearized system equations are defined as follows:

[0226]

[0227]

[0228] in

[0229]

[0230]

[0231]

[0232]

[0233] The observation equation is:

[0234]

[0235] The system employs a graph state-space modeling method, which discretizes the system (58) into six time series or constant terms. This significantly alters the structure of the system of equations to be solved. δNAV and δa are constructed according to equations (24) to (26). b ,δω b The updates can be considered as constants for δγ and δK within the update cycle. Update according to the following formula:

[0236]

[0237] Constants δγ, δK(67)

[0238] The difference between the inertial navigation system position and the odometer position is used as the observation, i.e.:

[0239]

[0240] Suppose that during a certain time period, the graph optimization state space does not update the accelerometer, gyroscope offset, installation error, and odometer coefficients, then the overall variables to be solved are:

[0241]

[0242]

[0243]

[0244] A O X O =b O (72)

[0245] Solving equation (72) also requires considering the weighting matrix:

[0246] P O =diag[P0 Q0 R1 … Q N-1 R N ] T (73)

[0247] diag represents generating a diagonal matrix by using the right-hand vector as diagonal elements. Therefore, the least squares solution to equation (72) is:

[0248]

[0249] Solving equations (69) to (74) using a graph optimization algorithm completes the steps of solving the problem using a graph state-space model and graph optimization method. The graph state-space model represented by equations (69) to (72) is as follows: Figure 8 As shown.

[0250] When this system is applied to inertial navigation and odometry-assisted mapping problems, it uses the coefficient error of the odometry to establish a time series different from that of inertial navigation, and discretizes the continuous state-space model into a graphical state-space model.

[0251] The system provided in this embodiment of the invention is described in a brief manner. For any parts not mentioned in the embodiment section, please refer to the corresponding content in the foregoing embodiment.

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

Claims

1. An optimal estimation method, characterized in that, Includes the following steps: A continuous state-space modeling method is used to model the system to be estimated, resulting in a continuous state space. The system to be estimated is a combination of GNSS navigation and inertial navigation, which is loosely combined. GNSS navigation provides position information to inertial navigation to suppress the divergence of inertial navigation errors. The continuous state space is discretized into a graphical state space by utilizing the characteristics of the system to be estimated. The optimal estimation method includes: The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space; Set multiple time series and a constant term R. b The time series includes the inertial navigation state time series δNAV. i accelerometer error state time series and gyroscope error state time series Accelerometer error state time series and gyroscope error state time series The update frequency is lower than that of the inertial navigation state time series δNAV i The update frequency; constant term R b For lever arm; Based on the time series and constant term R b Discretize the continuous state space into a graphical state space; The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimators of the system to be estimated. The time series and constant term R are used as the basis b Discretizing a continuous state space into a graphical state space specifically includes: Assuming that the continuous state space does not update the accelerometer error and gyroscope error within a certain time period, the variable matrix of the continuous state space is: ； The continuous state space is discretized into the graphical state space based on the variable matrix.

2. The optimal estimation method according to claim 1, characterized in that, The features of the system to be estimated include system constants, time series with different update frequencies, and / or dynamic constraints.

3. The optimal estimation method according to claim 2, characterized in that, The graph state space is obtained by linking the nodes and edges of the graph to contain the states corresponding to the features.

4. An optimal estimation system, characterized in that, A continuous state space modeling method is used to model the system to be estimated, resulting in a continuous state space; The system to be estimated is a combination of GNSS navigation and inertial navigation, which is a loose combination; wherein, GNSS navigation provides position to inertial navigation to suppress the divergence of inertial navigation error; This optimal estimation system is used for: The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space; Set multiple time series and a constant term R. b The time series includes the inertial navigation state time series δNAV. i accelerometer error state time series and gyroscope error state time series Accelerometer error state time series and gyroscope error state time series The update frequency is lower than that of the inertial navigation state time series δNAV i The update frequency; constant term R b For lever arm; Based on the time series and constant term R b Discretize the continuous state space into a graphical state space; The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimators of the system to be estimated. The time series and constant term R are used as the basis b Discretizing a continuous state space into a graphical state space specifically includes: Assuming that the continuous state space does not update the accelerometer error and gyroscope error within a certain time period, the variable matrix of the continuous state space is: ； The continuous state space is discretized into the graphical state space based on the variable matrix.

5. An optimal estimation system, characterized in that, A continuous state space modeling method is used to model the system to be estimated, resulting in a continuous state space; The system to be estimated is a combination of inertial navigation and RTK navigation, which is a compact combination. RTK navigation continuously observes satellites through a satellite navigation receiver at a known location and transmits the observed data to a rover receiver at the user end. The rover receiver is used to obtain the user's coordinates based on the observed data. This optimal estimation system is used for: The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space; Set the phase offset without cycle slip as a constant term; Discretize the continuous state space into a graphical state space based on the constant term; The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimators of the system to be estimated. The discretization of the continuous state space into a graphical state space based on the constant term specifically includes: Assuming the system to be estimated has no carrier phase cycle slip, the variable matrix in the continuous state space is: The accelerometer error state time series consists of three directions. The vector formed The gyroscope error state time series consists of three directions. The vector formed , Indicates lever arm error, Represents satellite ambiguity, a 9-dimensional vector. , representing the 3D position, 3D velocity, and 3D attitude error calculated by the system at time k. This indicates the receiver positioning error; The continuous state space is discretized into the graphical state space based on the variable matrix.

6. An optimal estimation system, characterized in that, A continuous state space modeling method is used to model the system to be estimated, resulting in a continuous state space; The system to be estimated is a combination of inertial navigation and odometry; wherein, the odometry uses its linear divergence characteristics to suppress the second divergence of system errors in inertial navigation; This optimal estimation system is used for: The system to be estimated is modeled using the continuous state space modeling method to obtain the continuous state space; Establish a time series using the systematic error of the odometer; Based on this time series, the continuous state space is discretized into a graphical state space; The graph state space is analyzed using a pre-defined graph optimization algorithm to obtain the estimators of the system to be estimated. The specific steps of establishing a time series using the systematic error of the odometer include: Calculate the difference between the position obtained by the odometry and the position obtained by the inertial navigation. Set this difference as the observation; Assuming that the continuous state space does not update accelerometer error, gyroscope error, installation error, and odometer coefficients within a certain time period, then the variable matrix of the continuous state space is: The continuous state space is discretized into the graphical state space based on the variable matrix; The accelerometer error state time series consists of three directions. The vector formed The gyroscope error state time series consists of three directions. The vector formed Odometer installation error Mileage coefficient error 9-dimensional vector , representing the 3D position, 3D velocity, and 3D attitude error calculated by the system at time k. This represents the difference between the position calculated by the odometer and the position error of the inertial navigation system at time k.

Images