Railway train combined positioning method based on dual-adaptive cubature Kalman filtering
Through the dual adaptive cubature Kalman filter algorithm and data fusion, the problems of reduced accuracy and easy divergence of traditional train positioning in mountainous environments are solved, and high-precision and stable train positioning is achieved, which can adapt to noise changes in complex environments.
Patent Information
- Application Number
- CN202510867916.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-06-26
- Publication Date
- 2025-10-10
AI Technical Summary
Traditional train positioning methods have problems of reduced accuracy and easy divergence in complex mountainous environments, especially the cumulative error of the axle pulse speed sensor and the inaccurate positioning caused by wheel diameter wear.
A combined railway train positioning method based on dual adaptive cubature Kalman filters is adopted. By fusing data from wheel axle pulse sensors, GNSS receivers, and accelerometers, combined with adaptive cubature Kalman filters and square root algorithms, a state and observation model is constructed. Information fusion is performed using twin filters to improve positioning accuracy and stability.
It significantly improves the accuracy and stability of train positioning, adapts to noise changes in complex environments, suppresses filtering divergence, and enhances the system's anti-interference and reliability. Simulation experiments have verified its superiority in complex environments.
Smart Images
Figure CN120756551A_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the technical field of train positioning, and particularly relates to a railway train combined positioning method based on double adaptive cubature Kalman filtering. BACKGROUND
[0002] Train operation control system (referred to as train control system) is one of the core technical equipment of rail transit, and positioning technology plays a core role in ensuring safe and efficient operation of trains. The traditional train positioning method uses wheel axle pulse speed sensor to realize train speed positioning, but problems such as idling, slipping and wheel diameter wear will cause cumulative error. For complex mountainous environment, traditional methods such as extended Kalman filter face problems such as precision decline and easy divergence, and it is urgent to research more stable train autonomous combined positioning method to overcome the influence of complex environment on train positioning and realize low-cost, high-precision and stable train positioning. SUMMARY
[0003] The present application relates to the technical field of train positioning, and particularly relates to a railway train combined positioning method based on double adaptive cubature Kalman filtering.
[0004] The technical scheme of the present application is as follows: a railway train combined positioning method based on double adaptive cubature Kalman filtering comprises the following steps:
[0005] S1, obtaining wheel axle pulse sensor speed, mileage measurement value, GNSS position measurement value and acceleration measurement value of the train through the output signals of the devices arranged on the train; obtaining the mileage and position constraint conditions of the train;
[0006] S2, constructing an observation model according to the wheel axle pulse sensor speed, mileage measurement value, GNSS position measurement value and acceleration measurement value of the train; constructing a state model according to the mileage and position constraint conditions of the train;
[0007] S3, generating a state vector by using a first sub-filter based on the state model and the observation model;
[0008] S4, taking the state vector as the input of a second sub-filter, and performing fusion processing by using the second sub-filter to obtain the fusion state information of the train.
[0009] Further, in S1, the output signals of the wheel axle pulse sensor, GNSS receiver and accelerometer arranged on the train are collected, and the output signals are calculated to obtain the wheel axle pulse sensor speed, mileage measurement value, GNSS position measurement value and acceleration measurement value; wherein the calculation formula for calculating the output signal of the wheel axle pulse sensor is:
[0010]
[0011] In the formula, ΔS represents the mileage of the train, Δt represents the speed measurement period, n represents the pulse measurement value of the current period, D represents the diameter of the train wheel, v represents the running speed of the train, and N represents the number of pulses emitted by the wheel axle pulse sensor per revolution of the wheel.
[0012] Further, in S2, the observation model z k is expressed as:
[0013] z k = [x k , y k , v k , s k , a k ] T ;
[0014] In the formula, x k represents the eastward coordinate of the train at time k, y k represents the northward coordinate of the train at time k, v k represents the running speed of the train at time k, s k represents the mileage of the train at time k, a k represents the acceleration of the train at time k, and T represents the transpose operation.
[0015] In S2, the expression of the state model is:
[0016]
[0017] In the formula, x k-1 represents the state vector of the train at time k-1, u k represents the observation vector at time k, f(·) represents the mapping function of the electronic map, w k represents the state transition noise, v k represents the observation noise sequence, H represents the observation matrix, and E4 represents a 4x4 unit matrix.
[0018] The state transition noise w k and the observation noise sequence v k are subject to a Gaussian distribution with a mean of 0 and a covariance matrix Q k ,R k .
[0019] Further, S3 includes the following sub-steps:
[0020] S31, using a first sub-filter to determine whether the filter divergence satisfies If yes, using the Sage-Husa noise estimator to perform noise updating on the observation model, otherwise not performing noise updating, and entering S32; wherein ε k represents the filter residual error, and R krepresents the noise covariance matrix, tr(·) represents the trace operation on the matrix, T represents the transpose operation, γ represents the reserve coefficient, and γ≥1; the first sub-filter adopts an adaptive cubature Kalman filter;
[0021] S32, performing filtering using the first sub-filter to obtain a filtering result;
[0022] S33. Process the filtering result and the noise covariance matrix of the state model using a square root algorithm to obtain a state vector.
[0023] Furthermore, in S31, the expression of the Sage-Husa noise estimator is:
[0024]
[0025] Where, ε k represents the filter residual, z k represents the observation model, h(·) represents the observation equation of the nonlinear process, represents the system state covariance matrix from k-1 to k moments, represents the system measurement covariance matrix at time k, d k For the forgetting factor, represents the system measurement covariance matrix at time k-1, H represents the observation matrix, P k|k-1 Represents the predicted covariance matrix from k-1 to k, and T represents the transpose operation.
[0026] Furthermore, in S32, the expression for filtering performed by the first sub-filter is:
[0027]
[0028] Where S k|k-1 represents the volume point state estimation covariance matrix, m represents the number of sampling points, represents the measurement prediction vector of the volume point, represents the target measurement prediction vector, T represents the transposition operation, Denotes the observation covariance matrix of the first sub-filter, C k represents the state measurement covariance matrix at time k, Represents the state prediction of the volume point from k-1 to k time, Represents the predicted state vector from k-1 to k time, S k represents the measurement covariance matrix, represents the state estimation vector of the first sub-filter at time k, represents the state vector obtained by filtering the first sub-filter, K k represents the Kalman gain, z k represents the observation vector.
[0029] Furthermore, in S33, the expression of the square root algorithm is:
[0030]
[0031] Where R k (1) represents the observation noise covariance matrix of the first subfilter setting, R k (2) represents the observation noise covariance matrix of the second subfilter setting, R k represents the intermediate variable matrix obtained by performing square root processing on the first sub-filter, and T represents the transpose operation.
[0032] Furthermore, S4 includes the following sub-steps:
[0033] S41, inputting the state variable into the second sub-filter for fusion filtering; the second sub-filter adopts adaptive cubature Kalman filtering;
[0034] S42: Feedback the fusion filtering result to the first sub-filter, and convert it using the mapping function of the electronic map to obtain the fusion status information of the train.
[0035] Furthermore, in S41, the expression for performing fusion filtering is:
[0036]
[0037] Where, represents the state vector before filtering by the second sub-filter, represents the state vector obtained by filtering by the first sub-filter, represents the mileage component in the state vector of the first sub-filter, z k represents the observation model, x k represents the eastward coordinate of the train at time k, y k represents the north coordinate of the train at time k, v k represents the train speed at time k, s k represents the train mileage at time k, a k represents the train acceleration at time k, and T represents the transposition operation.
[0038] Furthermore, in S42, the expression for feeding back the fusion filtering result to the second sub-filter is:
[0039]
[0040] Where, represents the east-west coordinate of the state vector of the first sub-filter, represents the north coordinate in the state vector of the first sub-filter, represents the mileage component in the state vector of the first sub-filter, represents the mileage component in the state vector of the second sub-filter, and f(·) represents the mapping function of the electronic map.
[0041] (1) The present invention significantly improves the accuracy of train positioning by introducing a dual adaptive cubature Kalman filter algorithm, adapting to noise variations in complex environments. The square root algorithm and improved filter divergence criterion are used to effectively suppress the filter divergence problem and enhance the stability and anti-interference performance of the system.
[0042] (2) The present invention combines GNSS, wheel axle pulse sensors, accelerometers and electronic maps to achieve effective fusion of multi-sensor data, thereby improving the reliability and accuracy of positioning. The performance of the proposed algorithm is verified through simulation experiments, demonstrating its superiority and practicality in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0043] Figure 1 The figure is a flow chart of a railway train combined positioning method based on dual adaptive cubature Kalman filter;
[0044] Figure 2 This is a flow chart of the combined positioning method for trains on the Western Plateau Railway in a specific implementation method of the present invention;
[0045] Figure 3 Build a simulation circuit diagram for the embodiment of the present invention;
[0046] Figure 4 This is a comparison chart of error values under different filtering conditions in an embodiment of the present invention;
[0047] Figure 5 2 is an error comparison diagram of an embodiment of the present invention under different models. DETAILED DESCRIPTION
[0048] The embodiments of the present invention will be further described below with reference to the accompanying drawings.
[0049] like Figure 1 As shown, the present invention provides a railway train combined positioning method based on dual adaptive cubature Kalman filtering, comprising the following steps:
[0050] S1. Obtain the train's wheel axle pulse sensor speed, mileage measurement, GNSS position measurement, and acceleration measurement through output signals from several devices installed on the train; and obtain the train's mileage and position constraints.
[0051] S2. Build an observation model based on the train's wheel axle pulse sensor speed, mileage measurement values, GNSS position measurement values, and acceleration measurement values; build a state model based on the train's mileage and position constraints;
[0052] S3, based on the state model and the observation model, using the first sub-filter to generate a state vector;
[0053] S4. Use the state vector as the input of the second sub-filter, perform fusion processing using the second sub-filter, and obtain the fusion state information of the train.
[0054] In this embodiment of the present invention, in S1, the output signals of the wheel axle pulse sensor, GNSS receiver, and accelerometer provided on the train are collected and resolved to obtain the wheel axle pulse sensor speed, mileage measurement value, GNSS position measurement value, and acceleration measurement value. The calculation formula for resolving the output signal of the wheel axle pulse sensor is:
[0055]
[0056] Where ΔS represents the mileage of the train, Δt represents the speed measurement period, n represents the pulse measurement value in this period, D represents the diameter of the train wheel, v represents the train speed, and N represents the number of pulses emitted by the axle pulse sensor per wheel rotation.
[0057] The output signal refers to the original signal detected by the speed measuring equipment that can reflect the movement state of the train. For example, the output signal of the wheel axle pulse sensor is the counting frequency of the pulse signal generated by the wheel rotation; the output signal of the accelerometer is the voltage generated by the internal piezoelectric crystal due to pressure deformation; the output signal of the GNSS receiver is the satellite signal of the satellite system received by the receiver.
[0058] In this embodiment of the present invention, in S2, a state-space approach is used to establish the system mathematical model required for information fusion. The system mathematical model includes a state model and an observation model. The state model is established based on the train's equations of motion and electronic map constraints to describe the train's motion process. The observation model is established based on sensor observation information to describe the observation information of each sensor. The state transition model is used to reflect the change in the train's motion state over time, that is, the relationship between the train's motion state at the previous moment and its motion state at the current moment.
[0059] Observation model z k The expression is:
[0060] z k =[x k ,y k ,v k ,s k ,a k ]T ;
[0061] Where x k represents the eastward coordinate of the train at time k, y k represents the north coordinate of the train at time k, v k represents the train speed at time k, s k represents the train mileage at time k, a k represents the train acceleration at time k, and T represents the transposition operation;
[0062] In S2, the expression of the state model is:
[0063]
[0064] Where x k-1 represents the train state vector at time k-1, u k represents the observation vector at time k, f(·) represents the mapping function of the electronic map, and w k represents the state transition noise, v k represents the observation noise sequence, H represents the observation matrix, and E4 represents the 4×4 unit matrix.
[0065] The state transition noise and the observation noise sequence have a mean of 0 and a covariance matrix of Q k ,R k Gaussian distribution.
[0066] In this embodiment of the present invention, S3 includes the following sub-steps:
[0067] S31, using the first sub-filter to determine whether the filter divergence satisfies If yes, the Sage-Husa noise estimator is used to update the observation model with noise, otherwise no noise update is performed and the process goes to S32; where ε k represents the filtering residual, R k represents the noise covariance matrix, tr(·) represents the trace operation on the matrix, T represents the transpose operation, γ represents the reserve coefficient, and γ≥1; the first sub-filter adopts an adaptive cubature Kalman filter;
[0068] S32, performing filtering using the first sub-filter to obtain a filtering result;
[0069] S33. Process the filtering result and the noise covariance matrix of the state model using a square root algorithm to obtain a state vector.
[0070] A dual filter is set up. The first sub-filter judges the filter divergence according to the measurement data and then filters the observation model to obtain preliminary estimation information. The observation covariance matrix information and the filtering result are fed back to the second sub-filter through the square root algorithm. The sub-filter adopts adaptive cubature Kalman filtering.
[0071] In the embodiment of the present invention, in S31, the expression of the Sage-Husa noise estimator is:
[0072]
[0073] Where, ε k represents the filter residual, z k represents the observation model, h(·) represents the observation equation of the nonlinear process, represents the system state covariance matrix from k-1 to k moments, represents the system measurement covariance matrix at time k, d k For the forgetting factor, represents the system measurement covariance matrix at time k-1, H represents the observation matrix, P k|k-1 Represents the predicted covariance matrix from k-1 to k, and T represents the transpose operation.
[0074] In the embodiment of the present invention, in S32, the expression for filtering performed by the first sub-filter is:
[0075]
[0076] Where S k|k-1 represents the volume point state estimation covariance matrix, m represents the number of sampling points, represents the measurement prediction vector of the volume point, represents the target measurement prediction vector, T represents the transposition operation, represents the observation covariance matrix of the first sub-filter, C k represents the state measurement covariance matrix at time k, Represents the state prediction of the volume point from k-1 to k time, Represents the predicted state vector from k-1 to k time, S k represents the measurement covariance matrix, represents the state estimation vector of the first sub-filter at time k, represents the state vector obtained by filtering the first sub-filter, K k represents the Kalman gain, z k represents the observation vector.
[0077] In the embodiment of the present invention, in S33, the expression of the square root algorithm is:
[0078]
[0079] Where R k (1) represents the observation noise covariance matrix of the first subfilter setting, R k (2) represents the observation noise covariance matrix of the second subfilter setting, R k represents the intermediate variable matrix obtained by performing square root processing on the first sub-filter, and T represents the transpose operation.
[0080] In this embodiment of the present invention, S4 includes the following sub-steps:
[0081] S41, inputting the state variable into the second sub-filter for fusion filtering; the second sub-filter adopts adaptive cubature Kalman filtering;
[0082] S42: Feedback the fusion filtering result to the first sub-filter, and convert it using the mapping function of the electronic map to obtain the fusion status information of the train.
[0083] In the embodiment of the present invention, in S41, the expression for performing fusion filtering is:
[0084]
[0085] Where, represents the state vector before filtering by the second sub-filter, represents the state vector obtained by filtering by the first sub-filter, represents the mileage component in the state vector of the first sub-filter, z k represents the observation model, x k represents the eastward coordinate of the train at time k, y k represents the north coordinate of the train at time k, v k represents the train speed at time k, s k represents the train mileage at time k, a k represents the train acceleration at time k, and T represents the transposition operation.
[0086] In this embodiment of the present invention, in S42, the expression for feeding back the fusion filtering result to the second sub-filter is:
[0087]
[0088] Where, represents the east-west coordinate of the state vector of the first sub-filter, represents the north coordinate in the state vector of the first sub-filter, represents the mileage component in the state vector of the first sub-filter, represents the mileage component in the state vector of the second sub-filter, and f(·) represents the mapping function of the electronic map.
[0089] The information fusion scheme of the present invention is as follows Figure 2 As shown in the figure, B01 is the train position information received by the GNSS receiver, B02 is the number of pulses received by the wheel axle pulse sensor, B03 is the voltage data detected by the accelerometer, and B04 is the first sub-filter. The first sub-filter fuses the information B01-B03 and sends the filtered result and the noise covariance matrix to the second sub-filter, i.e., B05. The second sub-filter combines the GNSS information (B01) with the filtering result of the first sub-filter to obtain the filtered result (B06).
[0090] To verify the train positioning method of the present invention, a simulation test platform was used to generate train operation data (speed data, acceleration data, and coordinate position information). This simulation data was used as the actual train operation status, and sensor data was simulated based on this data. The simulation environment is shown in Table 1.
[0091] Table 1
[0092]
[0093] like Figure 3 Figure 2 shows a simulated route diagram for a 60km section from Chengdu to Deyang, with six stations. To further validate the model's superiority, multiple different filter models were evaluated and compared using the same simulation environment. Specifically, the positioning method of the present invention was compared with a standard unscented Kalman filter and a cubic Kalman filter. The simulation results are shown in Table 2.
[0094] Table 2
[0095]
[0096] like Figure 4 As shown, the horizontal axis is the train mileage, and the vertical axis of the sub-graph is the error between the predicted value and the true value for easting, northing, and mileage. The dotted line represents the use of UKF, the segmented dotted line represents the use of CKF, and the thick solid line represents the dual filter of the present invention.
[0097] Through Table 2 and Figure 4 It can be seen that the measurement error of the dual filter designed by the present invention is significantly smaller than the measurement errors of the two standard Kalman filters, thus verifying the effectiveness of the present invention in train positioning. Furthermore, when the wheel axle pulse sensors have cumulative errors and the GNSS has large errors at certain times (abnormal conditions), the fusion positioning error is not significantly affected, and high accuracy is maintained, thus verifying the effectiveness of the present invention in fault tolerance.
[0098] To verify the superiority of the model and method selected in this invention, a comparison was conducted using the filtering scheme proposed in this invention, a combined filtering scheme without the second sub-filter, and a combined filtering scheme without the square root algorithm. The simulation comparison results are shown in Table 3.
[0099] Table 3
[0100]
[0101]
[0102] like Figure 5 As shown in the figure, the horizontal axis is the train mileage, and the vertical axis is the error between the predicted value and the true value for easting, northing, and mileage, respectively. The dotted line represents the filtering algorithm without the second sub-filter, the segmented dotted line represents the filter without the square root algorithm, and the thick solid line represents the dual filter of this design.
[0103] Through Table 3 and Figure 5 It can be seen that compared with the model that only uses a single adaptive filter, the dual-filter model designed in the present invention can effectively suppress the performance degradation caused by filtering divergence when dealing with frequent changes in observation noise, ensuring that the weight of GNSS observation information remains stable during the filtering process and does not gradually approach zero; compared with the model that does not use the square root filter, it has stronger robustness.
[0104] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and it should be understood that the scope of protection of the present invention is not limited to such specific descriptions and embodiments. Those skilled in the art can make various other specific variations and combinations based on the technical teachings disclosed in the present invention without departing from the essence of the present invention, and such variations and combinations are still within the scope of protection of the present invention.
Claims
1. A railway train combined positioning method based on dual adaptive cubature Kalman filtering, characterized in that: The following steps are involved: S1. Obtain the train's wheel axle pulse sensor speed, mileage measurement, GNSS position measurement, and acceleration measurement through output signals from several devices installed on the train; Get the train's mileage and location constraints; S2. Construct an observation model based on the train's wheel axle pulse sensor speed, mileage measurement values, GNSS position measurement values, and acceleration measurement values; Construct a state model based on the train's mileage and location constraints; S3, based on the state model and the observation model, using the first sub-filter to generate a state vector; S4. Use the state vector as the input of the second sub-filter, perform fusion processing using the second sub-filter, and obtain the fusion state information of the train.
2. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 1 is characterized in that: In S1, the output signals of the wheel axle pulse sensor, GNSS receiver, and accelerometer installed on the train are collected and the output signals are resolved to obtain the wheel axle pulse sensor speed, mileage measurement value, GNSS position measurement value, and acceleration measurement value; wherein the calculation formula for resolving the output signal of the wheel axle pulse sensor is: Where ΔS represents the mileage of the train, Δt represents the speed measurement period, n represents the pulse measurement value in this period, D represents the diameter of the train wheel, v represents the train speed, and N represents the number of pulses emitted by the axle pulse sensor per wheel rotation.
3. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 1 is characterized in that: In S2, the observation model z k The expression is: z k =[x k ,y k ,v k ,s k ,a k ] T ; Where x k represents the eastward coordinate of the train at time k, y k represents the north coordinate of the train at time k, v k represents the train speed at time k, s k represents the train mileage at time k, a k represents the train acceleration at time k, and T represents the transposition operation; In S2, the expression of the state model is: Where x k-1 represents the train state vector at time k-1, u k represents the observation vector at time k, f(·) represents the mapping function of the electronic map, and w k represents the state transition noise, v k represents the observation noise sequence, H represents the observation matrix, and E4 represents the 4×4 identity matrix.
4. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 1 is characterized in that: The S3 includes the following sub-steps: S31, using the first sub-filter to determine whether the filter divergence satisfies If yes, the Sage-Husa noise estimator is used to update the observation model with noise, otherwise no noise update is performed and the process goes to S32; where ε k represents the filtering residual, R k represents the noise covariance matrix, tr(·) represents the trace operation on the matrix, T represents the transpose operation, γ represents the reserve coefficient, and γ≥1; the first sub-filter adopts an adaptive cubature Kalman filter; S32, performing filtering using the first sub-filter to obtain a filtering result; S33. Process the filtering result and the noise covariance matrix of the state model using a square root algorithm to obtain a state vector.
5. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 4 is characterized in that: In S31, the expression of the Sage-Husa noise estimator is: Where, ε k represents the filter residual, z k represents the observation model, h(·) represents the observation equation of the nonlinear process, represents the system state covariance matrix from k-1 to k moments, represents the system measurement covariance matrix at time k, d k For the forgetting factor, represents the system measurement covariance matrix at time k-1, H represents the observation matrix, P k|k-1 Represents the predicted covariance matrix from k-1 to k, and T represents the transpose operation.
6. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 4 is characterized in that: In the above S32, the expression for filtering performed by the first sub-filter is: Where S k|k-1 represents the volume point state estimation covariance matrix, m represents the number of sampling points, represents the measurement prediction vector of the volume point, represents the target measurement prediction vector, T represents the transpose operation, represents the observation covariance matrix of the first sub-filter, C k represents the state measurement covariance matrix at time k, Represents the state prediction of the volume point from k-1 to k time, Represents the predicted state vector from k-1 to k time, S k represents the measurement covariance matrix, represents the state estimation vector of the first sub-filter at time k, represents the state vector obtained by filtering the first sub-filter, K k represents the Kalman gain, z k represents the observation vector.
7. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 4 is characterized in that: In S33, the expression of the square root algorithm is: Where R k (1) represents the observation noise covariance matrix of the first subfilter setting, R k (2) represents the observation noise covariance matrix of the second subfilter setting, R k represents the intermediate variable matrix obtained by performing square root processing on the first sub-filter, and T represents the transpose operation.
8. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 1 is characterized in that: The S4 includes the following sub-steps: S41, inputting the state variable into the second sub-filter for fusion filtering; the second sub-filter adopts adaptive cubature Kalman filtering; S42: Feedback the fusion filtering result to the first sub-filter, and convert it using the mapping function of the electronic map to obtain the fusion status information of the train.
9. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 8, characterized in that: In S41, the expression for performing fusion filtering is: Where, represents the state vector before filtering by the second sub-filter, represents the state vector obtained by filtering by the first sub-filter, represents the mileage component in the state vector of the first sub-filter, z k represents the observation model, x k represents the eastward coordinate of the train at time k, y k represents the north coordinate of the train at time k, v k represents the train speed at time k, s k represents the train mileage at time k, a k represents the train acceleration at time k, and T represents the transposition operation.
10. The railway train combined positioning method based on dual adaptive cubature Kalman filtering according to claim 8, characterized in that: In the above S42, the expression for feeding back the fusion filtering result to the second sub-filter is: Where, represents the east-west coordinate of the state vector of the first sub-filter, represents the north coordinate in the state vector of the first sub-filter, represents the mileage component in the state vector of the first sub-filter, represents the mileage component in the state vector of the second sub-filter, and f(·) represents the mapping function of the electronic map.