A positioning method based on collaborative fusion of factor graph and filtering

Abstract

The present invention discloses a positioning method based on collaborative fusion of factor graph and filtering, specifically as follows: Step 1: Adopt a GNSS multi-channel parallel processing mode to generate local initial local carrier signals and local pseudo-codes; perform orthogonal separation on the local carrier signals to generate orthogonal signals, and mix the orthogonal signals with intermediate frequency signals respectively to obtain orthogonal signals after carrier stripping; Step 2: Perform pseudo-code correlation operations on the orthogonal signals after carrier stripping with the local pseudo-codes respectively to obtain in-phase and quadrature branch leading, prompt, and lag correlation values, integrate and sum each correlation value and transmit the results to a pre-filter respectively; Step 3: Based on the output results of the pre-filter, use an FGO-EKF filter to calculate the position and velocity of the carrier at the current moment; Step 4: According to the position and velocity calculated in Step 3, update the local carrier signals and local pseudo-codes at the current moment; then turn to Step 1.

Description

Technical Field

[0001] The present invention belongs to the technical field of baseband signal processing, and particularly relates to a positioning method based on collaborative fusion of factor graph and filtering. Background Art

[0002] With the rapid development of fields such as intelligent transportation and driverless, the requirements for the accuracy and safety of satellite navigation positioning systems are getting higher and higher. Although the traditional vector tracking algorithm improves the high-dynamic performance of the receiver with the assistance of inertial navigation information, inertial navigation has the characteristic of error accumulation and the cost of high-precision inertial navigation is relatively high, making it difficult to apply in some complex environments.

[0003] Currently, with the full completion of the Beidou system and the continuous promotion of the Beidou industry and applications, the environments that GNSS receivers need to face are becoming increasingly diverse. In challenging environments (such as interference, spoofing, multipath, etc.), satellite signals face various problems such as low signal strength, multipath interference, electronic spoofing, and high dynamics. At this time, a multi-source information fusion system becomes a preferred solution. Traditional multi-source fusion technologies that include satellite measurements mainly rely on the performance of each independent system and fail to effectively improve the performance of the satellite navigation system itself through the fusion method. Summary of the Invention

[0004] Object of the Invention: To solve the problems existing in the above-mentioned prior art, the present invention provides a positioning method based on collaborative fusion of factor graph and filtering.

[0005] Technical Solution: The present invention provides a positioning method based on collaborative fusion of factor graph and filtering, which specifically includes the following steps:

[0006] Step 1: Adopt a GNSS multi-channel parallel processing mode. Each channel captures the initial satellite signal respectively, and generates corresponding initial local carrier signals and local pseudo-codes according to the capture results by using a local NCO.

[0007] Step 2: Orthogonally separate the local carrier signals of each channel to generate orthogonal signals, and mix these pairs of orthogonal signals with the intermediate-frequency signals respectively to obtain a pair of orthogonal signals after carrier stripping.

[0008] Step 3: Perform pseudo-code correlation operations on the orthogonal signals after carrier stripping with the local pseudo-codes respectively to obtain in-phase and quadrature branch early, prompt, and late correlation values. Integrate and sum each correlation value and transmit the results to a pre-filter respectively to obtain carrier phase discrimination, carrier frequency discrimination, and pseudo-code phase discrimination results.

[0009] Step 4: Construct an FGO-EKF filter. Based on the carrier phase discrimination, carrier phase and frequency discrimination, and pseudo-code phase discrimination results obtained in Step 2, use the FGO-EKF filter to calculate the position and velocity of the carrier at the current moment k.

[0010] Step 5: Calculate the pseudo-code phase, carrier frequency, and pseudo-code frequency of the satellite at the current moment based on the position and velocity at the current moment k calculated in Step 3; update the local carrier signal and local pseudo-code according to the pseudo-code phase, carrier frequency, and pseudo-code frequency at the current moment, and then go to Step 2.

