A Kalman Filter and Least Squares Adaptive Switching Positioning Method for GNSS Single-Point Positioning

Abstract

This invention relates to Global Navigation Satellite Systems (GNSS), specifically providing a Kalman filter and least squares adaptive switching positioning method for GNSS single-point positioning. It constructs a dual monitoring mechanism by real-time monitoring of the P-array change rate and P-array trace values; and dynamically determines a dual adaptive threshold by comprehensively considering the number of available observables and the estimated number of epochs in KF. When the threshold condition is exceeded, it actively exits KF and switches to LS, automatically returning to KF after the environment recovers. This invention achieves high continuity and high reliability positioning throughout, effectively avoiding frequent mode switching while ensuring positioning accuracy, thus improving the overall continuity and robustness of positioning. This invention is applicable to single-point positioning scenarios in complex environments such as urban canyons, tree-lined areas, and under viaducts.

Description

Technical Field

[0001] This invention relates to the field of Global Navigation Satellite System (GNSS) positioning technology, specifically to a Kalman filter and least squares adaptive switching positioning method for GNSS single-point positioning, which is applicable to single-point positioning scenarios in complex environments such as urban canyons, tree-shaded areas, and under viaducts. Background Technology

[0002] Kalman filtering (KF) has been widely used in GNSS dynamic positioning data processing due to its recursive nature, the fact that it does not require storing large amounts of historical observation data, and its ability to process observation results in real time. Under good signal conditions, KF can smooth the positioning results using the state recursion relationship between epochs, avoiding positioning jitter errors caused by the relatively independent solutions of each epoch, as seen in least squares algorithms. The estimation error covariance matrix P of KF quantitatively characterizes the uncertainty of the state estimation—the smaller the P matrix, the more reliable the estimation; a sharp increase or slow expansion of the P matrix to a "high variance steady state" indicates filter divergence.

[0003] When the receiver enters harsh environments such as urban canyons, forests, or under overpasses, the quality of the observations deteriorates sharply. In such cases, the KF filter gain adjustment and state prediction may become biased due to unreliable observation information, potentially leading to filter divergence. Filter divergence refers to the actual mean square error of the filter being much larger than the estimated value, and this difference increasing over time.

[0004] Existing technologies combine least squares (LS) with Kalman filtering (KF), but these methods have the following shortcomings:

[0005] 1) Existing technologies often use innovation (residual) sequences for hypothesis testing, but when the statistical characteristics of slow-varying errors or observation noise are unknown, innovation (residual) tests often fail and cannot detect the slow divergence of the filter in time.

[0006] 2) Most existing methods use qualitative judgment or residual test to determine whether the filter is diverging, and lack quantitative evaluation methods based on the filter's intrinsic state parameters (such as the covariance matrix P).

[0007] 3) Existing methods often rely on a single indicator (such as the size of the residual or the number of satellites) when determining whether to exit KF, without fully considering multi-dimensional information such as the number of KF estimated epochs and the changing trend of the P array, which can easily lead to misjudgment or switching lag.

[0008] 4) Existing methods typically rely solely on whether the number of satellites meets the minimum requirement when switching back from least squares to KF, without adequately verifying the quality of the least squares solution. This leads to premature switching back to KF when the observation quality is still unstable, causing filter oscillations.

[0009] The publication "CN120043518B" entitled "An Adaptive Integrated Navigation Method for GNSS Signal Quality" discloses a filtering calculation for integrated navigation, while the publication "CN122283774A" entitled "An Adaptive Robust Method for IGGⅢ Parameters Applicable to GNSS Positioning in Complex Environments" discloses a method for gross error detection and elimination in the filtering process. However, neither of these publications mentions the judgment of extended Kalman filter divergence or the operation after filter divergence. This invention relates to the judgment of Kalman filter divergence and the adaptive switching of the calculation model. Summary of the Invention

[0010] To address the shortcomings of the existing technologies, this invention provides an adaptive switching method for single-point localization using Kalman filtering and least squares. A dual monitoring mechanism is constructed by real-time monitoring of the rate of change and the P-array trace (Tr(P)). Furthermore, considering both the number of available observables and the estimated number of epochs in KF, a dual adaptive threshold is dynamically determined. When the threshold condition is exceeded, the system actively exits KF and switches to LS; after the environment recovers, it automatically returns to KF.

