An improved factor graph-based vehicle positioning method

Abstract

The application discloses a kind of improved factor graph vehicle positioning method.The application includes the following steps: first, the motion information data collected by vehicle sensor is obtained;Then, according to the collected motion information data, a factor graph model is constructed;Then, according to the factor graph model, combined with the collected motion information data, the GNSS measurement value is detected by using the multi-condition analysis fault detection method, and the fault GNSS measurement information is obtained;Then, after the fault GNSS measurement information is isolated by deleting the corresponding factor node, the GNSS measurement information added to the factor graph model is updated;Finally, based on the current factor graph model, all navigation state quantities in the sliding window are optimized and solved, and the navigation information of the vehicle carrier is obtained, so as to realize the fusion positioning of the vehicle carrier.The application effectively suppresses the influence of abnormal GNSS measurement signal on navigation accuracy and effectively reduces the system false alarm rate.

Description

Technical Field

[0001] This invention relates to a positioning method in the field of vehicle-mounted integrated navigation technology, and more particularly to a vehicle carrier positioning method based on an improved factor map. Background Technology

[0002] Global Navigation Satellite Systems (GNSS) provide accurate position and velocity information in outdoor environments around the clock, serving as a primary source of global positioning reference for Intelligent Transportation Systems (ITS). However, GNSS positioning accuracy is significantly affected by the environment. Interference from multipath and non-line-of-sight factors can easily lead to interference, spoofing, and rejection, thus impacting navigation accuracy. In contrast, Inertial Navigation Systems (INS) can autonomously provide pose information and have a high output frequency, making them less dependent on the external environment. However, navigation errors accumulate over time, limiting their ability to provide high-precision positioning information to a relatively short period. GNSS / INS integrated navigation systems combine the advantages of both, avoiding the shortcomings of single navigation methods and providing high-precision, high-frequency navigation results.

[0003] EKF-based INS / GNSS integrated navigation methods have been widely used. However, EKF, based on the first-order Markov assumption, can only iterate step-by-step, failing to fully utilize historical information. Furthermore, in traditional integrated navigation methods, adding or removing sensors requires system reconstruction, increasing complexity and time consumption. The recently proposed factor graph approach offers a novel approach to multi-sensor fusion. Compared to the single-iteration EKF method, the factor graph method utilizes multiple measurements from historical moments to simultaneously estimate state variables and obtain the global optimum. Its multiple iterations and relinearization capabilities effectively improve the accuracy of solving nonlinear problems, and its plug-and-play feature makes adding or removing sensor factors very convenient.

[0004] Although the potential of factor graph methods in positioning has been explored, research shows that improved methods based on factor graphs can better adapt to the error distribution of multi-sensor fusion and have a certain robustness to outlier measurements. However, GNSS positioning integrity faces significant challenges in complex environments such as urban canyons. Multipath effects and non-line-of-sight interference are the main sources of GNSS gross errors, and faults have a significant impact on the accuracy of integrated navigation. Many scholars have proposed outlier detection methods using residual Mahalanobis distance, judging faults by setting a chi-square test threshold. This method is simple to operate and has low computational cost, but it has a high false negative rate for slowly varying faults.

[0005] Meanwhile, traditional factor graph methods require optimization of all variables. As new variables are added, the system's information matrix grows increasingly large, leading to a continuous increase in computational complexity. Sliding windows, by using marginal probabilities to marginalize past states, improve computational efficiency while preserving some historical information. Studies have also shown that multiple iterations of sliding window calculations can help improve the accuracy of fault detection. Summary of the Invention

[0006] In vehicle-mounted integrated navigation systems, GNSS signals are susceptible to multipath and non-line-of-sight interference, and gross errors and model-free errors in the observation data affect positioning accuracy. Furthermore, during practical use, especially in single-point positioning, the STD (Standard Distance) of the GNSS receiver may exhibit anomalies, leading to missed alarms. Therefore, to address the shortcomings of existing factor graph methods, this invention introduces a sliding window and proposes a multi-condition analysis-based fault detection method. The sliding window improves computational speed, and combined with the proposed fault detection method, erroneous measurement information is isolated promptly, thereby improving the accuracy of the integrated navigation system and reducing the missed alarm rate.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] 1) Acquire motion information data collected by sensors on the vehicle;