[0011] Furthermore, the pre-filter in Step 3 adopts an EKF filter, and the system state variables of the pre-filter are expressed as follows:

[0012]

[0013] where pre represents the pre-filter, represents the state transition matrix of the pre-filter from moment k - 1 to moment k, represents the process noise of the pre-filter at moment k - 1, is expressed as:

[0014]

[0015] where T int represents the coherent integration time, f code represents the pseudo-code frequency of the channel signal, f s represents the carrier frequency of the channel signal; the system state variables of the pre-filter include the carrier phase error the Doppler frequency error δf of the carrier carrier the Doppler frequency change error δa of the carrier carrier the pseudo-code phase error δτ, and the signal amplitude A;

[0016] The expression of the measurement equation of the pre-filter is as follows:

[0017] Z pre = H pre ΔX pre + V pre

[0018] where H pre represents the measurement coefficient matrix of the pre-filter, V pre represents the measurement noise of the pre-filter, and the expression of H pre is as follows:

[0019]

[0020] I L is the integration result of the I-branch sequence after correlation with the local lag code; I P is the integration result of the I-branch sequence after correlation with the local prompt code; I Eis the integration result of the I-branch sequence after correlation with the local early code; Q L is the integration result of the Q-branch sequence after correlation with the local late code; Q P is the integration result of the Q-branch sequence after correlation with the local prompt code; Q E is the integration result of the Q-branch sequence after correlation with the local early code.

[0021] Furthermore, the FGO-EKF filter includes FGO and EKF navigation filters.

[0022] Furthermore, the specific steps of step 4 are as follows:

[0023] Step 4.1: Calculate the pseudorange residual and pseudorange rate residual based on the carrier phase discrimination, carrier phase discrimination and frequency discrimination, and pseudocode phase discrimination obtained in step 2;

[0024] Step 4.2: Based on the pseudorange residual and pseudorange rate residual, and use the EKF navigation filter to calculate the position and velocity of GNSS;

[0025] Step 4.3: Use the FGO filter to construct GNSS, IMU and other sensor factor nodes, and obtain the position and velocity of the carrier at the current moment through graph optimization. The other sensors include sensors on the carrier other than GNSS and IMU.

[0026] Furthermore, the pseudorange residual δρ n and the pseudorange rate residual are expressed as follows:

[0027]

[0028] where δτ n represents the pseudocode phase error of the nth satellite, c represents the speed of light, represents the pseudocode frequency of the nth satellite, represents the carrier frequency of the nth satellite.

