Doppler effect elimination and fusion positioning method for high-speed train

Abstract

The invention provides a Doppler effect elimination and fusion positioning method for a high-speed train, and the method comprises the steps: deducing a mathematical expression of a distance measurement error caused by a Doppler effect based on a bidirectional time of flight (ToF) method; secondly, using UWB / IMU time sequence prior to eliminate distance measurement error deviation, and analyzing residual error variance; and finally, constructing a self-adaptive UWB / IMU fusion positioning framework, deducing a UWB measurement equation random item closed expression, and realizing Doppler effect elimination and fusion model parameter self-adaptive adjustment. According to the method, the precision of UWB / IMU fusion positioning is remarkably improved, and the influence of the Doppler effect on the bidirectional ToF ranging and fusion positioning model is overcome.

Description

TECHNICAL FIELD

[0001] The present application relates to wireless communication and network, in particular to a Doppler effect elimination and fusion positioning technology for high-speed trains. BACKGROUND

[0002] With the modernization development of urban rail transit, high-precision positioning is of great significance to location-based services (LBS) and functional safety. According to the standard of “Technical Requirements for Urban Rail Transit Communication-based Train Control System”, the positioning error of the train in normal operation is ≤6.25 m, and the positioning error of the platform without installed shield doors is ≤0.5 m. A single positioning method cannot guarantee the specified positioning accuracy and reliability, and multi-sensor fusion positioning is the future development trend of positioning technology. In the existing train control system, an inertial measurement unit (IMU) has been deployed to provide real-time three-dimensional acceleration measurement, and the target moving speed and travel distance are solved by accumulation, but with the increase of travel time, there is a problem of error accumulation. A part of research fuses ultra-wideband (UWB) technology with IMU to overcome the problem of error accumulation and apply it to the field of urban rail transit.

[0003] Currently, the main problems of UWB / IMU fusion positioning applied in the rail transit scene are: 1) similar to other wireless signals, UWB signals are easily affected by Doppler effect in high-speed moving environment, but the current technology does not consider the Doppler error introduced in the UWB two-way time of flight (ToF) ranging; 2) the existing technology cannot utilize the UWB / IMU time series prior information to eliminate the Doppler effect; 3) in the existing Kalman filtering framework, the UWB observation error term is an empirical value, but in actual application, due to the influence of Doppler effect, its error fluctuates constantly, and the fixed inaccurate empirical value has a bad influence on the fusion model, leading to performance degradation. SUMMARY

[0004] In view of the deficiencies of the prior art, the Doppler effect elimination and fusion positioning method for high-speed trains is proposed on the basis of the existing UWB / IMU fusion positioning model, which is suitable for the UWB / IMU fusion positioning framework and can be applied to high-speed mobile scenarios. First, the influence of the Doppler effect on time measurement under the two-way ToF ranging framework is derived, and a mathematical expression of the ranging error introduced by the Doppler effect is constructed. Second, the statistical distribution characteristics of the ranging error are analyzed by using the UWB / IMU time series prior information, the Doppler effect elimination and the residual random error variance estimation are realized. Finally, the closed-form expression of the UWB observation error term is constructed, the UWB / IMU adaptive fusion positioning model is formed, the adaptivity of the predicted observation weight distribution is adjusted, and the influence of the Doppler effect on the fusion positioning model is minimized.

[0005] Technical scheme A Doppler effect elimination and fusion positioning method for high-speed trains, comprising the following steps: Step (1), UWB ranging based on two-way ToF ranging algorithm, construct the time-of-flight measurement mathematical model under the Doppler effect; and calculate the ranging error caused by the Doppler effect.

[0006] Step (2), for the UWB / IMU fusion positioning time series, extract the target state prior information; derive the statistical characteristics of the UWB ranging error introduced by the Doppler effect; eliminate the interference of the Doppler effect on the UWB ranging.

[0007] Step (3), for the UWB / IMU fusion positioning framework after eliminating the Doppler interference bias, derive the closed-form expression of the random error term of the UWB measurement equation; construct the adaptive UWB / IMU fusion positioning model, and complete the high-precision fusion positioning.

[0008] Beneficial effects 1. The Doppler effect in the two-way ToF ranging is modeled for the first time, and the mathematical expression of the ranging error introduced by the Doppler effect is derived, which provides support for Doppler error elimination.