[0009] 2) Construct an IMU-ODO / GNSS factor map model based on the collected motion information data;

[0010] 3) Based on the IMU-ODO / GNSS factor map model and combined with the collected motion information data, the multi-condition analysis fault detection method is used to detect faults in GNSS measurements and obtain faulty GNSS measurement information.

[0011] 4) After isolating the faulty GNSS measurement information by deleting the corresponding factor nodes, update the GNSS measurement information added to the IMU-ODO / GNSS factor graph model;

[0012] 5) Based on the current IMU-ODO / GNSS factor graph model, optimize and solve all navigation state variables within the sliding window to obtain the navigation information of the vehicle carrier, thereby realizing the fusion positioning of the vehicle carrier.

[0013] Specifically, 2) refers to:

[0014] Based on the collected motion information data, an IMU-ODO pre-integration model is constructed and IMU-ODO factor nodes are established, as well as a GNSS measurement model and GNSS factor nodes are constructed, thus forming an IMU-ODO / GNSS factor graph model.

[0015] The formula for the IMU-ODO pre-integration model is as follows:

[0016]

[0017]

[0018]

[0019] Among them, b k The carrier coordinate system at time k is system b; It is the rotation matrix from frame b to the reference coordinate system w at time k-1; g w Let Δt be the gravitational force in the w coordinate system; k The time interval is from k-1 to k; Let b be the velocity projection and position projection of the b system relative to the w system at time k. These are the velocity and position projections of the b-frame relative to the w-frame at time k-1, respectively. For the IMU mounting angle, For wheel speed increments, The lever arm for wheel speed, These are the attitude quaternions at time k and time k-1, respectively. The rotation of the b-frame from time k-1 to time k; The attitude and velocity increments from time k-1 to time k.

[0020] The IMU-ODO / GNSS factor graphical model includes IMU-ODO pre-integrated factors and GNSS factors, specifically:

[0021] The IMU-ODO pre-integration factor satisfies the following formula:

[0022]

[0023] f bias (k k k k-1 )=d(k k -u(k k ))

[0024]

[0025] Among them, f IMU-ODO () represents the factor node of the IMU-ODO pre-integration model constructed by incorporating ODO into the pre-integration process, X k+1 X k The navigation state variables at time k-1 and time k are respectively. For zero bias of the gyroscope, For zero bias in the accelerometer, ck z is the wheel speed proportionality factor. k For sensor measurement information, d() is the Mahalanobis distance, h() is the observation equation, and f bias () represents the bias node; u(k) k ) represents the bias parameter k k The random walk model;

[0026] GNSS factors satisfy the following formula:

[0027]

[0028] Among them, f GNSS () represents a GNSS factor node; Let l be the GNSS position in the w coordinate system. b For the lever arm error between GNSS and IMU, Let be the rotation matrix from the b-system to the w-system; Let be the projection of the position of the b-frame relative to the w-frame at time k.

[0029] In the third point, the criteria in the multi-condition analysis fault detection method include residual criteria, STD criteria, and incremental error criteria.

[0030] The formula for the STD criterion is as follows:

[0031]

[0032] Where STD is the standard deviation of the position, d1 is the third error threshold, F indicates that the detection result is a fault, and T indicates that the detection result is normal.

[0033] The formula for the incremental error criterion is as follows:

[0034]

[0035] in, s1 is the difference between the GNSS position increment and the IMU-ODO pre-integrated mileage increment within adjacent time intervals, s1 is the second error threshold, F indicates that the detection result is a fault, and T indicates that the detection result is a normal condition.

[0036] In step 5), the Gauss-Newton nonlinear optimization method is used to solve all navigation state variables within the sliding window. After each solution is completed, the sliding window is moved back by one length until all sensor data have been solved and calculated to obtain the navigation information of the vehicle carrier.

[0037] The beneficial effects of this invention are:

[0038] This invention proposes a factor graph-based IMU-ODO / GNSS integrated navigation framework suitable for vehicle-mounted applications. By introducing a sliding window and combining it with a proposed fault detection method based on multi-condition analysis, it solves the problem of navigation accuracy degradation caused by GNSS measurement faults in traditional factor graph methods. Simultaneously, the proposed fault detection method effectively reduces the false alarm rate and improves system integrity.