[0011] Invention principle:

[0012] This invention constructs a dual adaptive switching mechanism based on the covariance matrix (P-matrix) state and environmental factors, solving the problem of traditional Kalman filtering (KF) being prone to divergence and lacking self-recovery when there are sudden changes in observations or model mismatch. It achieves seamless complementarity between "dynamic tracking (KF)" and "independent robust solution (LS)".

[0013] The dual monitoring indicators of this invention:

[0014] The rate of change of the P-array reflects the dynamic stability of the filter's convergence state. A sharp increase in the rate of change indicates model mismatch or abnormal observations leading to uncontrolled covariance.

[0015] P-trace: Reflects the total magnitude of estimation uncertainty. An excessively large trace indicates a collapse in confidence, requiring immediate stop-loss.

[0016] A single indicator is prone to misjudgment; switching only occurs when both indicators are triggered simultaneously, significantly reducing the false switching rate.

[0017] The dynamic threshold strategy of this invention:

[0018] Instead of a fixed threshold, the number of available observations (related to satellite geometry intensity GDOP) and the number of KF epochs (distinguishing between cold start and steady state) are introduced as adjustment factors.

[0019] The threshold for jitter reduction is relaxed during cold starts or when there are few satellites, while the threshold is tightened to maintain accuracy during steady-state operations or when there are many satellites, thus achieving environmental adaptability.

[0020] The bidirectional automatic switching mechanism of this invention:

[0021] KF→LS: When the monitoring exceeds the standard, actively cut off the recursive dependency and use the "memoryless, pure current observation" characteristics of LS to reset the solution and block the accumulation of errors.

[0022] LS→KF: After environmental recovery (P-array fallback), it automatically reverts to KF, using historical data to smooth noise and restore high-precision dynamic tracking capabilities.

[0023] Therefore, this method solves the KF divergence problem: traditional KF will incorrectly shrink the covariance P once the initial value deviation is large or encounters gross errors, resulting in "stubborn" tracking errors; this method achieves online fault isolation and self-repair by forcibly switching LS.

[0024] Balancing real-time performance and robustness: KF is used to ensure smooth trajectory and dynamic response in normal environments, while LS is used to ensure uninterrupted solution and no cumulative drift in harsh environments (such as urban canyons with multiple paths and signal obstruction).

[0025] To achieve the objectives of this invention, the technical solution is as follows:

[0026] A Kalman filter and least squares adaptive switching positioning method for GNSS single-point positioning, the method comprising:

[0027] Step S1: System Initialization:

[0028] Set the initial state vector of the Kalman filter. and the initial estimation error covariance matrix ,

[0029] Based on the approximate positioning accuracy of the receiver, set the KF estimation epoch counter k=0;

[0030] Step S2: Kalman filter estimation:

[0031] At the current epoch k (k≥1), acquire GNSS satellite observation data and navigation ephemeris data, perform data preprocessing including satellite cutoff elevation angle removal and gross error removal, perform Kalman filtering for time updates and measurement updates, and obtain the state estimate for the current epoch. and the estimated error covariance matrix ;

[0032] Step S3: Calculate the rate of change and trace of position P matrix:

[0033] Rate of change and trace: For the variances of the diagonals of the matrices corresponding to positions X, Y, Z in matrix P, the rate of change is the rate of change of the matrix before and after XYZ, and the trace is the sum of the variances of XYZ. When the rate of change and the trace are small, it indicates that no abrupt changes have occurred in the filtering.

[0034] Convergence criterion: When the rate of change of the trace approaches zero and the value stabilizes at a low level, it indicates that the estimation has converged;

[0035] Step S4: Determine the adaptive threshold:

[0036] Two adaptive thresholds are determined: the threshold for the rate of change of the P matrix. absolute threshold of trace Both constitute dual criteria;

[0037] After the Kalman filter starts estimation, it is in the initial convergence phase. The P matrix gradually converges from an initial large value to a steady-state small value. During this period, the rate of change and trace of the P matrix are naturally large.

[0038] When the number of available satellites is less than 10, the observation redundancy decreases significantly, the condition number of the normal matrix of the observation equation increases, and the P-matrix of the KF becomes more sensitive to observation noise, resulting in increased convergence values ​​and fluctuation amplitudes. Therefore, the number of available observations N at the current epoch is used. sat The estimated epoch number k of KF is used as the basis for the segmented raising of the two thresholds, avoiding frequent erroneous exits from KF estimation.