[0029] Furthermore, the state quantity ΔX of the EKF navigation filter gnav = [δp, δv, δt b , δt d Τ , where δp represents the position error of GNSS in the ECEF coordinate system, δv represents the velocity error of GNSS in the ECEF coordinate system, δt b and δt d represent the clock error and clock drift error respectively;

[0030] The prediction equation of the state quantity is as follows:

[0031] ​

[0032] Among them, gnav represents the EKF navigation filter, represents the process noise of the EKF navigation filter at time k-1, represents the state transition matrix of the EKF navigation filter at time k-1, The expression of is as follows:

[0033]

[0034] T nav represents the time interval of satellite position calculation at time k-1;

[0035] The measurement vector Z of the EKF navigation filter gnav The expression of is as follows:

[0036] includes: Z gnav =H gnav ΔX gnav +V gnav

[0037] Among them, V gnav represents the measurement noise, ΔX gnav represents the state quantity of the EKF navigation filter, and the measurement vector Z gnav includes the pseudorange error of n satellites and the pseudorange rate error of n satellites; H gnav represents the measurement coefficient matrix of the EKF navigation filter.

[0038] Furthermore, the expression of the GNSS factor node in step 4.3 is as follows:

[0039]

[0040] Among them, represents the GNSS factor node at time k, represents the state quantity in the FGO filter at time k, nav represents the FGO filter; Z GNSS =[p g v g T , where p g represents the position of GNSS, v g represents the velocity of GNSS, and T represents the transpose.

[0041] Furthermore, the expression of the pseudocode phase of the satellite at the current moment in step 5 is:

[0042]

[0043] Among them, ​Represents the pseudo-code phase of the nth satellite signal at the kth moment, and ΔT represents the time interval from the (k - 1)th moment to the kth moment of GNSS. and Represent the satellite position and the GNSS position at the kth moment respectively, and ||.|| represents the Euclidean distance. Represents the change in GNSS clock error from the (k - 1)th moment to the kth moment. Represents the change in the clock error of the nth satellite from the (k - 1)th moment to the kth moment. Represents the pseudo-code frequency of the nth satellite signal received by GNSS at the kth moment, and c represents the speed of light.

[0044] Furthermore, the expression for the carrier frequency of the satellite at the current moment in step 5 is:

[0045]

[0046] Wherein, Represents the carrier frequency of the nth satellite signal received at the kth moment. Represents the carrier frequency when the nth satellite signal is transmitted. and Represent the velocity vectors of GNSS and the nth satellite at the kth moment respectively. Represents the clock drift of the nth satellite at the kth moment. Represents the clock drift of GNSS at the kth moment, I n Represents the unit vector of GNSS on the line of sight of the nth satellite, and c represents the speed of light.

[0047] Furthermore, the expression for the pseudo-code frequency of the satellite at the current moment in step 5 is:

[0048]

[0049] Wherein, Represents the pseudo-code frequency of the nth satellite signal received at the kth moment. Represents the pseudo-code frequency when the nth satellite signal is transmitted. and Represent the velocity vectors of GNSS and the nth satellite at the kth moment respectively. Represents the clock drift of the nth satellite at the kth moment. Represents the clock drift of GNSS at the kth moment, I n Represents the unit vector of GNSS on the line of sight of the nth satellite, and c represents the speed of light.

[0050] Beneficial effects:

[0051] 1. The present invention has the characteristic of flexible access, can real-time eliminate the satellite channels with large pseudo-range residuals in the receiver, and improve the satellite positioning accuracy.

[0052] 2. In the EKF filtering algorithm of the present invention, the estimation of the system loop carrier phase, carrier frequency, and pseudo-code phase depends on the measurement information of the factor nodes in the factor graph. When the satellite measurement information is abnormal in a short time, the pseudo-range compensation calculated by the fusion of other factor nodes can be used to improve the stability and anti-interference ability of the receiver in a complex environment.

[0053] 3. The FGO-EKF filter adopted by the present invention optimizes the covariance of the factor nodes using the state estimation covariance of the EKF compared with the traditional deep combination. At the same time, it can also optimize the historical information, further smooth the trajectory, and improve the navigation accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 is the tracking loop of the present invention.

[0055] Figure 2 is the structural diagram of the FGO-EKF filter. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0056] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention.

[0057] The core idea of the present invention is to propose a vector tracking algorithm based on the collaborative fusion of factor graph and filtering based on the signal processing model of the software receiver and the idea of multi-source fusion. As Figure 1 shown, it includes a total of 4 parts:

[0058] Step 1: Adopt the GNSS multi-channel parallel processing mode. Each channel captures the initial satellite signal respectively, and generates the corresponding initial local carrier signal and local pseudo-code according to the capture result; perform orthogonal separation on the local carrier signal of each channel to generate orthogonal signals, and mix these pairs of orthogonal signals with the intermediate frequency signal respectively to obtain a pair of orthogonal signals after carrier stripping. The intermediate frequency signal is obtained by mixing the radio frequency signal collected by the satellite antenna and then down-converting.

[0059] Step 2: Perform pseudo-code correlation operations on the pair of orthogonal signals after carrier stripping in Step 1 with the local pseudo-code respectively to obtain the in-phase, quadrature branch early, prompt, and late correlation values. Integrate and sum each branch sequence (that is, each correlation value) and transmit the results to the EKF pre-filter respectively to obtain the carrier phase discrimination, carrier frequency discrimination, and pseudo-code phase discrimination results.

[0060] Step 3: Based on the carrier phase discrimination and frequency discrimination, and the pseudo-code phase discrimination results obtained in Step 2, then calculate navigation information such as pseudo-range residuals and pseudo-range rate residuals as measurement information, and use position error, velocity error, and receiver error as state variables, and transmit them to the FGO-EKF filter for filtering estimation respectively; the FGO-EKF filter includes an FGO filter and an EKF navigation filter;

[0061] Step 4: Use the filtering estimation results to predict the carrier frequency and pseudo-code frequency respectively, so as to control the carrier NCO and pseudo-code NCO to generate new local carrier signals and local pseudo-codes, update the local carrier signals and local pseudo-codes according to the pseudo-code phase, carrier frequency, and pseudo-code frequency at the current moment, and then go to Step 1.

[0062] As Figure 2 shown, Step 3 is divided into the following steps:

[0063] 3.1 First, calculate the pseudo-range and pseudo-range rate (satellite) according to the carrier phase discrimination and frequency discrimination, and the pseudo-code phase discrimination results output by the EKF pre-filter to obtain the measurement of the EKF navigation filter. The calculation formula is as follows:

[0064]

[0065] where, δτ n represents the pseudo-code phase error of the nth satellite, c represents the speed of light, represents the pseudo-code frequency of the nth satellite, represents the carrier frequency of the nth satellite.

[0066] 3.2 Secondly, obtain the current state through the optimal estimation of the EKF navigation filter. The state variables are constructed as follows:

[0067] ΔX gnav =[δp,δv,δt b ,δt d Τ

[0068] where δp represents the position error of GNSS in the ECEF coordinate system, δv represents the velocity error of GNSS in the ECEF coordinate system, δt b and δt d represent the clock error and clock drift error respectively.

[0069] The state prediction equation is constructed as follows:

[0070]

[0071] where, gnav represents the EKF navigation filter, represents the process noise of the EKF navigation filter at the k-1 moment, ​Denote the state transition matrix of the EKF navigation filter at time k-1; T nav Denote the time interval of satellite position calculation at time k-1.

[0072] In the navigation filter FGO-EKF, the measurement vector is constructed as follows:

[0073]

[0074] where, δρ n Denote the pseudorange error of the nth satellite, Denote the pseudorange rate error of the nth satellite, T denotes transpose.

[0075] The measurement equation is constructed as follows:

[0076] Z gnav =H gnav ΔX gnav +V gnav

[0077]

[0078] where, V gnav Denote the measurement noise, ΔX gnav Denote the state quantity of the EKF navigation filter, n denotes the nth satellite, r i Denote the distance between the ith satellite and the receiver, (x k ,y k ,z k ) denote the predicted position of the receiver at time k.

[0079] 3.3 Finally, use the FGO filter to construct the GNSS factor node, use the FGO filter to construct the factor nodes for other sensors, construct the factor graph through the factor nodes, and obtain the position and velocity of the carrier at the current moment through graph optimization. The other sensors include the IMU sensor combined with satellite measurement information and other sensor measurements to construct the factor graph.

[0080] The FGO-EKF filter contains an FGO filter. The FGO filter constructs the state equation of the overall system as follows;

[0081]

[0082] where, the superscript nav denotes the navigation filter, and Denote the system state quantities at time k-1 and time k, f(.) denotes the sensor solution equation, Denote the sensor solution noise at time k-1, which is default Gaussian white noise, Denote the system measurement quantity at time k, h(.) denotes the system measurement equation, Represents the system measurement noise at time k.

[0083] The state variables in the state equation are:

[0084]

[0085] Where Represents the attitude angle error, including δv represents the velocity error, including δv E , δv N , δv U ; δp is the position error, including δp N , δp E , δp U . Here, the reference coordinate system of the navigation system is the northeast-up navigation coordinate system. Before fusion, the measurement information of each sensor needs to be converted to the northeast-up coordinate system.

[0086] In the FGO-EKF filter, the position and velocity information obtained by the EKF output satellite ephemeris solution is used to construct the satellite factor nodes in FGO. The node equations are constructed as follows:

[0087]

[0088] Where, Represents the GNSS factor node at time k, Represents the state quantity in the FGO filter at time k, nav represents the FGO filter; Z GNSS = [p g v g T , where p g Represents the position of GNSS, v g Represents the velocity of GNSS. The GNSS position and velocity are output by the EKF navigation filter and need to be converted from the ECEF coordinate system to the northeast-up coordinate system. T represents the transpose.

[0089] The factor node equation of the IMU constructed by the position, velocity and attitude information obtained by IMU pre-integration is

[0090]

[0091] Where, the superscript IMU represents inertial navigation, Z IMU Is the measurement value of inertial navigation, h(.) represents the inertial navigation pre-integration process, Σ IMU Represents the covariance matrix of inertial navigation. The covariance matrix is obtained by the error function and is used to correct the inertial navigation parameters during the pre-integration process.

[0092] Based on the absolute position and velocity information in the northeast celestial coordinate system obtained by the solution of other sensors, where the other sensors are set according to the environment where the carrier is located. For example, in an urban environment with many high-rise buildings and roads, the absolute position and velocity obtained by using the iterative closest point algorithm of lidar sensors in combination with the initial pose and time frequency; in a high-altitude environment with a priori map, the absolute position and velocity obtained by scene matching and time frequency, etc. The constructed factor node equation is

[0093]

[0094] Among them, the superscript SENSOR represents other sensors, k represents the moment, and ∑ SENSOR represents the covariance matrix of other sensors. The measurement value of other sensors is Z SENSOR =[p s v s T , and the satellite and other sensor factor nodes are constructed as unary nodes. Among the above three types of factor nodes, the satellite measurement information is obtained by the solution of the navigation filter, and the IMU and other sensor information are input through the system.

[0095] In the FGO-EKF filter, the maximum a posteriori probability estimation formula is used for position and velocity solution, and the Levenberg-Marquardt algorithm is used to solve the non-linear least squares problem in the state estimation process. The optimal state variable of the system at moment k The solution process is as follows:[[]]END]]

[0096]

[0097] Among them, H prior represents the prior equation, represents the measurement vector of the IMU at moment i, represents the GNSS measurement vector at moment i, represents the measurement vector of other sensors at moment i; v IMU represents the covariance matrix of the IMU, and H IMU represents the IMU measurement equation, and n represents the length of the sliding window.

[0098] The prediction process for the pseudo-code frequency and carrier frequency in step 4 is as follows:[[]]END]]