[0039] This invention first proposes a factor graph-based integrated navigation fault detection method suitable for vehicle-mounted applications, which effectively solves the problem of large delays in detecting slow faults in existing methods.

[0040] With the development of intelligent transportation systems, in-vehicle integrated navigation systems are becoming increasingly sophisticated. Factor graph technology can effectively fuse data from multiple sensors and has a plug-and-play feature, showing great application potential in integrated navigation systems. This invention introduces a sliding window into the traditional model. Through the edge-shifting strategy of the sliding window, historical information is preserved while reducing the computational load of the system. This invention also proposes a fault detection method based on multi-condition analysis to identify and isolate faults in a timely manner, effectively reducing the impact of faults on the accuracy of the integrated navigation system.

[0041] It should be noted that not all technical solutions of this invention can achieve all the above-mentioned effects. Attached Figure Description

[0042] Figure 1 This is an overall framework diagram of an improved factor graph-based vehicle positioning method according to the present invention.

[0043] Figure 2 This invention provides an improved factor graph model for vehicle positioning methods.

[0044] Figure 3 This invention presents an improved factor map vehicle positioning method using GNSS and IMU-ODO incremental error vector decomposition diagrams.

[0045] Figure 4 This is a sliding window demonstration diagram of an improved factor graph vehicle positioning method according to the present invention.

[0046] Figure 5 This is a flowchart illustrating the steps of an improved factor graph-based vehicle positioning method according to the present invention.

[0047] Figure 6 This is a simulation result diagram of the fault detection of an improved factor graph vehicle positioning method according to the present invention. Detailed Implementation

[0048] The accompanying drawings will be briefly introduced below in conjunction with the objectives and technical solutions of this invention, and the invention will be further described in detail with reference to specific embodiments and related formula derivations.

[0049] like Figure 1 and Figure 5 As shown, the present invention includes the following steps:

[0050] 1) Acquire motion information data collected by sensors on the vehicle. Motion data includes acceleration, angular velocity, position, standard deviation of position (STD), and velocity. Position and STD are acquired via GNSS (Global Navigation Satellite System), three-axis angular velocity and three-axis acceleration are acquired via IMU (Inertial Measurement Unit), and wheel speed is obtained via ODO (Odometer of Wheel Speed). In practice, system noise and system state variables need to be initialized. Navigation system state variables include: vehicle position, vehicle velocity, vehicle attitude, gyroscope and accelerator zero bias, and wheel speed scaling factor. System noise is selected based on the sensor characteristics. Vehicle position information is the latitude, longitude, and altitude of the vehicle's geographical location, initialized to the initial GNSS latitude and longitude values. Velocity information includes eastward, northward, and azimuth velocities, initialized to 0 m / s. Attitude information includes pitch, roll, and yaw angles, initialized to the attitude angles after initial alignment.

[0051] 2) Construct an IMU-ODO / GNSS factor map model based on the collected motion information data;

[0052] 2) Specifically: Based on the collected motion information data, an IMU-ODO pre-integration model is constructed and IMU-ODO factor nodes are established, as well as a GNSS measurement model and GNSS factor nodes are constructed, thereby forming an IMU-ODO / GNSS factor graph model; such as Figure 2 As shown, when GNSS measurement information arrives, the corresponding factor nodes are connected to the factor nodes constructed by IMU-ODO. The IMU-ODO pre-integration model is constructed based on the angular velocity, acceleration, and velocity information acquired by the IMU and ODO, while the GNSS measurement model is constructed based on the position information acquired by GNSS.

[0053] The formula for the IMU-ODO pre-integral model is as follows:

[0054]

[0055]

[0056] Among them, b k The carrier coordinate system at time k is system b; It is the rotation matrix from frame b to the reference coordinate system w at time k-1; g w Let Δt be the gravitational force in the w coordinate system; k The time interval is from k-1 to k; Let b be the velocity projection and position projection of the b system relative to the w system at time k. These are the velocity and position projections of the b-frame relative to the w-frame at time k-1, respectively. For the IMU mounting angle, For wheel speed increments, The lever arm for wheel speed, These are the attitude quaternions at time k and time k-1, respectively. The rotation of the b-frame from time k-1 to time k; The attitude and velocity increments from time k-1 to time k.