[0039] 1) Threshold for the rate of change of the P-matrix :

[0040] The rate of change threshold is used to determine whether the P-array has suddenly deteriorated. Sudden and rapid deterioration usually means that the innovation residual is too large or the filter diverges, indicating that a step interference has occurred in the dynamic environment. The basic threshold is preset and raised in segments according to the number of epochs and satellites.

[0041]

[0042] in, This serves as the basic threshold for the rate of change of the P matrix. For epoch number correction term, This is a correction term for the number of satellites;

[0043] 2) Trace absolute threshold :

[0044] The trace absolute threshold is used to determine whether the P-matrix is ​​in a cumulative "high-variance steady state," representing the sum of the variances of the total uncertainty of the state estimate. An abnormal increase in the trace indicates filter divergence or confidence collapse. A preset base threshold is used, and it is also raised in segments based on the number of epochs and satellites.

[0045]

[0046] in, The base threshold for position P trace. For epoch number correction term, This is a correction term for the number of satellites.

[0047] Step S5: Dual-condition trigger switching judgment:

[0048] In urban environments, short-term anomalies in a single observation may cause a sudden increase or decrease in the rate of change of the P-matrix, but the trace of the P-matrix may still be within the normal range. Therefore, this step only determines that the KF estimation is unreliable and triggers a switch to step S6 when both conditions are met simultaneously at the same epoch.

[0049] The switching is not triggered when the conditions are met, effectively avoiding erroneous switching caused by the instantaneous fluctuation of a single indicator.

[0050] Step S6: Exit KF estimation and switch to least squares estimation;

[0051] When the KF estimate is determined to be unreliable, stop the KF estimation for the current epoch and subsequent epochs, and set the KF running status flag. KF Set the value to 0 and use least squares estimation for positioning;

[0052] Step S7: Environmental restoration monitoring and return to KF;

[0053] Step S8: Output the positioning result.

[0054] At the moment the Kalman filter (KF) is started, the state covariance matrix is ​​initialized based on the receiver's current approximate positioning accuracy (such as the single-point positioning error range), and the filter iteration step counter is forced to zero, marking the start of the recursive estimation of the filtering process from "time zero".

[0055] k=0: The starting flag for the internal iteration count of the filtering algorithm, indicating that no "prediction-update" loop has been performed yet. Initialization must be completed before entering the recursive process of k=1,2,...

[0056] That is, it refers to constructing the initial state vector using the receiver's current coarse position solution (such as the least squares solution). The initial error covariance matrix P0; the worse the accuracy, the larger the diagonal elements (variance) of the initial error covariance matrix should be, indicating high uncertainty about the initial value, and the filter will rely more on subsequent observations for correction.

[0057] According to the present invention, a Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning is described below:

[0058] Step S1: Initial state vector Obtained through single-epoch least squares estimation, including three-dimensional position parameters. and receiver clock bias parameters ,Right now:

[0059] Initial estimation error covariance matrix Set as a diagonal matrix:

[0060] in, , , These are the position parameters, The initial variance of the receiver clock bias parameter.

[0061] Based on the approximate positioning accuracy setting of the receiver, the KF estimation epoch counter k=0 is set.

[0062] According to the present invention, a Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning is described below:

[0063] Step S2: The time update of the Kalman filter is performed as follows:

[0064] 1) State prediction:

[0065] in Let be the state transition matrix from epoch k-1 to epoch k. This is the state estimate for epoch k-1. For k-epoch state predictions, a uniform velocity model or a constant acceleration model is used to construct the state transition matrix for static or low-dynamic positioning scenarios.

[0066] 2) Prediction error covariance matrix:

[0067] in The process noise covariance matrix reflects the uncertainty of the system dynamics model. Let k-1 be the error covariance matrix. Let k be the prediction error covariance matrix.

[0068] The measurement updates for the Kalman filter are performed as follows:

[0069] 3) Kalman gain matrix:

[0070] in For the observation matrix, To observe the noise covariance matrix, the variance of each observation in the Kalman gain matrix is ​​determined based on the satellite elevation angle or signal-to-noise ratio.