[0099] Estimate the pseudo-code phase at the current moment according to the position result after filtering by the navigation filter FGO-EKF. The formula is

[0100]

[0101] Among them, ​represents the pseudo-code phase of the nth satellite signal at the kth moment, and ΔT represents the time interval from the (k - 1)th moment to the kth moment of GNSS. and represent the satellite position and the GNSS position at the kth moment respectively, and ||.|| represents the Euclidean distance. represents the change in GNSS clock error from the (k - 1)th moment to the kth moment. represents the change in the clock error of the nth satellite from the (k - 1)th moment to the kth moment. represents the pseudo-code frequency of the nth satellite signal received by GNSS at the kth moment, and c represents the speed of light.

[0102] The formula for estimating the carrier frequency at the current moment is as follows:

[0103]

[0104] where represents the carrier frequency of the nth satellite signal received at the kth moment. represents the carrier frequency when the nth satellite signal is transmitted. and represent the velocity vectors of GNSS and the nth satellite at the kth moment respectively. represents the clock drift of the nth satellite at the kth moment. represents the clock drift of GNSS at the kth moment, I n represents the unit vector of GNSS on the line of sight of the nth satellite, and c represents the speed of light.

[0105] The formula for estimating the current pseudo-code frequency is as follows:

[0106]

[0107] where represents the pseudo-code frequency of the nth satellite signal received at the kth moment. represents the pseudo-code frequency when the nth satellite signal is transmitted. and represent the velocity vectors of GNSS and the nth satellite at the kth moment respectively. represents the clock drift of the nth satellite at the kth moment. represents the clock drift of GNSS at the kth moment, I n represents the unit vector of GNSS on the line of sight of the nth satellite, and c represents the speed of light.