[0057] Attitude and velocity increments from k-1 to k Satisfy the following formula:

[0058]

[0059]

[0060] in

[0061]

[0062]

[0063] Among them, f b ω b These are the specific force and angular velocity measurements output by the IMU in the b-series. Let be velocity and angle, respectively, and be the integrals of specific force and angular velocity, respectively.

[0064] When introducing OD observation data, NHC constraints are considered, including the velocity from frame b to the vehicle coordinate system v. It can be represented as:

[0065]

[0066] The mileage increment derived from speed is:

[0067]

[0068] in, For the IMU mounting angle, The wheel speed increment is represented as This is the output of the wheel speed odometer.

[0069] Therefore, the location updates for the IMU and odometry are represented as:

[0070]

[0071] The IMU-ODO / GNSS factor graphical model includes IMU-ODO pre-integrated factors and GNSS factors, specifically:

[0072] The IMU-ODO pre-integration factor satisfies the following formula:

[0073]

[0074] f bias (k k k k-1 )=d(k k -u(k k ))

[0075]

[0076] Among them, f IMU-ODO () represents the factor node of the IMU-ODO pre-integration model constructed by incorporating ODO into the pre-integration process, X k+1 X k The navigation state variables at time k-1 and time k are respectively. For zero bias of the gyroscope, For zero bias in the accelerometer, c k z is the wheel speed proportionality factor. k For sensor measurement information, d() is the Mahalanobis distance, h() is the observation equation, and f bias () represents the bias node; u(k) k ) represents the bias parameter k k The random walk model;

[0077] GNSS positioning results are obtained from a GNSS receiver. The measurement equation considering the lever arm effect can be expressed as:

[0078]

[0079] Let n be the GNSS position in the w coordinate system. k To measure position noise.

[0080] GNSS factors satisfy the following formula:

[0081]

[0082] Among them, f GNSS () represents a GNSS factor node; Let l be the GNSS position in the w coordinate system. b For the lever arm error between GNSS and IMU, Let be the rotation matrix from the b-system to the w-system; Let be the projection of the position of the b-frame relative to the w-frame at time k.

[0083] 3) Based on the IMU-ODO / GNSS factor graph model and the position standard deviation in the collected motion information data, a multi-condition analysis fault detection algorithm is used to detect faults in GNSS measurements and obtain faulty GNSS measurement information. If a fault is detected, the corresponding node factor is directly deleted to isolate the faulty measurement information.

[0084] In 3), the criteria in the multi-condition analysis fault detection algorithm include residual criteria, STD criteria, and incremental error criteria.

[0085] The formula for the STD criterion is as follows:

[0086]

[0087] Where STD is the standard deviation of the position, d1 is the third error threshold, F indicates that the detection result is a fault, and T indicates that the detection result is normal.

[0088] The formula for the incremental error criterion is as follows:

[0089]

[0090] in, s1 is the difference between the GNSS position increment and the IMU-ODO pre-integrated mileage increment within adjacent time intervals, s1 is the second error threshold, F indicates that the detection result is a fault, and T indicates that the detection result is a normal condition.

[0091] The specific derivations of the residual criterion and the error increment criterion are as follows:

[0092] Based on the GNSS factor model, the residual equation for the GNSS positioning factor can be obtained as follows:

[0093]

[0094] Assuming that the observation equation model and the IMU-ODO model parameters are free of outlier errors, then the measurement noise follows a Gaussian distribution with an expected value of 0 and a constant variance of ∑0, and we can obtain z i probability density distribution:

[0095]

[0096] If GNSS equipment malfunctions, the statistical properties of the measurement noise will change, and the residuals will... Hereinafter, the mean of r(k) will no longer be equal to 0, as described below:

[0097] Make a binary assumption about r(k), where H0 represents the absence of faults:

[0098] E{r(k)}=0,E{r(k)r(k) T}=∑0

[0099] H1 indicates a fault:

[0100] E{r(k)}=μ, E{(r(k)-μ)(r(k)-μ) T}=∑0

[0101] Therefore, the residual r(k) criterion is obtained:

[0102]

[0103] Where r1 is the first error threshold.