[0009] 2. The Doppler error elimination is effectively combined with the UWB / IMU fusion positioning, the prior knowledge at the last time in the fusion positioning is utilized to realize the elimination of the Doppler error bias and improve the UWB ranging accuracy.

[0010] 3. The statistical characteristics of the random error after Doppler error elimination are analyzed, the closed-form expression of the UWB / IMU fusion positioning is formed, the adaptive adjustment of the fusion framework at different positions and different times is realized, and the UWB / IMU fusion positioning accuracy is improved. BRIEF DESCRIPTION OF DRAWINGS

[0011] FIG. 1Embodiments of the present application Doppler effect elimination and fusion positioning model flow chart; FIG. 2 Embodiments of the present application bidirectional ToF positioning principle diagram; FIG. 3 Embodiments of the present application bidirectional ToF positioning principle diagram under Doppler interference; FIG. 4 Embodiments of the present application Doppler effect elimination method performance comparison chart; FIG. 5 Embodiments of the present application UWB / IMU fusion positioning model performance comparison chart. DETAILED DESCRIPTION

[0012] The technical solutions provided by the present application will be further described below in conjunction with specific embodiments and their accompanying drawings. The advantages and features of the present application will become clearer in conjunction with the following description.

[0013] The Doppler effect elimination and fusion positioning method for high-speed trains proposed by the present application is suitable for a UWB / IMU fusion positioning framework and can be applied to high-speed mobile scenarios.

[0014] The present application first considers the error of the UWB ranging method (i.e., bidirectional ToF positioning), constructs a mathematical expression of the ranging error introduced by the Doppler effect, then eliminates the influence of the Doppler effect on ranging by using UWB / IMU time series prior information, and constructs a UWB observation error mathematical expression; finally, a UWB / IMU adaptive fusion positioning model is formed to realize optimal merging of prediction and observation data and further reduce the influence of the Doppler effect on the fusion model. The specific implementation process is as shown in FIG. 1 .

[0015] The implementation process of the present application will be described in detail below, and the specific steps are as follows: Step 1. Time of flight measurement mathematical model under Doppler effect modeling, and calculating the influence of Doppler effect on ranging error Step (11), in the case of no measurement error, as shown in FIG. 2 , the relative time between the base station tag is assumed to be , , , , according to the bidirectional ToF principle, the time of flight can be calculated as: . Assuming the electromagnetic wave transmission rate is , the real distance between the base station and the tag is calculated as: . Step (12), in actual measurement, considering the measurement error and Doppler effect Introduced error, build ranging model: . Where the ranging error obeys Gaussian distribution with mean 0 and variance , that is The error introduced by Doppler effect is related to the state of the moving target.

[0016] The specific steps of time-of-flight measurement under Doppler effect: Step (13), when the tag moves, the two-way ToF ranging principle is as shown in FIG. 3 , and its ranging result is affected by the speed component in the base station-tag transmission direction. Assuming that the coordinates of the known reference base station are , the position vector and moving speed vector of the tag are , , and the speed vector of the tag The speed component of the tag in the direction of the first base station-tag transmission signal can be expressed as: . When the tag moves close to the base station direction, ; when the tag moves away from the base station direction, .

[0017] Step (14), when the tag moves, assuming that the distance between the base station and the tag when the base station starts ranging is the true distance, and the corresponding error-free transmission time is . According to the influence of Doppler frequency shift on one ranging, the ToF measurement in the first communication can be derived as . Where represents the speed component of the tag speed in the base station-tag direction.

[0018] The specific steps of the ranging error caused by Doppler effect: Step (15), derive the ToF measurement in the second and third communications Based on two-way ToF, assuming that the UWB node processing time is , the ToF measurement in the second and third communications is derived as: , . Step (16), derive the two-way ToF measurement value under the influence of Doppler effect Step (161), according to the two-way ToF ranging principle diagram​FIG. 3 ), the relationship between the ToF measurement and the relative time of the UWB node in step (11) can be expressed as: Step (162), substituting the expression in step (161) into step (11), the bidirectional ToF measurement can be solved: . Step (163), substituting the ToF measurements of the three communications in steps (14), (15) into the expression in step (161), the bidirectional ToF measurement result under the influence of the Doppler effect can be derived: . Step (17), constructing the mathematical expression of the ranging error introduced by the Doppler effect Step (171), according to the measured ToF error and the electromagnetic wave transmission rate , the mathematical expression of the ranging error can be derived: . Step (172), dividing the numerator and denominator of the above formula by , we get . Step (173), since , in the above formula can be approximated to 0, and the mathematical expression of the ranging error introduced by the Doppler effect can be simplified as: . Through the above process, the mathematical expression of the influence of the Doppler effect on bidirectional ToF ranging is constructed, which provides theoretical support for the elimination of the Doppler effect in subsequent step 2.