[0108] In addition, it should be noted that, in the above specific embodiments, the various specific technical features described can be combined in any suitable way without contradiction. To avoid unnecessary repetition, the present invention will not describe various possible combination methods separately.

Claims

1. A positioning method based on the collaborative fusion of factor graph and filtering, characterized in that: The specific steps include: Step 1: Using the GNSS multi-channel parallel processing mode, each channel captures the initial satellite signal respectively, and uses the local NCO to generate the corresponding initial local carrier signal and local pseudo code according to the capture result; Step 2: Perform orthogonal separation on the local carrier signal of each channel to generate an orthogonal signal, and mix the pair of orthogonal signals with the intermediate frequency signal respectively to obtain a pair of orthogonal signals after carrier stripping; Step 3: Perform pseudo-code correlation operations on the orthogonal signals after carrier stripping with the local pseudo-code to obtain the leading, immediate, and lagging correlation values ​​on the in-phase and quadrature branches, integrate and sum each correlation value, and transmit the results to the pre-filter to obtain the carrier phase detection, carrier frequency detection, and pseudo-code phase detection results; Step 4: Construct an FGO-EKF filter. Based on the carrier phase detection, carrier phase and frequency detection, and pseudo code phase detection results obtained in step 2, use the FGO-EKF filter to calculate the position and velocity of the carrier at the current time k; Step 5: According to the position and speed at the current time k calculated in step 3, calculate the satellite's current pseudo code phase, the current carrier frequency and the current pseudo code frequency; update the local carrier signal and local pseudo code according to the current pseudo code phase, carrier frequency and pseudo code frequency, and then go to step 2.