[0104] Within two adjacent GNSS epochs, the GNSS position increment is: Meanwhile, the IMU-ODO pre-integration is recursively calculated in increments, with mileage increments being... Figure 3 X in k and X k-1 These are the navigation state variables before GNSS fusion. Both the GNSS position increment and the IMU-ODO pre-integrated mileage increment can reflect the system's position change from time k-1 to k. When GNSS measurements are normal, the difference between the GNSS position increment and the IMU-ODO mileage increment is very small; conversely, if the GNSS measurement information is incorrect, the increment error will increase significantly. Based on this, it can be determined whether the GNSS measurement information is faulty. Since the increment is a vector, vector decomposition is used in the carrier coordinate system to accurately reflect the change in increment.

[0105] lateral incremental error It can be represented as:

[0106]

[0107] longitudinal incremental error for:

[0108]

[0109] Therefore, the incremental error between IMU-ODO pre-integration and GNSS for:

[0110]

[0111] The criterion for the incremental error between wheel speed and GNSS is:

[0112]

[0113] The fault detection method based on multi-condition analysis determines whether the built-in STD information is normal based on a comprehensive criterion combining residual criteria, STD criteria, and GNSS and IMU-ODO incremental error criteria. Specifically, it is divided into four categories, as shown in Table 1:

[0114] Table 1 shows the results of the criterion.

[0115]

[0116] In cases 1 or 3, when the residual method and STD detection results are the same, the result is judged as normal, i.e., consistent with the STD judgment. In case 2, where the residual method judges it as normal but the STD method judges it as abnormal, this is a false alarm, which has a low probability of occurring in practice. In case 4, the residual method shows that the GNSS signal is normal, while the STD method shows an abnormal situation, which is a common phenomenon in navigation systems: missed alarms. Because the residual method is more sensitive to faults, when faults accumulate, the system needs additional time to recover. Therefore, we introduce incremental errors from GNSS and IMU-ODO to help determine the end of the fault. If the incremental errors from GNSS and IMU-ODO are judged as normal at this time, the system fault is considered to have ended and returned to normal.

[0117] 4) Taking advantage of the plug-and-play feature of factor graphs, faulty GNSS measurement information is isolated by deleting the corresponding factor nodes, and then the GNSS measurement information added to the IMU-ODO / GNSS factor graph model is updated.

[0118] 5) Based on the current IMU-ODO / GNSS factor graph model, select the sliding window size, optimize and solve all navigation state variables within the sliding window to obtain the navigation information of the vehicle carrier, thereby realizing the fusion positioning of the vehicle carrier.

[0119] In step 5), the navigation state variables of the vehicle include vehicle position, vehicle velocity, vehicle attitude, gyroscope zero bias and accumulator zero bias, and wheel speed scaling factor. The Gauss-Newton (GN) nonlinear optimization method is used to solve for all navigation state variables within the sliding window. After each solution, the sliding window is shifted forward by one length until all sensor data have been solved and calculated, thus obtaining the vehicle's navigation information, i.e., the vehicle's navigation result.

[0120] The sliding window optimization section utilizes the edge-mapping property of the sliding window to perform optimization within the window and edge-mapping data outside the window. After each optimization, the sliding window moves backward for the next optimization until all data has been optimized, yielding the final navigation result. Figure 4The diagram illustrates the sliding window method, assuming a sensor data update frequency of 10Hz, an optimization calculation frequency of 5Hz, and a sliding window size of 4. It can be seen that in the first second, the sliding window is still in its initial stage, containing only two variables. From the fifth second onwards, with each update, two past variables are deleted and two new variables are added, and so on.

[0121] This embodiment verifies the effectiveness of the proposed fault detection method through fault simulation. The gyroscope's zero-bias stability is set to 0.9° / h, the accelerometer's zero-bias stability to 0.01mg, and the GNSS positioning accuracy to 1.5m. The simulation time is 800s. A slowly varying fault is added to the GNSS latitude and longitude direction within a 180s period from 400s to 580s, with a maximum fault size of 25m. Since this simulates fault and missed alarm conditions, the STD test is normal at this time. The detection effects of three different methods—residual error test, GNSS and IMU-ODO incremental error test, and integrated test—are compared. The fault detection simulation results are as follows: Figure 6 As shown, Figure 6 'a' represents the statistic for residual detection. Figure 6 'b' represents the judgment result of the residual detection. Figure 6 c is the statistic for incremental error detection. Figure 6 d represents the judgment result of the incremental error criterion. Figure 6 'e' represents the statistical measure of the comprehensive test. Figure 6 The value of f is the result of the comprehensive test.