[0019] Step 2. Eliminate the interference of the Doppler effect on UWB ranging, including the following steps Specific steps of extracting target state prior information from UWB / IMU time series: Step (21), assuming that the state vector of the tag in the UWB / IMU time series is composed of three-dimensional coordinates , velocity , and acceleration , the state vector at the moment is expressed as: . Step (22), in the UWB / IMU fusion time series, the state vector at the moment predicted according to the state at the moment is extracted As a priori (see step (34)), the moving target obeys a Gaussian distribution . Where the predicted state vector is also composed of the position coordinates, velocity, acceleration of the moving target, denoted as . The mean value is the real state vector of the moving target at the th moment , and the variance is the prediction error covariance matrix, denoted as: , , , The error covariance matrices of the predicted position, velocity, and acceleration are respectively The full 0 matrix is denoted as

[0020] The specific steps for deriving the ranging error statistical characteristics are as follows: Step (23), the UWB tag communicates with the UWB base station in turn, and the distance information from the UWB tag to the UWB base station is solved by using the two-way ToF in turn. The position information of the tag is solved by using the ToA positioning method by comprehensively analyzing multiple ranging data. Since there is a ranging error in the two-way ToF measurement, it will cause a ToA positioning error. The error vector in ToA positioning is derived Step (231), considering the ranging error in step (12), the ToA positioning equation is constructed as: , Wherein, denotes the positioning result under the influence of interference, the observation matrix and the measurement vector are respectively denoted as: , . Step (232), according to the above measurement vector expression and the ranging error model in step (12), it can be obtained that the error term introduced by the measurement vector is . Step (233), substituting the above formula into the measurement vector expression, the error vector in the measurement vector can be derived, and the expression is: . Step (234), considering that the distance between the base station and the tag is much larger than the ranging error, i.e. , the error term introduced in step (232) can be approximated as ​​Substitute the ranging error model in step (12), it is simplified as: . Step (24), derive the statistical properties of error terms Step (241), according to the UWB / IMU time series prediction, the predicted position vector at the moment , .

[0021] Step (242), according to the maximum likelihood probability, the mean and variance of the error term are derived respectively as: . The specific steps of Doppler interference bias elimination are as follows: Step (25), according to the mean of the bias error term, the measurement error vector is estimated as: . Step (26), correct the measurement vector , and solve the tag position coordinates based on the least square method as : . After bias elimination, the corrected measurement vector exists random error with mean 0 and error covariance .

[0022] Through the above process, the statistical distribution characteristics of the error vector are derived, the influence of Doppler effect on UWB positioning is eliminated, and the influence of the ranging error introduced by Doppler effect on the fusion model in step 3 is reduced.

[0023] Step 3. Closed-form fusion positioning model The specific steps of closed-form expression of the random term of UWB measurement equation are as follows: Step (31), observation model construction Step (311), at the moment, assuming that the corrected measurement vector in step 2 is the observation vector of the fusion model, the relationship between it and the true state vector is expressed as: , wherein, Represents the state-measurement matrix. The error term in the observation equation is represented by a vector with a mean of 0 and a covariance matrix of... Gaussian distribution .

[0024] Step (312): In the fusion localization model, the state-measurement matrix is ​​the observation vector. Regarding the true state vector The Jacobian matrix is ​​expressed as: . Step (32), observation error term Covariance closed expression Step (321), assuming that the error vector is processed through step 2, The deviation has been eliminated, and the remaining error vector It follows a Gaussian distribution with a mean of 0 and the following variance: . Step (322), taking into account the processing time in practical applications Uncertainty, introduce scaling factor Set error terms The variance is expressed in closed form as: . The parameters can be adjusted This makes the observation model more closely resemble the actual scenario.