2. A positioning method based on collaborative fusion of factor graph and filtering according to claim 1, characterized in that: The pre-filter in step 3 adopts an EKF filter, and the system state quantity of the pre-filter The expression is as follows: Among them, pre represents the pre-filter, represents the state transition matrix of the pre-filter from time k-1 to time k, represents the process noise of the prefilter at time k-1, The expression is: Among them, T int represents the coherent integration time, f code Indicates the channel signal pseudo code frequency, f s Indicates the channel signal carrier frequency; the system state quantity of the pre-filter includes the carrier phase error The Doppler frequency error of the carrier δf carrier , the Doppler frequency variation error of the carrier δa carrier , pseudo code phase error δτ, signal amplitude A; The measurement equation of the pre-filter is expressed as follows: With pre =H pre ΔX pre +V pre Among them, H pre represents the measurement coefficient matrix of the pre-filter, V pre represents the measurement noise of the prefilter, H pre The expression is as follows: I L is the integration result of the I branch sequence after correlation with the local lag code; I P is the I branch sequence integration result after correlation with the local instant code; I E is the integration result of the I branch sequence after correlation with the local advance code; Q L is the Q branch sequence integration result after correlation with the local delayed code; Q P is the Q branch sequence integration result after correlation with the local instant code; Q E is the integration result of the Q branch sequence after correlation with the local advance code.

3. The positioning method based on the collaborative fusion of factor graph and filtering according to claim 1 is characterized in that: The FGO-EKF filter includes FGO and EKF navigation filters.

4. The positioning method based on the collaborative fusion of factor graph and filtering according to claim 3 is characterized in that: The step 4 is specifically as follows: Step 4.1: Calculate the pseudorange residual and pseudorange rate residual according to the carrier phase detection, carrier phase and frequency detection and pseudocode phase detection obtained in step 2; Step 4.2: Based on the pseudorange residual and pseudorange rate residual, the GNSS position and velocity are calculated using the EKF navigation filter; Step 4.3: Use FGO filter to construct GNSS, IMU and other sensor factor nodes, and obtain the position and speed of the carrier at the current moment through graph optimization. The other sensors include sensors other than GNSS and IMU on the carrier.