[0122] For the fault curve, a value of 1 indicates that the detection method identifies a fault, and a value of 0 indicates normal operation. The fault detection delays for different detection methods are shown in Table 2. Figure 6 As shown in Figure e, the proposed method can effectively distinguish between two adjacent different faults, while the residual test method considers them as a single fault segment, resulting in significant delays. The detection results shown in Table 2 indicate that all methods can detect the start of a fault relatively quickly, but compared to the residual test method, the proposed method can effectively detect the end of a fault, reducing the fault end detection time by 90%.

[0123] Table 2 compares the fault detection delays of different detection methods.

[0124]

[0125] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. An improved factor map-based vehicle positioning method, characterized in that, Includes the following steps: 1) Acquire motion information data collected by sensors on the vehicle; 2) Construct an IMU-ODO / GNSS factor map model based on the collected motion information data; Specifically, 2) refers to: Based on the collected motion information data, an IMU-ODO pre-integration model is constructed and IMU-ODO factor nodes are established, as well as a GNSS measurement model and GNSS factor nodes are constructed, thereby forming an IMU-ODO / GNSS factor graph model; The formula for the IMU-ODO pre-integration model is as follows: in, yes The coordinate system of the carrier at time Tie; yes From the moment Set to reference coordinate system Rotation matrix of the system; for Gravity in a coordinate system; for arrive The time interval; , for Moment Relative Tied to Velocity projection and position projection under the system. , They are respectively Moment Relative Tied to Velocity and position projection under the system; For the IMU mounting angle, For wheel speed increment, The lever arm for wheel speed, , They are respectively Time and The attitude quaternion at time; for arrive Within a moment System rotation; , for arrive At any given moment, the attitude and velocity increments; The IMU-ODO / GNSS factor graphical model includes IMU-ODO pre-integrated factors and GNSS factors, specifically: The IMU-ODO pre-integration factor satisfies the following formula: in, It refers to the factor nodes of the IMU-ODO pre-integration model constructed by adding ODO to the pre-integration process. , They are respectively Time and Navigation state variables at any given time. For zero bias of the gyroscope, To achieve zero bias in the accelerometer, This is the wheel speed proportional factor. For sensor measurement information, The Mahalanobis distance, For the observation equation, It is an offset node; For bias parameters The random walk model; GNSS factors satisfy the following formula: in, It is a GNSS factor node; for GNSS position in coordinate system For the lever arm error between GNSS and IMU, From Tie Rotation matrix of the system; for Moment Relative Tied to Position projection under the system; 3) Based on the IMU-ODO / GNSS factor map model and combined with the collected motion information data, the multi-condition analysis fault detection method is used to detect faults in GNSS measurements and obtain faulty GNSS measurement information; the criteria in the multi-condition analysis fault detection method include residual criteria, STD criteria, and incremental error criteria. 4) After isolating the faulty GNSS measurement information by deleting the corresponding factor nodes, update the GNSS measurement information added to the IMU-ODO / GNSS factor graph model; 5) Based on the current IMU-ODO / GNSS factor graph model, optimize and solve all navigation state variables within the sliding window to obtain the navigation information of the vehicle carrier, thereby realizing the fusion positioning of the vehicle carrier.

2. The improved factor map-based vehicle positioning method according to claim 1, characterized in that, The formula for the STD criterion is as follows: in, The standard deviation of the location, The third error threshold is F, which indicates that the detection result is a fault, and T indicates that the detection result is normal.

3. The improved factor map-based vehicle positioning method according to claim 1, characterized in that, The formula for the incremental error criterion is as follows: in, This represents the difference between the GNSS position increment and the IMU-ODO pre-integrated mileage increment within adjacent time intervals. The second error threshold is F, which indicates that the detection result is a fault, and T indicates that the detection result is normal.

4. The improved factor map-based vehicle positioning method according to claim 1, characterized in that, In step 5), the Gauss-Newton nonlinear optimization method is used to solve all navigation state variables within the sliding window. After each solution is completed, the sliding window is moved back by one length until all sensor data have been solved and calculated to obtain the navigation information of the vehicle carrier.

Images