[0071] That is, a dynamic model is constructed using satellite elevation angle or signal-to-noise ratio (SNR) to calculate the diagonal elements of R in real time, and then the Kalman gain is automatically adjusted through a formula to reduce the weight of low-quality observations. The adaptive strategy solves the problem that the traditional fixed R value cannot cope with dynamic environments (such as signal fluctuations caused by urban canyons or tree obstruction).

[0072] 4) State estimation update:

[0073] in This is the observation vector for the current epoch. This is the state estimate for epoch k-1. This is the predicted state value for epoch k.

[0074] 5) Update the estimated error covariance matrix:

[0075] Where I is the identity matrix.

[0076] According to the present invention, a Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning is described below:

[0077] Step S5: Dual-condition trigger switching judgment includes:

[0078] 1) Condition for the rate of change of position P matrix:

[0079]

[0080] That is, if the P-array changes too rapidly, it reflects a sudden and drastic deterioration in the environment;

[0081] 2) The trace of the position P matrix:

[0082]

[0083] This means that the absolute variance of matrix P is at an excessively high level, indicating that matrix P is in a state of slow divergence or "high variance steady state".

[0084]

[0085] and

[0086] If, and only if, both of the above inequalities are true, the current KF estimate is determined to be unreliable, and step S6 is immediately executed to switch to least squares estimation.

[0087] If any condition is not met, that is or If the KF estimate is still reliable, continue using KF and return to step S2 for the next epoch estimate.

[0088] According to the present invention, a Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning is described below:

[0089] Step S6: The least squares estimation is performed in the following manner:

[0090] 1) Constructing the GNSS pseudorange observation equation:

[0091]

[0092] Where ρ is an n×1 dimensional pseudorange observation vector, n is the number of available satellites, G is an n×4 dimensional design matrix, and x is a 4×1 dimensional vector of parameters to be estimated. v is the observation residual vector;

[0093] 2) The weighted least squares solution is:

[0094]

[0095]

[0096] Where W is an n×n dimensional observation weight matrix, and its diagonal elements W i Determined based on satellite elevation angle, ELE i This represents the elevation angle of the i-th satellite.

[0097] According to the present invention, a Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning is described below:

[0098] When switching to least squares estimation, the estimate of the last reliable moment of KF is used as the initial value of the least squares iteration to accelerate least squares convergence and improve the stability of the solution.

[0099] According to the present invention, a Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning is described below:

[0100] Step S7: Environmental Restoration Monitoring and Return to KF

[0101] During the localization process using least squares estimation, if the following recovery conditions are met simultaneously, the environment is determined to have recovered, KF estimation is restarted, and the process returns to step S2:

[0102] 1) Number of observables N sat >8,

[0103] 2) The RMS (root mean square) value of the residuals after least squares positioning verification over 10 consecutive epochs. V ≤5,

[0104] in:

[0105]

[0106] When both of the above conditions are met, the KF running status flag will be set. KF Set the value to 1, use the current LS estimate as the initial state vector of KF, reset the P matrix to the initial value P0, reset the counter k=0, and restart KF estimation.

[0107] According to the present invention, a Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning is described below:

[0108] Step S8: Output the positioning result:

[0109] According to the KF running status flag KF Output the location result for the current epoch:

[0110] 1) When Flag KF When =1, output the KF estimated position.

[0111] 2) When Flag KF When =0, output the LS estimated position.

[0112] Beneficial effects of this invention:

[0113] 1) The criteria are more direct and comprehensive. The P-array change rate and P-array trace are used as dual criteria to directly quantify the uncertainty of KF estimation, which is more sensitive and comprehensive than indirect indicators such as innovation or residuals.

[0114] 2) To compensate for the inherent blind spot of the rate of change criterion, the trace absolute threshold criterion is introduced to effectively identify the "high variance steady state" situation in which the P matrix has slowly expanded but the rate of change is very small, thus avoiding the continuous output of unreliable positioning by KF in this state.

[0115] 3) The threshold has working condition adaptability and adopts a piecewise constant model. The base value corresponds to the normal working condition after the filter converges. In the initial stage of filtering, the P array naturally fluctuates greatly, and the threshold is automatically raised. When the number of satellites is insufficient, the P array is more sensitive to noise, and the threshold is automatically raised.

[0116] 4) Effectively avoids erroneous switching in the early stage of filtering. By actively raising the threshold in the early stage of filtering, sufficient tolerance space is provided for the natural convergence of the P matrix, which solves the problem in the existing technology that the filter is mistakenly judged as diverging and erroneously exits KF before it has converged.