5. A positioning method based on collaborative fusion of factor graph and filtering according to claim 4, characterized in that: The pseudorange residual δρ n and pseudorange rate residual The expression is as follows: Among them, δτ n represents the pseudo code phase error of the nth satellite, c represents the speed of light, represents the pseudo code frequency of the nth star, Indicates the carrier frequency of the nth star.

6. The positioning method based on the collaborative fusion of factor graph and filtering according to claim 4 is characterized in that: The state quantity ΔX of the EKF navigation filter gnav =[δp,δv,δt b ,δt d ] Τ , where δp represents the position error of GNSS in the ECEF coordinate system, δv represents the velocity error of GNSS in the ECEF coordinate system, and δt b and δt d Represent the clock error and clock drift error respectively; The prediction equation of the state quantity is as follows: Where gnav represents the EKF navigation filter, represents the process noise of the EKF navigation filter at time k-1, represents the state transition matrix of the EKF navigation filter at time k-1, The expression is as follows: T nav represents the time interval for calculating the satellite position at time k-1; EKF navigation filter measurement vector Z gnav The expression is as follows: Includes: Z gnav =H gnav ΔX gnav +V gnav Among them, V gnav Denotes the measurement noise, ΔX gnav Represents the state of the EKF navigation filter, the measurement vector Z gnav Including the pseudo-range errors of n satellites and the pseudo-range rate errors of n satellites; H gnav represents the measurement coefficient matrix of the EKF navigation filter.

7. The positioning method based on the collaborative fusion of factor graph and filtering according to claim 4 is characterized in that: The expression of the GNSS factor node in step 4.3 is as follows: in, represents the GNSS factor node at time k, represents the state of the FGO filter at time k, nav represents the FGO filter; Z GNSS =[p g v g ] T , where p g represents the GNSS position, v g represents the speed of GNSS and T represents transpose.

8. The positioning method based on the collaborative fusion of factor graph and filtering according to claim 1 is characterized in that: The expression of the pseudo code phase of the satellite at the current time in step 5 is: in, represents the pseudo code phase of the nth satellite signal at the kth time, ΔT represents the time interval from the k-1th time to the kth time of GNSS, and represent the satellite position and GNSS position at the kth moment respectively, ||.|| represents the Euclidean distance, Indicates the change in GNSS clock error from time k-1 to time k, It represents the change of the clock error of the nth satellite from time k-1 to time k. It represents the pseudo code frequency of the nth satellite signal received by GNSS at time k, and c represents the speed of light.

9. The positioning method based on the collaborative fusion of factor graph and filtering according to claim 1, characterized in that: The expression of the carrier frequency of the satellite at the current moment in step 5 is: in, represents the carrier frequency of the received nth satellite signal at time k, Indicates the carrier frequency when the nth satellite signal is transmitted, and The velocity vectors of GNSS and the nth satellite at the kth time, respectively, represents the clock drift of the nth satellite at the kth time, represents the clock drift of GNSS at the kth moment, I n represents the unit vector of GNSS in the line of sight of the nth satellite, and c represents the speed of light.

10. The positioning method based on the collaborative fusion of factor graph and filtering according to claim 1, characterized in that: The expression of the pseudo code frequency of the satellite at the current moment in step 5 is: in, represents the pseudo code frequency of the nth satellite signal received at time k, Indicates the pseudo code frequency when the nth satellite signal is transmitted, and The velocity vectors of GNSS and the nth satellite at the kth time, respectively, represents the clock drift of the nth satellite at the kth time, represents the clock drift of GNSS at the kth moment, I n represents the unit vector of GNSS in the line of sight of the nth satellite, and c represents the speed of light.

Images