Trajectory Recovery Based on Interval Forward–Backward Propagation Algorithm Fusing Multi-Source Information

Abstract

In the tracking scheme in which global navigation satellite system (GNSS) measurement is temporally lost or the sampling frequency is insufficient, dead reckoning based on the inertial measurement unit (IMU) and other location-related information can be fused as a supplement for real-time trajectory recovery. The tracking scheme based on interval analysis outputs interval results containing the ground truth, which gives it the advantage of convenience in multi-source information fusion. In this paper, a trajectory-recovery algorithm based on interval analysis is proposed, which can conveniently fuse GNSS measurement, IMU data, and map constraints and then output an interval result containing the actual trajectory. In essence, the location-related information such as satellite measurement, inertial data, and map constraints is collected by practical experiments and then converted into interval form. Thereby, the interval-overlapping calculation is performed through forward and backward propagation to accomplish the trajectory recovery. The practical experimental results show that the trajectory recovery accuracy based on the proposed algorithm performs better than the traditional Kalman filter algorithm, and the estimated interval results deterministically contain the actual trajectory. More importantly, the proposed interval algorithm is approved to be convenient to fuse additional location-related information.

1. Introduction

With the development of positioning and navigation technology, the global navigation satellite system (GNSS) module is widely used in various smart devices, such as mobile phones, autonomous vehicles, etc. The generated position and trajectory data can be used to ensure the safety of users and provide them with more location-based services (LBSs), such as vehicle navigation [1], and driving behavior analysis [2], etc. However, the effectiveness of these services relies on the sampling rate of trajectory data, and the trajectory with low sampling rate will lose the key information of moving targets and increase the uncertainty between sampling points. In addition, the high uncertainty between the continuous sampling points of the trajectory will also seriously affect the relevant research on the trajectory, including trajectory clustering analysis [3], trajectory indexing [4], and trajectory classification [5]. Therefore, it is necessary to develop a high-quality trajectory-recovery algorithm based on the original low-sampling GNSS data.

To obtain high sampling rate trajectory data, many efforts have been made in the literature. However, there are two problems to be solved with current existing research schemes. First, many methods are based on numerical algorithms, including the dead reckoning (DR) algorithm [6,7] and the Kalman filter algorithm [8,9], etc. However, they lack consideration of error, or cannot solve the error accumulation, and the final estimated result is far from the actual value. Secondly, many studies cannot provide a framework to integrate the map information and points of interest available in the road network [9,10,11] in a convenient manner.

The main contributions of this paper can be summarized as follows. By investigating the above state-of-the-art trajectory recovery schemes, this paper proposes an interval forward–backward propagation algorithm (FBP), which can integrate the measurement data of multiple sources for target tracking, such as the inertial navigation data, the GNSS data, and the map constraint information. The novelty of our proposed trajectory-recovery scheme lies in two main contributions.

• The first contribution is to propose a new, reliable framework with which to fuse various positioning location-related information more conveniently. Because the data of inertial navigation information, GNSS information, and map constraint information contain uncertainty, an information fusion method by intersection operation proposed in this paper is suitable for the fusion of multi-source information;
• The second contribution is that a novel algorithm proposed in this paper can ensure that the actual trajectory of the target is included in the trajectory interval result calculated by the proposed algorithm.

The remainder of this paper is organized as follows. Section 2 describes the work related to trajectory recovery. Section 3 briefly introduces the proposed system model proposed in this paper. Section 4 shows the experiment and some simulation results. Section 5 analyzes and summarizes the effect of this algorithm.

2. Related Works

In this section, we review the existing research work on trajectory recovery, including trajectory recovery works based on interval analysis [12,13,14,15,16,17], and other studies which fail to consider map information integration [18,19,20,21,22].