[0117] 5) Two-way adaptive connection: realizes a closed-loop mechanism of active exit from KF→LS and automatic return from LS→KF, ensures the adaptation of the optimal estimation method in all scenarios, and improves the overall reliability of the positioning system. Attached Figure Description

[0118] Figure 1 This is a flowchart of the testing method of the present invention.

[0119] Figure 2 This is a sequence diagram showing the total number of satellites throughout the entire process.

[0120] Figure 3 This is a sequence diagram of the rate of change of the position P matrix throughout the entire process.

[0121] Figure 4 This is a trace sequence diagram of the entire position P matrix.

[0122] Figure 5 This is a sequence diagram of the three-dimensional positioning error throughout the entire process. Detailed Implementation

[0123] The following is combined Figure 1 The present invention will be described in further detail below. In this embodiment, a vehicle-mounted GNSS receiver is used to collect ranging signals. The receiver sampling frequency is 1Hz. The test route is from an open area to a densely wooded area, and then from the densely wooded area to an open scene, further demonstrating the working process of the Kalman filter and least squares adaptive switching method.

[0124] The entire trip is divided into three stages:

[0125] Phase 1 (Era 1–288): Open roads, unobstructed skies;

[0126] Phase Two (288–742 epochs): Entering a densely wooded road, the thick canopy of trees severely obstructed satellite signals;

[0127] Phase Three (742–1000 AD): Leaving the shade and returning to the open road.

[0128] Figures 2-4 It is a sequence diagram of the number of satellites involved in the calculation, the rate of change of position P array throughout the entire test route, and the trace of position P array, which are calculated using only Kalman filtering. The time periods of the two open scenes and the shaded scene have been marked. Figure 5 The figures show the positioning error curves using only Kalman filtering and the 3D positioning error curves using the Kalman filtering and least squares switching proposed in this invention. (See attached figures.) Figure 5 As shown, the positioning error remains consistent between using the switching method of this invention and using only KF from epoch 1 to 364 (the red and blue curves in the figure coincide), and the positioning error begins to change from epoch 364 onwards.

[0129] Based on the three stages corresponding to the attached diagram, the number of satellites, the rate of change of the P array, and the trace of the P array are analyzed in the three stages respectively.

[0130] Phase 1: KF stable convergence in open environment:

[0131] When epoch = 0, step S1, system initialization, is executed. The initial state vector is obtained by performing least-squares calculations using pseudorange observations from the 22 satellites in the first epoch. Meanwhile, the initial error covariance matrix is ​​set to P0=diag(25,25,25,5), and the KF epoch count is set to k=0.

[0132] When k=1, proceed to step S2 for Kalman filter estimation. At this stage, the number of satellites N can be used. sat The number of satellites stabilized at 20–25, with a cutoff elevation angle of 10°, and no significant gross errors were observed. Time and measurement updates were performed epoch-by-epoch, and the state estimation gradually converged.

[0133] Calculation of the rate of change of the P-array:

[0134] 1) Calculate the rate of change of the position P matrix:

[0135] Extract the estimation error covariance matrix of the current epoch k. The diagonal elements corresponding to the three-dimensional position parameters X, Y, Z , , and the diagonal elements corresponding to the previous epoch k−1 , , .

[0136] By combining the covariance matrices at the three locations, calculate the rate of change of the P matrix elements corresponding to the three-dimensional locations. :

[0137] Where T is the epoch interval.

[0138] 2) Calculate the trace of the position P matrix:

[0139] Extract the 3×3 submatrix corresponding to the three-dimensional position in the P matrix at the current k-epoch. ;

[0140]

[0141] Calculate its trace Tr k As an absolute measure of the uncertainty of the location estimated by KF:

[0142] .

[0143] Taking k=20 as an example, extract the diagonal elements of the three-dimensional position of matrix P: , , Corresponding to P array =1.18. Previous epoch , , The corresponding values ​​are 0.23, 0.78, and 0.11, respectively.

[0144]

[0145] Therefore, rate of change At this time N sat =22, k=20. Based on the adaptive threshold rule in step S4, set the empirical baseline threshold. =30, then the threshold for the rate of change is:

[0146]