[0025] Specific steps of the UWB / IMU adaptive fusion localization model: In step (33), the IMU can measure the three-dimensional acceleration of the moving target in real time, which is represented as a vector. In fusion localization, the change in acceleration of the IMU is set as the model input, expressed as: . Step (34): Predict the target state at the current time based on the target state at the previous time step. Step (341), based on the basic characteristics of kinematics and target status at any time ,predict Target status at any time: . Where the state transition matrix Represented as: . Step (342): Since the acceleration vector input by the IMU has an error, its covariance is set to... Considering Time target state There is an error, whose covariance matrix is represented as , the corresponding predicted target state Error covariance is: , Wherein, according to the basic principle of kinematics error conversion matrix is derived as: . Step (343), the above predicted state vector And error covariance , into step 2, can realize the elimination of Doppler effect.

[0026] Step (35), in the fusion model, according to the estimated prediction, observation error covariance (P , ) Calculation of Kalman gain, expressed as . Step (35), realize UWB / IMU fusion positioning Step (351), according to the solution of Kalman gain, the prediction and observation vector, expressed as: . The error covariance of the merged is represented as . Step (352), with the movement of the label, using the current output target state And error covariance matrix , for Time state prediction.

[0027] Through the above process, the error covariance of the observation vector is constructed, the error term of the observation equation at different positions is accurately estimated, and the UWB / IMU adaptive fusion positioning can be realized.

[0028] Build simulation environment, compare the method of the invention and the existing method, the results are as follows: (1) Doppler effect elimination performance analysis As FIG. 4The results are compared with typical ranging interference cancellation algorithms. The comparison methods include: SDP (Semi-Definite Programming) [1], RSDP (Robust Semi-Definite Programming) [2], MRC (Maximal Ratio Combining) [3] and LAS-SDT-v (Localization and Synchronization-Sequential Doppler Shift and TOA Measurements-Velocity, based on velocity cooperation combined with Doppler frequency shift and ToA measurement) [4]. Among them, SDP considers UWB ranging error and uses convex optimization to search for the optimal positioning result, while RSDP improves on this basis by adding a robustness factor. MRC considers the influence of mixed interference and uses maximum ratio combining to obtain the optimal position estimate of UWB tags. LAS-SDT-v is based on velocity cooperation and combines Doppler frequency shift and ToA measurement for positioning and time synchronization.

[0029] The root mean square error (RMSE) is used as the evaluation metric.