In recent years, there have been many outstanding research programs in the field of location tracking and trajectory recovery. For example, Wang et al. proposed a deep hybrid trajectory recovery model based on the current popular artificial intelligence method in [9], aiming to achieve intelligent trajectory recovery. However, the application of depth learning is still inadequate for some nonlinear trajectory recovery scenes. Therefore, based on the good performance of interval analysis in the field of mechanical control research [12], many researchers expect interval analysis to have good performance in track recovery and accurate positioning because interval analysis can eliminate the uncertainty in data acquisition and plays an important role in nonlinear state estimation. As expected by scholars, the interval analysis method significantly improves the accuracy of vehicle location and trajectory recovery. Soares et al. proposed a target tracking method based on interval analysis [13], in which the error of the vehicle location error obtained was smaller than that of the particle filter. However, the experimental result of Soares et al. shows that the interval results are messy, which will increase the error of local positioning points. Traditionally, the vehicle is usually modeled as a grid, and the trajectory of the vehicle is modeled as a line. This usually leads to great uncertainty between the estimated trajectory and the actual trajectory. Therefore, Xu et al. modeled the automated guided vehicle (AGV) as an interval and estimated its trajectory interval [14]. The problem of the large deviation between the estimated trajectory and the actual trajectory is solved by this method. However, the author limited the value of the AGV yaw angle in the experiment, so the method may not be available in the actual situation. In [15], Hamed et al. used high confidence intervals to estimate the trajectory and altitude of the aircraft, and the result shows that the estimated trajectory area is still growing. In addition to the messy interval and the large area of the interval, researchers rarely pay attention to the correction of GNSS for track recovery. Therefore, Wang et al. proposed the method of interval analysis and GNSS integration to estimate the vehicle trajectory in [16]. Then, the estimated trajectory is compared with the trajectory of the DR algorithm, the results of which show that the trajectory obtained by interval and GNSS fusion is closer to the reference path. However, the trajectory interval obtained by Wang et al. was obtained through experiments on uncovered roads, and they did not verify whether their method was suitable for GNSS interference. In [17], Ghalia et al. proposed the use of bounds-error estimation (BEE) and belief-state estimation (BSE) to get the trajectory of the vehicle in the nonlinear state and compared it with the trajectory obtained by the particle filter. The results show that the accuracy of BSE is higher. The method of Ghalia et al. has been optimized in the research field of trajectory estimation. However, the trajectory obtained by their method still has the problems of large intervals area and messy intervals. These problems will cause the estimated vehicle trajectory to deviate from the actual road.

Positioning based on the map information measured can improve the positioning accuracy and improve the robustness of positioning algorithms, such as radar data and road scene images [18], road visual features collected by using road scene fingerprints, or 3D features [20]. However, they pay little attention to the map information in actual situations, so they lose the information to improve positioning accuracy. In this case, Wang et al. proposed to use the node information of streets and buildings (OpenStreetMap) for precise positioning in [19]. This positioning method improves the location accuracy of urban scenes. However, due to the lack of improvement in the accuracy of suburban location, Wang et al.’s precise location method lacks universality. In order to find the map information that can be used for general positioning, lm et al. proposed to use the vertical angle characteristics of buildings and ground as the map information for precise positioning in [21]. Experiments show that this method can improve positioning accuracy compared with other methods. However, the measurement of the map information requires the use of the 2D radar, which is not applicable to general real-time positioning scenarios. Oh et al. used lidar and stereo vision sensors to measure obstacles in the course of vehicle movement as map information [22], and then proposed an occupation grid filter to fuse the map information. This method of merging map information and sensor information can obviously improve the positioning accuracy of vehicles. The experiment shows that the location error distance is in a stable range. Therefore, the map information can really help improve positioning accuracy. However, when the map information is difficult to measure [18,20], or the map information lacks universality [21], or the survey map information cannot be easily and quickly integrated with the proposed method [22], the applicability and efficiency of the positioning system will be reduced.

The above trajectory-optimization algorithms are suitable for the research of trajectory recovery. However, many kinds of map information in the road network have not been integrated and analyzed. To the best of our knowledge, a significant point of our work is that map constraint information is added and integrated with inertial sensor information and GNSS information to perform the trajectory recovery research. The actual experiment also proves that the trajectory calculated by our proposed method is more effective.

3. Trajectory Recovery Frameworks

Before describing our proposed FBP algorithm, we shall first introduce the existing GNSS/IMU based trajectory recovery framework for comparison.

Table 1. Notations.

Notation	Description
$\theta (t)$	The yaw angle of the vehicle at $t$
$v(t)$	The velocity of the vehicle at $t$
$a_x(t)$	The acceleration of the vehicle in the $x$ direction at $t$
$a_y(t)$	The acceleration of the vehicle in the $y$ direction at $t$
$S_x(t)$	The displacement of the vehicle in the $x$ direction at $t$
$S_y(t)$	The displacement of the vehicle in the $y$ direction at $t$
$\tau$	The sampling interval of vehicle sensor data
$x(k)$	The Kalman filter state in time slot $k$
$u(k)$	The Kalman filter system control value in time slot $k$
$w(k)$	The process noise in time slot $k$
$z(k)$	The system measurements in time slot $k$
$y(k)$	The measurement noise in time slot $k$
$A$	The Kalman filter state transition matrix
$B$	The Kalman filter control input matrix
$H$	The Kalman filter state observation matrix
$x(t)$	The trajectory coordinates of the vehicle at $t$
$\left[x\right](t)$	The trajectory interval based forward propagation of the vehicles at $t$
$\left[x^{\prime }\right](t)$	The vehicle’s trajectory based on FBP of the at $t$
$\left[v\right](t)$	The velocity interval of the vehicle at $t$
$\left[\theta \right](t)$	The yaw angle interval of the vehicle at $t$
$\left[a\right](t)$	The acceleration interval of the vehicle at $t$
$\left[S\right](t)$	The trajectory interval of the vehicle at $t$
$\overline{k}$	The sampling points of the sensor between two GNSS coordinate points of the vehicle
$x_{tr}(t)$	The calculated trajectory coordinates of vehicle at $t$
$area(t)$	The trajectory interval area of vehicle at $t$
$\left[x_m\right](t)$	The vehicle’s trajectory interval obtained by the FBP algorithm integrated map constraint information at $t$
$\left[Ma{p}_c\right]$	Map constraint information in a road network
$t_m$	The timestamp of map constraint information occurrence