[0147] The aforementioned base threshold T for the rate of change of the P matrix base The preferred value is 30, and the P-array base threshold Tr base The preferred value is 50. This value is not a limitation of the present invention, but a basic parameter preset based on engineering experience, taking into account the statistical characteristics of positioning errors of typical vehicle-mounted GNSS receivers in static and dynamic tests in open environments.

[0148] 1. When k < 100, after the Kalman filter starts, the trace of the initial state covariance matrix is ​​75 (P0 = diag(25,25,25,5)). The trace threshold is set to 50, and the filter will violate the trace threshold in the first epoch. Actively raising the trace threshold by 30 avoids the problem of the filter being misjudged as diverging and forcibly exiting before convergence. Similarly, increasing the rate of change correction term by 5 is also to accommodate the large rate of change generated when the P matrix decreases rapidly in the initial stage.

[0149] 2. When the number of satellites is less than 10, the geometry of the GNSS observation equations deteriorates, and observation noise is amplified, causing the fluctuation amplitude and steady-state value of the P array to naturally increase. By raising the rate of change threshold by 5 and the trace threshold by 20, erroneous switching is prevented due to the natural increase of the P array caused by poor satellite geometry, thus ensuring the continuity of system operation in weak environments.

[0150] Set an experience-based threshold =50, then the corresponding trace threshold is:

[0151]

[0152] Obviously, and Throughout the entire phase, KF operated stably, and the Flag... KF Keep it at 1, and output the KF smooth positioning result.

[0153] Secondly, entering Phase Two: Entering a dense forest, environmental degradation triggers a switch to LS:

[0154] As vehicles enter densely wooded roads (from epoch 288 onwards), the dense canopy causes the number of available satellites to gradually decrease to 5-9, and the multipath effect significantly increases pseudorange observation noise.

[0155] Taking k=319 as an example, N sat =9<10, and k=319>100, calculate the adaptive threshold according to step S4, and set the empirical baseline threshold. =30, then the threshold for the rate of change is:

[0156]

[0157] Set an experience-based threshold =50, then the corresponding trace threshold is:

[0158] .

[0159] Observing the data, the trace of the P-array is gradually increasing, and the 3D positioning error is also gradually increasing, but at this time... , Neither of the two conditions was met, so no switching was triggered. This indicates that a small number of epochs of gradual increase in the P matrix will not lead to switching.

[0160] As the vehicle ventured deeper into the wooded area, observation conditions deteriorated further. At k=364, pseudorange multipath error caused KF innovation anomalies, and the P array began to expand rapidly. At this point, k=364... , If both conditions are met simultaneously, the switching decision is triggered (step S5).

[0161] Therefore, according to the strategy adopted in this invention, at the end of epoch k=364, the system sets the KF running status flag (Flag). KF Set the value to 0 and stop KF estimation in subsequent epochs, then switch to weighted least squares estimation (step S6). At the moment of switching, use the KF estimate at k=364 as the initial iteration value for least squares to improve stability. From k=364 onwards, the output localization results are provided by LS.

[0162] In the LS stage, although the number of satellites is still small (5-8), the least squares solution is solved independently for each epoch, and it will not accumulate errors and diverge like the KF solution.

[0163] In the latter half of Phase Two: Environmental Restoration Monitoring and Return to KF:

[0164] LS continued operating, with the vehicle continuing to drive through the shade, continuously monitoring the RMS of the post-test residuals and the number of satellites involved in the calculation. During epochs 364–743, N... satMaintaining 5-8 particles does not meet the conditions for switching to KF. At epoch 743, the number recovers to over 9 particles, and after multiple iterative calculations, the RMS of the posterior residual of LS is... V It gradually descended to below 5 meters.

[0165] At epoch = 795, the RMS of the post-hoc residuals of the LS test for 10 consecutive epochs. V All are no more than 5 meters, and N sat =9>8, the recovery conditions are met. Execute the return to KF operation: Set the Flag. KF Reset to 1, and take the LS estimate at the current epoch = 795 as the new initial state vector of KF. The estimated error covariance matrix is ​​reset to the initial diagonal matrix P0=diag(25,25,25,5). Starting from epoch 795, the Kalman filter estimation is restarted (returning to step S2), and the open environment localization in stage three is entered.

[0166] Phase Three: KF Re-converges in the Open Space:

[0167] After epoch k=795, the satellite conditions are good (N sat (≥15), KF begins to converge again. In the initial few epochs, the location results are smooth but slightly fluctuating due to the large impact of resetting P0; as filtering progresses, the P matrix quickly converges to a stable small value. During this period, the adaptive threshold remains low due to the recounting of k and sufficient satellite count, but because the environment is stable, and The values ​​are all very small and will not trigger a switch. KF The value remains at 1, and the system outputs the estimated KF position stably until the end of the journey.

[0168] Throughout the entire journey, the system fully utilizes the smoothness and high accuracy of KF (Kinetic Field Measurement) in open areas. In shaded areas, it promptly switches to LS (Low-Side Field Measurement), avoiding positioning jumps and unreliable results caused by KF divergence. Upon returning to an open environment, it automatically recovers KF, achieving high continuity and high reliability in positioning throughout the entire process. This embodiment verifies that the method of the present invention effectively avoids frequent mode switching while ensuring positioning accuracy, improving the overall continuity and robustness of positioning.

Claims

1. A Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning, characterized in that: The method includes: Step S1: System Initialization: Set the initial state vector of the Kalman filter. and the initial estimated error covariance matrix , Based on the approximate positioning accuracy setting of the receiver, set the KF estimated epoch counter k=0; Step S2: Kalman filter estimation: At the current epoch k, where k≥1, acquire GNSS satellite observation data and navigation ephemeris data, perform data preprocessing including satellite cutoff elevation angle removal and gross error removal, perform Kalman filtering for time updates and measurement updates, and obtain the state estimate for the current epoch. and the estimated error covariance matrix ; Step S3: Calculate the rate of change and trace of position P matrix: Rate of change and trace: For the variances of the diagonals of the matrices corresponding to positions X, Y, Z in matrix P, the rate of change is the rate of change of the matrix before and after XYZ, and the trace is the sum of the variances of XYZ. When the rate of change and the trace are small, it indicates that no abrupt changes have occurred in the filtering. Convergence criterion: When the rate of change of the trace approaches zero and the value stabilizes at a low level, it indicates that the estimation has converged; Step S4: Determine the adaptive threshold: Two adaptive thresholds are determined: the threshold for the rate of change of the P matrix. absolute threshold of trace Both constitute dual criteria; After the Kalman filter starts estimating, it is in the initial convergence phase. The P matrix gradually converges from an initial large value to a steady-state small value. During this period, the rate of change and trace of the P matrix are naturally large. When the number of available satellites is less than 10, the observation redundancy decreases significantly, the condition number of the normal matrix of the observation equation increases, and the P-matrix of the KF becomes more sensitive to observation noise, resulting in increased convergence values ​​and fluctuation amplitudes. Therefore, the number of available observations N at the current epoch is used. sat The KF estimation epoch number k is used as the basis for the segmented raising of the two thresholds, avoiding frequent erroneous exits from the KF estimation. 1) Threshold for the rate of change of the P-matrix : The rate of change threshold is used to determine whether the P-array has suddenly deteriorated. Sudden and rapid deterioration usually means that the innovation residual is too large or the filter diverges, indicating that a step interference has occurred in the dynamic environment. The basic threshold is preset and raised in segments according to the number of epochs and satellites. in, This is the basic threshold for the rate of change of the P matrix. For epoch number correction term, For satellite number correction terms: 2) Trace absolute threshold : The trace absolute threshold is used to determine whether the P-matrix is ​​in a cumulative "high variance steady state," representing the sum of the variances of the total uncertainty of the state estimate. An abnormal increase in the trace indicates filter divergence or confidence collapse. A preset base threshold is used, and it is also raised in segments based on the number of epochs and satellites. in, The base threshold for position P-array. For epoch number correction term, This is a correction term for the number of satellites; Step S5: Dual-condition trigger switching judgment: In urban environments, short-term anomalies in a single observation may cause a sudden increase or decrease in the rate of change of the P-matrix, but the trace of the P-matrix may still be within the normal range. Therefore, this step only determines that the KF estimation is unreliable and triggers a switch to step S6 when both conditions are met simultaneously at the same epoch. The switching is not triggered when the conditions are met, effectively avoiding erroneous switching caused by the instantaneous fluctuation of a single indicator; Step S6: Exit KF estimation and switch to least squares estimation; When the KF estimate is determined to be unreliable, stop the KF estimation for the current epoch and subsequent epochs, and set the KF running status flag. KF Set the value to 0 and use least squares estimation for positioning; Step S7: Environmental restoration monitoring and return to KF; Step S8: Output the positioning result.