[0030] Depend on FIG. 4 It is evident that the positioning performance of the four aforementioned algorithms significantly decreases with increasing processing time and target speed. In contrast, the proposed algorithm exhibits less performance change and shows a particularly significant performance improvement. For example, when the average processing time of a UWB node is 10 ms (e.g., ... FIG. 4 (as shown by the solid line in (a)) When the moving target speed is 35 m / s, the Doppler effect elimination method proposed in steps 1 and 2 reduces the root mean square error (RMSE) by 3.25 m, 0.89 m, 0.64 m, and 0.13 m, respectively, compared to the four algorithms mentioned above. When the moving target speed increases to 70 m / s, the localization RMSE of the proposed method decreases by 3.33 m, 1.05 m, 0.83 m, and 0.18 m, respectively. When the average processing time of the UWB node increases to 50 ms, and the moving target speed is still 70 m / s, the localization performance of the proposed method is improved by 82.28%, 75.12%, 75.83%, and 73.73%, respectively, compared to the four methods mentioned above.

[0031] (2) Performance analysis of fusion positioning model This embodiment compares typical fusion localization methods, including: EKF (Extended Kalman Filter) [5], AR-EKF (Adaptive Robust-Extended Kalman Filter) [6], FB-EKF (Forward Backward-Extended Kalman Filter) [3] and ESKF-SI (Error State Kalman Filter-Soft Information) [7]. Among them, EKF is a commonly used filtering method. AR-EKF adds an adaptive robust factor on this basis. FB-EKF adds a backward recursive process on the basis of the original forward prediction. ESKF-SI constructs a time series of localization errors and uses the error state Kalman filter method to correct the localization results.

[0032] The results are as follows FIG. 5 As shown in (a)-(c), the acceleration is set to 1 m / s². 2 Uniform acceleration, uniform velocity, -1 m / s 2 A model of uniformly decelerated motion.

[0033] It is evident that the EKF, AR-EKF, and ESKF-SI schemes cannot achieve the optimal UWB / IMU combination, and performance degradation occurs with prolonged use. The FB-EKF scheme utilizes MRC to eliminate some interference and further leverages forward-backward EKF to achieve trajectory smoothing. However, its performance is significantly affected by IMU accuracy. The method proposed in this invention can eliminate ranging errors caused by the Doppler effect, achieving adaptive fusion of UWB / IMU inputs, outperforming other fusion positioning schemes and less affected by duration and IMU accuracy. When the IMU has accurate input, i.e. FIG. 5 The solid line. Compared with EKF, AREKF, FB-EKF, and ESKF-SI methods, the proposed algorithm has average RMSE gains of 0.42 m, 0.36 m, 0.08 m, and 0.33 m, respectively. When the IMU input is inaccurate, i.e. FIG. 5 The dashed lines in the diagram indicate that the proposed algorithm consistently achieves better UWB / IMU fusion. Compared to the four methods mentioned above, the proposed algorithm improves localization performance by 63.08%, 60.96%, 47.05%, and 57.34%, respectively.

[0034] The above description is only a description of the preferred embodiments of the present application, and is not any limitation on the scope of the present application. Any modification or change made by any person skilled in the art according to the above disclosed technical content should be regarded as an equivalent effective embodiment, and belongs to the protection scope of the technical scheme of the present application.

[0035] The references are as follows: [1] K. Yang, G. Wang and Z. -Q. Luo, Efficient Convex Relaxation Methods for Robust Target Localization by a Sensor Network Using Time Differences of Arrivals[J]. IEEE Transactions on Signal Processing , 57(7):2775-2784, 2009. [2] H. Chen, G. Wang and N. Ansari, Improved Robust TOA-Based Localization via NLOS Balancing Parameter Estimation[J]. IEEE Transactions on Vehicular Technology , 68(6): 6177-6181, 2019. [3] W. -N. He, X. -L. Huang, Z. Xu, F. Hu and S. Yu, Robust Localization for Mobile Targets Along a Narrow Path With LoS / NLoS Interference[J]. IEEE Internet of Things Journal , 11(11): 20853-20866, 2024. [4] S. Zhao, N. Guo, X. -P. Zhang, X. Cui and M. Lu, Sequential Doppler-Shift-Based Optimal Localization and Synchronization With TOA[J]. IEEE Internet of Things Journal , 9(17): 16234-16246, 2022. [5] D. Feng et al., An Adaptive IMU / UWB Fusion Method for NLOS IndoorPositioning and Navigation[J]. IEEE Internet of Things Journal , 10(13): 11414-11428, 2023. [6] C. Wang, A. Xu, J. Kuang, X. Sui, Y. Hao and X. Niu. A High-Accuracy Indoor Localization System and Applications Based on Tightly CoupledUWB / INS / Floor Map Integration[J]. IEEE Sensors Journal , 21(16): 18166-18177,2021. [7] X. Zhou, L. Chen, Y. Chen, H. Yin, X. Chen and W. Wang, Fusion ofIMU and Probabilistic Model for Indoor Localization Based on BayesianFramework[J]. IEEE Internet of Things Journal , Early access, doi: 10.1109 / JIOT.2025.3535779.

Claims

1. A method for eliminating and fusing Doppler effects in high-speed trains, characterized in that, Includes the following steps: Step (1): Perform UWB ranging based on the two-way ToF ranging algorithm, construct a mathematical model for time of flight measurement under the Doppler effect, and calculate the ranging error caused by the Doppler effect; Step (2): Extract prior information of target state for UWB / IMU fusion positioning time series; derive statistical characteristics of UWB ranging error introduced by Doppler effect; eliminate interference of Doppler effect on UWB ranging; Step (3): For the UWB / IMU fusion positioning framework after Doppler interference bias elimination, derive the closed-form expression of the random error term of the UWB measurement equation; construct an adaptive UWB / IMU fusion positioning model to complete high-precision fusion positioning.

2. The Doppler effect elimination and fusion positioning method for high-speed trains according to claim 1, characterized in that, In step 1, The mathematical expression for the ranging error introduced by the Doppler effect can be simplified to: . in, Errors introduced by the Doppler effect For electromagnetic wave transmission rate, To calculate the true distance between the base station and the tag, For UWB node processing time, This represents the velocity component of the tag velocity in the base station-tag direction.

3. The Doppler effect elimination and fusion positioning method for high-speed trains according to claim 1, characterized in that, Step 2 includes the following steps: Step (21) assumes that the tag state vector in the UWB / IMU time series is composed of three-dimensional coordinates. ,speed acceleration Composition, the first The state vector at each time step is represented as: . Step (22): Extract the data from the UWB / IMU fusion time series based on the first... The first time state prediction Time-state vector As a priori, it follows a Gaussian distribution. The predicted state vector also consists of the position coordinates, velocity, and acceleration of the moving target, and is represented as follows: Its mean is the moving target's first The true state vector at each moment The variance is the prediction error covariance matrix, expressed as: , in, , , These are the error covariance matrices for predicted position, velocity, and acceleration, respectively. Indicates size is A matrix of all zeros; Step (23): The UWB tag communicates with the UWB base station sequentially, and uses bidirectional ToF to solve the distance information from the UWB tag to the UWB base station sequentially; combining multiple ranging data, the ToA positioning method is used to solve the tag's position information; the error vector in ToA positioning is derived as follows: Step (231), considering the ranging error in step (12), the ToA positioning equation is constructed as follows: , in, This represents the localization results under interference; the observation matrix is ​​used to represent the results. and measurement vector They are represented as follows: , . Step (232), based on the above measurement vector From the expression and the ranging error model in step (12), we can obtain that the error term introduced by the measurement vector is: . Step (233): Substitute the above formula into the measurement vector. The measurement vector is derived from the expression. Error vector in The expression is: . Step (234) considers that the distance between base station tags is much greater than the ranging error, i.e. The error term introduced in step (232) is approximately: Substituting into the ranging error model in step (12), it simplifies to: . Step (24) Derive the statistical properties of the error term Step (241), based on UWB / IMU time series prediction, the first Predicted location at time velocity vector Follows a Gaussian distribution. , ; Step (242): Based on the maximum likelihood probability, derive the first... Mean of each error term and variance They are respectively: , . Step (25), based on the mean of the deviation error term The estimated measurement error vector is: . Step (26), calibrate the measurement vector The label position coordinates were determined using the least squares method. : . After bias elimination, the corrected measurement vector There exists a random error with a mean of 0, and the error covariance is... .

4. The Doppler effect elimination and fusion positioning method for high-speed trains according to claim 1, characterized in that, Step 3 includes the following steps: Step (31), Observation model construction Step (311), in the first At time 1, assuming the measurement vector corrected in step 2... For the observation vector of the fusion model Its relationship with the true state vector The relationship between them is represented as follows: , in, Represents the state-measurement matrix. The error term in the observation equation is represented by a vector with a mean of 0 and a covariance matrix of... Gaussian distribution ; Step (312): In the fusion localization model, the state-measurement matrix is ​​the observation vector. Regarding the true state vector The Jacobian matrix is ​​expressed as: . Step (32), observation error term Closed expression of covariance: Step (321), assuming that the error vector is processed through step 2, The deviation has been eliminated, and the remaining error vector It follows a Gaussian distribution with a mean of 0 and the following variance: . Step (322), taking into account the processing time in practical applications Uncertainty, introduce scaling factor Set error terms The variance is expressed in closed form as: . The parameters can be adjusted This makes the observation model closer to the actual scenario; In step (33), the IMU can measure the three-dimensional acceleration of the moving target in real time, which is represented as a vector. In fusion localization, the acceleration change of the IMU is set as the model input, expressed as: . Step (34): Predict the target state at the current time based on the target state at the previous time step. Step (341), based on the basic characteristics of kinematics and target status at any time ,predict Target status at any time: . Where the state transition matrix Represented as: . Step (342): Since the acceleration vector input by the IMU has an error, its covariance is set to... Considering target status at any time There is an error, and its covariance matrix is ​​expressed as follows: The corresponding predicted target state is derived. The error covariance is: , The error transformation matrix, derived from the basic principles of kinematics, is as follows: . Step (343) involves converting the predicted state vector into a vector. and error covariance Substituting this into step 2, the Doppler effect can be eliminated; Step (35), in the fusion model, based on the estimated prediction and observation error covariance ( , Calculate the Kalman gain, expressed as . Step (35) to achieve UWB / IMU fusion positioning Step (351), based on the solved Kalman gain, merge the prediction and observation vectors, as follows: . The combined error covariance is expressed as: . Step (352): As the label moves, utilize the currently output target state. And error covariance matrix , used for Predicting the state at any given time.

Images