lists the notations in this paper.

3.1. Kalman Filter Based Trajectory Recovery Framework

First, we introduce the traditional Kalman filter algorithm into the considered trajectory recovery scenario for comparison. When the positioning environment is limited or the positioning sampling is insufficient, the research methods of accurate positioning and trajectory recovery mainly combine GNSS and inertial navigation units based on the Kalman filter algorithm.

The most common GNSS/IMU positioning is multi-source fusion technology. First, the DR algorithm is used to calculate the position of moving targets, and then the GNSS coordinate points are used to correct the calculated position data. According to the dead reckoning algorithm [23], the formula is as follows:

$$\{\begin{array}{c}{S}_x(t)=v(t)\tau \cos \theta (t)+\frac{1}{2}{a}_x(t){\tau }^2\\ S_y(t)=v(t)\tau \sin \theta (t)+\frac{1}{2}{a}_y(t){\tau }^2\end{array}.$$

(1)

After data fusion using the DR algorithm, the coordinate points converted by GNSS are used to correct the large deviation displacement coordinates obtained by the DR algorithm. Due to the error accumulation of the IMU sensor, the error between the moving target trajectory and the actual trajectory corrected by GNSS in the DR algorithm will be large. Now we introduce the Kalman filter algorithm to correct the integration result of the DR algorithm [24].

We establish the following Kalman filter model:

$$\{\begin{array}{c}x(k)=Ax(k-1)+Bu(k)+w(k)\\ z(k)=Hx(k)+y(k)\end{array}.$$

(2)

It can be seen from the above formula that the Kalman filter algorithm can reduce the position drift caused by the heading error and fault zero velocity measurement.

3.2. Interval Forward and Backward Propagation Trajectory Recovery Framework

Inertial navigation-integrated positioning will generate error accumulation, and the Kalman filter can be used to mitigate the error accumulation. However, the results of the Kalman filter framework may also generate errors due to the nonlinear motion model. In addition, the traditional numerical operation cannot guarantee that the actual trajectory of the vehicle exists in the calculation result, compared with the interval operation. Thus, the first goal of the proposed FBP method is to ensure that the actual trajectory of the vehicle exists in the calculated trajectory interval. In other word, our proposed FBP algorithm will provide a deterministic result containing the ground truth.

The proposed algorithm is explained as follows. We need to briefly introduce the process of interval analysis [25]. Interval analysis can be defined as the extension of classical real arithmetic operators to set theory. The intervals are defined by a lower and an upper bound, which are $x^{\inf }$ and $x^{\sup }$ respectively. Therefore, an interval $\left[x\right]=\left[x^{\inf },x^{\sup }\right]$ is defined as the set of all real numbers between those bounds [26,27]. The implicit meaning is that we only know the true value $x*$ is guaranteed to be in the interval $\left[x\right]$, i.e., $x*\in [x]$. For example, the detection error of GNSS equipment can generally be accurate to $\pm 5$ meter, so its real error exists in the interval $\left[-5,5\right]$ meter.

Step 1. In this paper, we transform the collected data into interval data, such as acceleration, yaw angle, velocity, and the GNSS coordinates. The upper and lower limits of these data are defined by their normal distribution [26]. Then, we fused the segmented data to obtain the displacement interval of each time slot,

$$\left[S\right](t)=\left[v\right](t)f(\left[\left[\theta \right](t)\right])+\frac{1}{2}\left[a\right](t){\tau }^2$$

(3)

$$f(\left[\left[\theta \right](t)\right])=\{\begin{array}{c}\cos \left[\left[\theta \right](t)\right],\mathrm{X} \mathrm{direction}\\ \sin \left[\left[\theta \right](t)\right],\mathrm{Y} \mathrm{direction}\end{array}.$$

(4)

It should be noted in Equations (3) and (4) that because the sampling time interval is fixed, we think that the interval $\tau$ is a real constant.

Step 2. When we solve kinematic problems, we often encounter the following equation:

$$\left[x\right](t+\tau )=\left[x\right](t)+\left[S\right](t).$$

(5)

In practice, the initial GNSS point and the end GNSS point are available. After obtaining the displacement interval $\left[S\right](t)$ at each sensor sampling time through Equation (3), we can obtain the trajectory interval at any sensor sampling time between two GNSS points through Equation (5). The trajectory coordinates of the two GNSS points have been determined. Therefore, we can use the method of interval intersection to reduce the trajectory area. In addition, the intersection of the end GNSS coordinate interval and the forward trajectory interval can constrain the area of the trajectory interval because the end GNSS coordinate is available. We can get the trajectory interval of the backward propagation as shown in Equation (6):

$$\left[x^{\prime }\right](t-\tau )=\left[x\right](t-\tau )\cap (\left[x^{\prime }\right](t)+\left[S\right](t-\tau )).$$

(6)

Equation (6) is the calculation process of vehicle’s trajectory from the end GNSS point to the initial GNSS point. The trajectory interval at each sampling time needs to intersect with the track interval estimated by the forward propagation in this way. Through Equation (6), the backward propagation interval can be constrained to estimate a definite interval. In particular, the reason why the displacement interval of the previous time slot is used here is that we calculate from the later time to the previous time. In practice, the initial trajectory coordinates and destination trajectory coordinates of the vehicle are known.

Step 3. A new optimized trajectory point is obtained by intersecting the end GNSS coordinate interval with the estimated trajectory interval in step 2. In fact, we also obtain a new optimized trajectory at the initial point or a sensor sampling point. At this time, we need to constrain the area of the entire trajectory interval through new forward propagation:

$$\left[x^{\prime }\right](t+\tau )=\left[x\right](t+\tau )\cap (\left[x^{\prime }\right](t)+\left[S\right](t)).$$

(7)

It should be noted that $\left[x^{\prime }\right](t)$ is the constrained trajectory interval, and $\left[x\right](t)$ is the trajectory interval estimated by the initial forward propagation (or forward and backward propagation, as shown in Algorithm 2). Because “interval forward and backward propagation” calculates the trajectory between two GNSS points, we know the trajectory coordinates of vehicle at the two GNSS points. Here, we define them as the initial GNSS interval $\left[x\right](0)$ and the end GNSS interval $\left[x\right](\overline{k})$. The introduction of the FBP algorithm is as follows.

In Algorithm 1, we use three functions to calculate the illustrative indicators in this paper. Next, we will explain the definitions of these three functions. Before introducing the three formulas, we first need to know that $[x](t)$ includes the trajectory interval of the vehicle along the $X$ direction and the $Y$ direction (the $X$ direction is the orientation of the vehicle at the initial time, and the $Y$ direction is a direction perpendicular to the orientation of the vehicle at the initial time). Therefore, $[x](t)$ can be formulated as:

$$\left[x\right](t)=\left[\left[x_X\right](t)\cup \left[x_Y\right](t)\right].$$

(8)

Hence, the three functions in Algorithm 1 can be formulated as:

$$f_{mid}([x](t))=(\frac{x_X^{\sup }+x_X^{\inf }}{2},\frac{x_Y^{\sup }+x_Y^{\inf }}{2})$$

(9)

$$f_{lenth}([x](t))=x_X^{\sup }-x_X^{\inf }$$

(10)

$$f_{width}([x](t))=x_Y^{\sup }-x_Y^{\inf }.$$

(11)

Algorithm 1 FBP (Interval based forward backward propagation)

Input: $\left[x\right](0)$, $\left[x\right][\overline{k}]$, $\left[S\right](t)$, $\overline{k}$

Output: $\left[x\right](t)$,$\left[x^{\prime }\right](t)$, $x_{tr}(t)$, $area(t)$

1   Initialize: $\left[x\right](t)=\varnothing$ $\left[x^{\prime }\right](t)=\varnothing$ 2   $x_{tr}(t)\leftarrow 0;$ 3   $area(t)\leftarrow 0;$ 4    $\mathbf{for} t=0$$\mathrm{to} \overline{k}$ do 5         $\mathbf{if} t+\tau =\overline{k}$ then 6                  $\left[x^{\prime }\right](t)=\left[x\right](t)\cap \left[x\right](\overline{k})$ 7          else if $t+\tau \ne \overline{k}$ then 8                     $\left[x\right](t+\tau )=\left[S\right](t)+\left[x\right](t)$ 9          end if 10   end for 11   $\mathbf{for} t=\overline{k}$$\mathrm{to} dt$ do 12            $\left[x^{\prime }\right](t-\tau )=(\left[x^{\prime }\right](t)-\left[S\right](t-\tau ))\cap \left[x\right](t-\tau )$ 13   end for                                    //interval-based backward propagation step 14   for $t=0$ to $\overline{k}$ do 15           $x_{tr}(t)=f_{mid}(\left[x^{\prime }\right](t))$ 16           $area(t)={\displaystyle \sum f_{lenth}(\left[x^{\prime }\right](t))·f_{width}(\left[x^{\prime }\right](t))}$ 17   end for

In Algorithm 1, we transformed the date of various sensors and GNSS coordinates into interval data, and obtained the vehicle’s trajectory interval $\left[x\right](t)$ through the forward propagation. It is shown as a box for a sampling point of the trajectory. By intersecting the calculated trajectory interval with the end GNSS point, we estimate the optimized trajectory interval $\left[x^{\prime }\right](\overline{k})$, and then the trajectory interval $\left[x^{\prime }\right](t)$ of the FBP will be estimated.

From the introduction in Section 1 and Section 2, we know that our work not only proposes a FBP algorithm to calculate the vehicle’s trajectory but also proposes to fuse the map constraint information in the road network, which will make the interval result more accurate. In addition, our proposed algorithm is more effective for the fusion of multiple incompatible map information. Next, we will introduce the process of the FBP algorithm to fuse map constraint information. Let’s see why our method is effective for the integration of multiple incompatible information.

It can be seen from Algorithm 2 that the map information can be converted into interval data $\left[Ma{p}_c\right]$ and used as the input of the backward propagation algorithm. The optimized trajectory interval under map constraint information is obtained by intersecting the map constraint interval data with the trajectory interval obtained from Algorithm 1. Then, the trajectory interval of the FBP algorithm integrating map constraint information $\left[x_m\right](t)$ is obtained through the forward propagation and backward propagation, respectively.

Algorithm 2 FBP algorithm with map constraint information

Input: $\left[x^{\prime }\right](t)$, $\left[S\right](t)$, $\left[Ma{p}_c\right]$, $\overline{k}$,$t_m$

Output: $\left[x_m\right](t)$, $x_{tr}(t)$, $area(t)$

1    Initialize: $\left[x_m\right](t)=\varnothing$ 2    $x_{tr}(t)\leftarrow 0;$ 3    $area(t)\leftarrow 0;$ 4   $\left[x_m\right](t_m)=\left[x\right](t_m)\cap \left[Ma{p}_c\right]$//Integrated map constraint information 5   for $t=t_m$ to $\tau$ do//Interval backward propagation algorithm 6           $\left[x_m\right](t-\tau )=\left[x^{\prime }\right](t-\tau )\cap (\left[x_m\right](t)-\left[S\right](t-\tau ))$ 7  end for 8  for $t=t_m$ to $\overline{k}-\tau$ do //Interval forward propagation algorithm 9          $\left[x_m\right](t+\tau )=\left[x^{\prime }\right](t+\tau )\cap (\left[x_m\right](t)+\left[S\right](t))$ 10  end for 11  for $t=0$ to $\overline{k}$ do 12          $x_{tr}(t)=f_{mid}(\left[x_m\right](t))$ 13          $area(t)={\displaystyle \sum f_{lenth}(\left[x_m\right](t))·f_{width}(\left[x_m\right](t))}$ 14   end for

4. Experiment Data and Analysis

4.1. Experimental Environment

In this experiment, a hardware platform was built, and a moving vehicle equipped with multi-sensors and a positioning system was made for data acquisition and storage. The hardware platform is shown in Figure 1c. The motion attitude parameters are collected in real time by inertial sensors with a sampling interval of 1 s. These attitude parameters include acceleration, motion yaw angle, and velocity. Then, the positioning system is fitted to the moving target to obtain the longitude coordinate and latitude coordinate of the vehicle. The sampling time interval is 5 s to simulate the low sampling rate of GNSS information. Figure 1a shows the actual trajectory and the trajectory performed from GNSS data convert to the collected longitude and latitude coordinates by inverse cylindrical projection [28]. Figure 1b shows the actual experimental scene in Figure 1a. Because our research scenario is the movement of a vehicle on wheels, if the sampling rate is high enough, we believe that the trajectory of the target is smooth in each GNSS and IMU sampling period. It should be emphasized that the above coordinate system gathered by inverse cylindrical projection is rectangular coordinates and measured in meters. We found that the vehicle’s trajectory estimation results can be completely presented in this coordinate system.

In the above figure, the red curve represents the curve of the plane coordinates obtained by the collected GNSS coordinates through Gaussian orthographic projection [28]. The blue curve represents the actual motion trajectory of the vehicle, and the black coordinate point is GNSS coordinate point. Due to the limitation of the actual experiment, we only marked and recorded the route of the vehicle and obtained the above blue curve. From the above figure, we can see that the actual trajectory curve is an irregular prototype, and the curve generated by GNSS point simulation is also close to this shape. It can be seen that the error between the curve generated by GNSS points and the actual trajectory of the vehicle is large. Therefore, it is ineffective to use only GNSS points with a low sampling rate for trajectory recovery.

As for the simulation environment, we used MATLAB and Python for joint debugging. We preprocessed the original data in MATLAB, and the trajectory data for comparison by GNSS/IMU integrated navigation and Kalman are calculated, the trajectory interval is calculated in Python by the proposed method. To show the computational cost of the proposed FBP algorithm, we also evaluate the elapsed time of different algorithms. The trajectory recovery data is fused on a personal computer carrying an Intel i7 processor with a main frequency of 2.60 GHz.

4.2. Map Constraint

As we reviewed in Section 1 and Section 2, in most of the localization tracking and trajectory recovery articles, the authors did not consider the integration of map constraint information in the road network in the algorithm. As we emphasized, in our work, we emphasized the integration of map constraint information. More importantly, the FBP algorithm we proposed can efficiently fuse map constraint information. Map constraint information refers to the available map information in the road network, such as bridges, crossroads, and other traffic locations. The GNSS coordinates of these locations are constrained [29,30,31,32], so map information can be used for the research of trajectory recovery. We get $\left[Ma{p}_c\right]$ in Algorithm 2 by adding interval boundary to map constraint information. In addition, the motion record of the vehicle in the actual experiment is shown in Figure 1b. Map constraint information is shown in the following figure.

It can be seen from Figure 1b that our experiment is used to estimate the trajectory of the unknown area between the GNSS points. The actual trajectory points of the vehicle are manually marked. When the buzzer on the vehicle rings, it indicates that the sensor data is being uploaded to the cloud platform, and manual marking is also required at this time. In the actual road network, vehicles may run in load of the coordinates determined, such as bridges and crossroads (as shown in Figure 2), which can be considered as box-shaped areas. And the location information of these special places can be inquired and obtained from the traffic website. Therefore, the length and width of the box can be converted into an initial map information interval. The interval of the map constraint is shown in the orange area above.

In normal circumstances, the initial map information interval is inclined in the coordinate system we established (as shown in Figure 1a). Therefore, the initial map information interval needs to be mathematically modeled into components that can be fused by our algorithm. Here we take the outer minimum rectangle of the initial map information interval as our map constraint interval, and this map constraint interval will be paralleled to our coordinate system. In addition, the map constraint information we measured is located at the place of ninth sensor sampling time. The box interval of the map constraint $\left[Ma{p}_c\right]$ is measured as [(11, 13), (−10.5, −8.5)].

4.3. Performance Metrics

We shall evaluate our proposed FBP algorithm in terms of the determinacy, accuracy, and computational cost, among which the determinacy implies whether the estimated interval result contains the ground truth.

4.3.1. Determinacy

As we want to express, the first role of our work is to guarantee the actual trajectory of the vehicle exists in the interval result calculated. Then, we will show the trajectory effect obtained by using the FBP algorithm.

To illustrate the effectiveness of our method, first we get Figure 3a through the interval forward propagation algorithm, then we get Figure 3b through the FBP algorithm, and then the effect of adding map constraint information is shown in Figure 4.

In the above figure, the blue square represents the calculated track section of the vehicle, and the yellow square represents the actual track section of the vehicle. Among them, the actual trajectory interval of the vehicle is relatively small, because we can determine the actual position of the car through marking and measurement. After that, we set the manual measurement error to 0.25 m [33]. From the above figure, we can see that the blue square also presents a semicircular shape and completely contains the actual track section of the vehicle. For this reason, our method is effective.

4.3.2. Accuracy

In this section, we mainly implement Algorithm 2. To highlight the efficiency of our method, we use the DR algorithm and Kalman filter algorithm as a comparison. The DR algorithm and Kalman filter algorithm are currently considered to have a good trajectory recovery effect.

To the best of our knowledge, this paper is the first work that combines map constraint information with trajectory recovery. We introduce the experimental design in Section 4.3 as follows:

• First, we performed the vehicle’s trajectory recovery based on the DR algorithm and Kalman filter algorithm by integrating the GNSS information, which is also the method adopted by most of the current work. However, the above experiment did not include map restriction information. As a comparison, we get the vehicle’s trajectory interval based on Algorithm 1. The result comparison is shown in Figure 5, and the error analysis result is shown in Figure 6;
• Secondly, to show that our algorithm has good compatibility with map information, we also integrated map constraint information into the DR algorithm and Kalman filter algorithm. Finally, the interval results of Algorithm 2 are used for comparison. The comparison considering the map constraint is shown in Figure 7, and the error analysis results are shown in Figure 8;

Next, we will analyze and introduce Figure 5, Figure 6, Figure 7 and Figure 8. First, we will introduce the curve definitions in Figure 5 and Figure 7. The black curve $D(t)$ of the marked square is the trajectory curve based on the DR algorithm, the green curve $K(t)$ of the marked hollow circle is the trajectory curve based on the Kalman filter algorithm, and the red curve $I(t)$ of the marked triangle is the trajectory curve based on the FBP algorithm. It should be noted that the value $I(t)$ is the same as $x_{tr}(t)$ described in Algorithm 1. For clarity, we define the trajectory of the FBP algorithm as $I(t)$. In Figure 6 and Figure 8, the blue curve is the error curve between the DR algorithm and the actual vehicle’s trajectory, the cyan curve is the error curve between the Kalman filter algorithm and the actual vehicle’s trajectory, and the red curve is the error curve between the FBP algorithm and the actual vehicle’s trajectory. More importantly, we calculated the mean square error value of the above errors and marked them as blue, cyan, and red dashed lines on the figure.

We can find that the curve $I(t)$ based on the FBP algorithm is close to the actual trajectory curve $R(t)$ of the vehicle, but the curve $D(t)$ and curve $K(t)$ are significantly away from the trajectory curve $R(t)$. This phenomenon can be seen in Figure 5 and Figure 7. In particular, we can find that in Figure 6 and Figure 8, the mean error of the FBP algorithm is much smaller than that of the DR algorithm and Kalman filter algorithm. This shows the effectiveness of our proposed algorithm.

In Figure 5, the vehicle runs from the coordinates (0, 0). The DR algorithm trajectory represented by the black curve is gradually farther from the actual vehicle’s trajectory because the distance between the curve $D(t)$ and curve $R(t)$ will always increase with the accumulation of errors in the process of sensor data fusion. In the end part, the trajectory $D(t)$ and trajectory $R(t)$ are close to each other, because the DR algorithm is corrected by the GNSS coordinates. However, we find that the green Kalman filter trajectory curve $K(t)$ is distributed on both sides of the vehicle’s actual trajectory curve $R(t)$. Because the Kalman filter algorithm can solve the problem of sensor error accumulation, the trajectory will not be away from the actual trajectory. However, the Kalman filter algorithm still cannot guarantee the existence of the actual trajectory. In addition, we find that the red FBP trajectory is close to the actual trajectory of the vehicle, because our interval results not only guarantee the existence of the actual trajectory but also integrate the map constraint information. In Figure 6, we calculated the mean error of the DR algorithm, Kalman filter algorithm, and FBP algorithm respectively as 2.438 m, 1.746 m, and 0.857 m. It can be seen that the average error of the DR algorithm is the largest and the error of FBP is the smallest. This is also consistent with the phenomenon shown in Figure 5.

As for Figure 7 and Figure 8 , we mainly want to show our viewer that the FBP algorithm proposed is suitable for map information fusion. In Figure 7, we integrate map constraint information into the DR algorithm, Kalman filter algorithm, and FBP algorithm (map constraint information is not included in GNSS coordinates). The results show that the map constraint information can correct the trajectories of the three frameworks because the coordinate of the map constraint information is guaranteed. The black DR algorithm trajectory curve will continue to be offset by error accumulation. The green Kalman filter trajectory curve still has a large error with the actual trajectory. However, the proposed method is closer to the trajectory curve with the fusion of map constraints, which indicates that the global trajectory interval is constrained with the addition of map constraint information, and it is also approved in Figure 4. In addition, after integrating the map constraint information, the mean error of the DR trajectory curve, Kalman filter trajectory curve, and FBP trajectory curve is 2.090 m, 1.724 m, and 0.809 m.

It should be noted that the Kalman filter in this paper as a comparison with our proposed FBP algorithm. In addition, we use two traditional schemes, i.e., Kalman filter and DR algorithm to fuse the data of sensor information, map constraint information, and two GNSS coordinates, as a comparison with our proposed FBP algorithm. When the target is occluded, the trajectory estimation is implemented by integrating two pieces of information; one is the IMU sensor information, and the other is the map information. When the GNSS signal is received, the vehicle will correct the current trajectory. Therefore, in the experimental scenario proposed in this paper, the DR algorithm, the Kalman filter and the FBP algorithm are all suitable for trajectory estimation. However, Kalman’s linear assumption will reduce the estimation accuracy as shown in Figure 6 and Figure 8, whereas the FBP algorithm can make full use of IMU sensor data and map information, and recover the track immediately after GNSS signal recovery.

In this paragraph, we mainly introduce the relevant details in Figure 6 and Figure 8. First of all, we need to understand that there are five sensor sampling points between every two GNSS samples. From the first sec to the 10th sec, the errors of the DR algorithm and Kalman filtering algorithm are increasing. There is a GNSS correction sampling point at the sixth sec. However, the GNSS correction is invalid because the vehicle has just started and the signal of the GNSS receiver is unstable. Then, in the 11th, 16th, 21st, and 26th sec, we can find obvious GNSS correction effects. In the actual measurement, sensor acquisition will also be unstable due to the impact of initialization at the beginning of the vehicle. At the 15th sec, the data acquisition was interrupted due to the interference of the vehicle during operation. Therefore, the FBP algorithm model, DR algorithm model, and Kalman filter model interfere and peaks appear in the above two stages.

Last but not least, through Section 4.3, we verified the topic we explained. Through the introduction and analysis from Figure 5, Figure 6, Figure 7 and Figure 8, we concluded that the proposed fusion map constraint information is beneficial to the completion of trajectory recovery. By comparing Figure 5 and Figure 6, we integrate the map constraint information with the minimum error, which shows the superiority of integrating map information. By comparing Figure 7 and Figure 8, we propose that the error of integrating map constraint information by the FBP algorithm is the smallest, which shows that our algorithm has a better effect on map information fusion. However, the Kalman filtering curve is distributed on both sides of the actual trajectory curve and the error is large because the estimation effect of the Kalman filter for the nonlinear model is not good. It is easy to lose the target when the correction effect is unavailable. This is also consistent with our actual experimental results. To sum up, the two metrics of our work have been successfully implemented.

4.3.3. Interval Area and Computational Cost

To indicate our metrics of accuracy and convenience clearly, we print out the area change and calculation time change of the proposed algorithm at different stages and after fusing different positioning information. The comparison of interval area and computing time is shown in

Table 2. Interval area size and computing cost.

Accuracy	Interval-Based Forward Propagation Algorithm	Interval-Based Forward–Backward Propagation Algorithm	FBP Algorithm Integrated GNSS Information
The Total Area of Interval	2938.34 m2	1648.82 m2	1441.16 m2
Interval Area Average	48.17 m2	27.02 m2	23.63 m2
Computing Cost of The Program	0.0334681 s	0.0331482 s	0.034207 s

. We performed the mean of area $area(t)$ calculated in Algorithms 1 and 2 and obtained the following area data.

It can be seen from the above table that, after the interval based forward–backward propagation algorithm is adopted, the interval area error is reduced, so the accuracy is increased, and the trajectory interval is more determined. In addition, we can also see that there is little time change in the stage of the interval forward propagation algorithm, the interval forward–backward propagation algorithm, and the FBP algorithm integrated GNSS information, which can be accurate to the millisecond level. Therefore, we can be sure that the proposed framework has the advantages of convenience and quickness. Just to make it clear, the IMU sensor information, map constraint information, and satellite measurement data are conveniently integrated through the FBP in this paper and obtain definite results compared with previous trajectory recovery studies. This also shows that our algorithm is effective in adaptive system optimization. In the actual situation, there will be more location optimization and constraint information on the road, so our algorithm can be fused with more road location information to make the final interval result more and more accurate.

The required computational time of the proposed FBP method is 0.04001 s, whereas the time of the traditional GNSS/IMU combined framework and the Kalman filter framework is 0.0504 s and 0.0623 s, respectively. Although the effectiveness improvement is not sufficient enough, the computational cost of the proposed method is acceptable.

5. Conclusions

In this paper, for the GNSS/IMU fusing tracking scenario, we proposed an interval forward and backward propagation algorithm to get an interval trajectory result, which determinately contains the ground truth. As a benchmark, the raw GNSS/IMU fusion method, i.e., the above DR algorithm and the traditional Kalman filter-based positioning algorithm are also implemented to recover trajectory for comparison, which shows that the error accumulation problem in nonlinear multi-source information fusion. The framework proposed in this paper can solve the above problems and the actual trajectory of the moving target is guaranteed to be within the range of the estimated interval results. Therefore, the trajectory interval can be obtained through the proposed interval forward and backward propagation algorithm. More importantly, the effectiveness and convenience of the proposed FBP algorithm on the multi-source information fusion are proven by integrating the interval map constraint information.