2. The Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning as described in claim 1, characterized in that: Step S1: Initial state vector Obtained through single-epoch least squares estimation, including three-dimensional position parameters. and receiver clock bias parameters ,Right now: , Initial estimation error covariance matrix Set as a diagonal matrix: in, , , These are the position parameters, The initial variance of the receiver clock bias parameter. Based on the approximate positioning accuracy setting of the receiver, the KF estimation epoch counter k=0 is set.

3. The Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning as described in claim 1, characterized in that: Step S2: The time update of the Kalman filter is performed as follows: 1) State prediction: in For static or low-dynamic positioning scenarios, the state transition matrix is ​​constructed using a uniform velocity model or a constant acceleration model, which is used to construct the state transition matrix from epoch k-1 to epoch k. This is the state estimate for epoch k-1. This is the predicted state value for epoch k. 2) Prediction error covariance matrix: in Let be the noise covariance matrix of the k-1 epoch process, reflecting the uncertainty of the system dynamics model. Let k-1 be the error covariance matrix. Let k be the prediction error covariance matrix. The measurement updates for the Kalman filter are performed in the following manner: 3) Kalman gain matrix: in For the k-epoch observation matrix, Given the k-epoch observation noise covariance matrix, determine the variance of each observation in the Kalman gain matrix based on the satellite elevation angle or signal-to-noise ratio. The Kalman gain matrix is ​​the k-epoch Kalman gain matrix. 4) State estimation update: in Let k be the observation vector. This is the state estimate for epoch k; 5) Update the estimated error covariance matrix: Where I is the identity matrix. Let be the k-epoch error covariance matrix.

4. The Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning as described in any one of claims 1-3, characterized in that: Step S5: Dual-condition trigger switching judgment includes: 1) Condition for the rate of change of position P matrix: That is, if the P-array changes too rapidly, it reflects a sudden and drastic deterioration in the environment; 2) The trace of the position P matrix: This means that the absolute variance of matrix P is at an excessively high level, indicating that matrix P has entered a slow divergence or a "high variance steady state". and If and only if both of the above inequalities are true, the current KF estimate is determined to be unreliable, and step S6 is immediately executed to switch to least squares estimation. If any condition is not met, that is or If the KF estimate is still reliable, continue using KF and return to step S2 for the next epoch estimate.

5. The Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning as described in any one of claims 1-3, characterized in that: Step S6: The least squares estimation is performed in the following manner: 1) Constructing the GNSS pseudorange observation equation: Where ρ is an n×1 dimensional pseudorange observation vector, n is the number of available satellites, G is an n×4 dimensional design matrix, and x is a 4×1 dimensional vector of parameters to be estimated. v is the observation residual vector. 2) The weighted least squares solution is: Where W is an n×n dimensional observation weight matrix, and its diagonal elements W i Determined based on satellite elevation angle, ELE i Represents the elevation angle of the i-th satellite. This is the least squares estimate.

6. The Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning as described in claim 5, characterized in that: When switching to least squares estimation, the estimate of the last reliable moment of KF is used as the initial value of the least squares iteration to accelerate least squares convergence and improve the stability of the solution.

7. The Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning as described in any one of claims 1-3, characterized in that: Step S7: Environmental restoration monitoring and return to KF: During the localization process using least squares estimation, if the following recovery conditions are met simultaneously, the environment is determined to have recovered, KF estimation is restarted, and the process returns to step S2: 1) Number of observables N sat >8, 2) RMS value of the least squares positioning residuals after 10 consecutive epochs V ≤5, in: Where V is the observation residual vector, W is the n×n dimensional observation weight matrix, and n is the number of available satellites. When both of the above conditions are met, the KF operation status flag will be set. KF Set the value to 1, use the current LS estimate as the initial state vector of KF, reset the P matrix to the initial value P0, reset the counter k=0, and restart KF estimation.

8. The Kalman filtering and least squares adaptive switching positioning method for GNSS single-point positioning as described in any one of claims 1-3, characterized in that: Step S8: Output the positioning result: According to the KF running status flag KF Output the location result for the current epoch: 1) When Flag KF When =1, output the KF estimated position. 2) When Flag KF When =0, output the LS estimated position.

Images