COLLABORATIVE NAVIGATION SIMULATION TOOL USING KALMAN FILTER WITH IMPLICIT CONSTRAINTS

Collaborative Positioning (CP) is a networked positioning technique in which different multi-sensor systems (nodes) enhance the accuracy and precision of these navigation solutions by performing measurements or by sharing information (links) between each other. The wide spectrum of available sensors that are used in these complex scenarios bring the necessity to analyze the sensibility of the system to different configurations in order to find optimal solutions. In this paper, we discuss the implementation and evaluation of a simulation tool that allows us to study these questions. The simulation tool is successfully implemented as a plane based localization problem, in which the sensor measurements are fused in a Collaborative Extended Kalman Filter (C-EKF) algorithm with implicit constraints. Using a real urban scenario with three vehicles equipped with various positioning sensors, the impact of the sensor configuration is investigated and discussed by intensive Monte Carlo simulations. The results show the influence of the laser scanner measurements on the accuracy and precision of the estimation, and the increased performance of the collaborative navigation techniques with respect to the single vehicle method.


INTRODUCTION & RELATED WORK
The number of users relying on positioning, navigation and timing (PNT) information increased significantly in the last years (Kealy et al., 2015). However, navigation solutions based on only standalone Global Navigation Satellite System (GNSS) navigation provide the user often with an inadequate solution, in which the GNSS signal is affected by multipath, diffraction and obstruction (Hofmann-Wellenhof et al., 2008). Inertial Measurement Unit (IMU) based solutions, as the commonly known as strapdown navigation solution (Groves (2013), Jwo et al. (2014)), deteriorates with time due to drifts caused by sensor biases. The use of coupling techniques (Petovello, 2003) enables to obtain a solution that provides time stability and that does not fully rely on the environment characteristics. Furthermore, approaches often discussed in robotics, make use of vision sensors (e.g. laser scanner (Brenner, 2016), camera (Coenen et al., 2017), etc.) to compute their relative position with respect to the different elements of the environment (Thrun et al., 2006), i.e. traffic lights, urban furniture or facade planes (Taguchi et al. (2013), Servant et al. (2010) or Weingarten and Siegwart (2005)).
The growth and development of wireless communication, computing and sensing capabilities, along with the ability to recognize and model the environment and vehicle's position, allow the multi-sensor systems (MSS) to navigate interconnected, describing a so-called collaborative/networked navigation scenario. Collaborative navigation techniques (de Ponte-Müller (2017) or Kealy et al. (2015)) are discussed to overcome the ranging sensor limitations (e.g. perception range and/or field of view obstruction) (de Ponte-Müller et al., 2016) and prove to be powerful coping with rank deficiency issues in GNSS challenging/denied areas (Lee et al. (2012) or Berefelt et al. (2004)). Another approach that overcomes the ranging sensor limitations can be found in Koppanyi et al. (2018), where a networked based navigation approach is designed using Ultra-Wideband (UWB) measurements to establish an ad-hoc network. Additional collaborative approaches using laser scanner and stereo cameras are presented in Fox et al. (1999) and in Knuth and Barooah (2009), respectively. Kokuti et al. (2017) show the different types of communication architecture for collaborative systems.
The utilization of simulation in the design process of engineering components and algorithms is widely extended. In the case of collaborative scenarios, the development of a simulation tool helps to cost-effectively and safely reproduce a wide spectrum of realistic situations in order to evaluate the performance of different sensor setups and configurations under different environmental scenarios. Simulation environments as the ones designed by Giroux et al. (2003), Jwo et al. (2014) or Gade (2005) represent valid simulation and post-processing methodologies for positioning and inertial measurements, although no environmental characteristics are considered. Zhang et al. (2018) use 3D city models in order to overcome the GNSS limitations in urban canyons using a collaborative approach. In Tideman and van Noort (2013) a comparison between PreScan and ITS Modeller commercial simulation tools for connected vehicle systems is carried out. PreScan consists on a simulation tool which allows the user to model the vehicle dynamics, sensor measurements, V2X communications (Kokuti et al., 2017), environment characteristics and weather conditions to evaluate the connected vehicle systems. ITS Modeller is a traffic simulator, that copes with the analysis of navigation scenarios involving thousands of vehicles. In Garcia-Fernandez and Schön (2018) we present, an in-house developed simulation tool that enables and assists the modelling of vehicle dynamics, sensor measurements and environment characteristics. Here, the multi-sensor system measurements are fused in a Collaborative-Linearized Kalman Filter (C-LKF) where each ray to plane intersection computed is treated as a landmark in which the coordinates are fully known. This paper presents a simulation tool suitable to evaluate the performance of different sensor combinations and configurations using a Collaborative-Extended Kalman Filter (C-EKF) in vehicular collaborative scenarios. Given that the tool provides different possibilities to simulate the vehicle dynamics and environmental characteristics, it is possible to analyze the performance of different sensor fusion strategies in any desired situation. This improves simulation approaches such as those implemented by Giroux et al. (2003), Jwo et al. (2014) or Gade (2005) -which only consider the vehicle dynamics' simulation-, or filter approaches such as Vogel et al. (2018), where only one vehicle is considered. In addition, the collaborative navigation approach using GNSS baselines to link the vehicles allows us to estimate the state parameters from each vehicle simultaneously in the same coordinate frame (unlike the approach presented by de Ponte-Müller et al. (2016), where the relative position and velocity of the observed vehicles are estimated in the ego-vehicle navigation frame). Furthermore, the approach presented in Garcia-Fernandez and Schön (2018) is advanced from a C-LKF to a C-EKF in order not to carry out the linearization process with respect to the true values of the state parameters (not known in real time applications). In Garcia-Fernandez and Schön (2018), each laser scanner point (result from the ray to 3D city model plane intersection) is assumed to be a landmark with known coordinates, leading to overoptimistic results. Here, we tackle this problem by assuming that the laser scanner point coordinates are unknown but they must belong to a plane, leading towards the adaptation of the Gauß-Markov Model to the presence of hard constrained equations as in Dang et al. (2009 (more information in Section 2.4.2). The overall filter performance depending on the type and noise of the observations or/and from changing the measurements configurations is investigated. To achieve this goal, Monte Carlo techniques are applied (Alkhatib et al., 2008;Gentle, 2003).
The rest of the paper is structured as follows: in Section 2 we introduce the structure and design of the simulation tool, in Section 3 we evaluate the results obtained from the different Monte Carlo simulations carried out, ending the paper with a conclusions section.

SIMULATION TOOL: STRUCTURE & DESIGN
The design is divided into four steps: generation of trajectories, environmental model selection, generation of measurements and sensor fusion algorithm.

Generation of Trajectories
In our tool, trajectories (attitude, position, velocity, angular rates and acceleration) can be generated depending on the target of the study. The following three different approaches are implemented: 1. Starting from velocity and attitude time series (Giroux et al. (2003) or Jwo et al. (2014)) 2. Starting from waypoints (coordinate time series) that are interpolated using cubic spline in order to achieve a smooth trajectory and velocity information, (Garcia-Fernandez and Schön (2017)). The remaining kinematic variables can be obtained following the approach of Giroux et al. (2003).
3. Using trajectories obtained with real data.
In this study, we follow approach 3 using trajectories of three vehicles (cf. Figure 2) obtained from the measurement campaign carried out by the i.c.sens . The trajectory of vehicle 1 was obtained by tight coupling a Novatel SPAN-SE GNSS receiver with iMAR IMU, and those of vehicles 2 & 3 from loosely coupled solution provided directly by a LORD Mi-croStrain IMU. They contain a wide range of maneuvers (acceleration, breaking, turning, etc.). The area of the city in which the experiment was performed consists of an urban area with urban canyons and areas with higher satellite visibility where the impact of different satellite geometries can be analyzed.

Environmental model selection
The simulation of the scenario includes the selection of the environmental model. The geometrical representation of the environment, constrain the measurement generation and therefore the state estimation in the sensor fusion algorithm.
The environmental model digitization using an external tool represents the first option for 2D applications (Garcia-Fernandez and Schön, 2017). However, the availability of 3D City Models with Level of Detail (LoD-2) (Digitales 3D Stadtmodell, 2017) or prealigned laser scanner point clouds obtained from the i.c.sens measurement campaign , open the possibility to realize a much more realistic scenario.
In this study, we evaluate the simulation tool by using the 3Dcity model with LoD-2 (cf. Fig. 1). In addition, the environmental model can help us to identify the GNSS line-of-sight and non-line-of-sight signal reception by checking possible intersections with the buildings as in the case of the represented vehicle with Satellite-4 (S-4) (cf. Fig. 1). Further improvements can be obtained by adding vegetation, e.g., from airborne LIDAR.

Measurement Generation
In this simulation tool, we consider that the vehicles are equipped with GNSS receivers, IMUs, laser scanner and stereo cameras. For simplicity, we do not consider cameras in this article. The generated measurements are: obtained from a GNSS receiver by measuring the pseudorange and the Doppler range rate at any epoch k (Hofmann-Wellenhof et al., 2008). If multiple vehicles measures the carrier phase from the same satellite, baselines between the vehicles can be computed (V2V measurements).
• IMU measurements: Applying the strap-down navigation algorithm (Groves, 2013) to accelerations and angular rates, the position, velocity and attitude, respectively, can be computed.
• Laser scanner measurements: The laser scanner measurements are computed by performing the intersection between a simulated laser scanner ray and the planes used to represent the buildings in the 3D city model as in Figure 1.
In this paper, the velocity and attitude measurements are assumed to be affected by white Gaussian noise in order to evaluate the effect of the measurement geometry on the state estimation. Nevertheless, the GNSS measurements error is modeled as a function of the Position Dilution Of Precision (PDOP) extracted from the GNSS measurements covariance (only GPS constellation considered) (Hofmann-Wellenhof et al., 2008) in order to introduce the effect of the satellite geometry on the measurement generation (complete loss of signal situations are not considered in this study).

Collaborative-Extended Kalman Filter (C-EKF) with implicit constraints
The sensor measurement fusion algorithm selected in this paper is the C-EKF with implicit constraints. In this section, the reasons why the filter was selected and its characteristics are discussed.
The filter operate by implementing the dynamic system equations that describe the propagation of the state mean and the covariance of the states (Prediction Step). In a second step (Update/Correction Step), the state mean and covariance are updated in every epoch after receiving the measurements described in Section 2.3.

Prediction
Step: Assume the following system: where Φ k−1 is the time discrete state transition matrix, x k is the unknown state vector, l k is the observation vector, H k is the design matrix for the measurements, w k−1 and v k are the process noise (modelled as random ramp) and measurement noise vectors (respectively), and Qd k−1 is the process noise covariance matrix. The predicted state (neglecting the control to simplify the system) can be computed as: The selected state vector for each vehicle is expressed as: where N, E and U are the North, East and Up components of the vehicle position, vN , vE and vU are the three components of the velocity in the previously indicated directions and φ, θ and ψ are the three attitude components (roll, pitch and yaw), respectively. Then, the state transition matrix, covariance matrix and process noise covariance matrix for collaborative approaches can be found in Garcia-Fernandez and Schön (2018). We selected a fixed constant velocity motion model in order to simplify the analysis, although decreases in the performance on rapid turning or heavy breaking maneuvers are expected as a result. This will be considered in the next version of the simulation software.

Observation vector and noise generation:
The observations generated in the filter are simulated as a function of the predicted states adding random observation noises generated from the normal distrubution given in Eq.2. Three different types of observations (GNSS, IMU and laser scanner) are generated (refer The GNSS and IMU observations are assumed to be preprocessed. Consequently we include in the filter the directly observed states (position velocity and attitude) as affected only by white Gaussian noise using the values displayed in Table 1.
The given nominal pose of every vehicle is used to implement the origin and geometry of the laser scanner measurements, using the selected environmental model as a target for the generated laser scanner rays (blue circles and cyan lines in Fig. 1). These are generated using a threshold of 40m in order to eliminate measurements that would exceed a certain distance to prevent from including in the simulation unrealistic measurements. Then, the simulated rays are intersected with the planes that form the 3D city model to obtain the error-free laser scanner observations.
Given the previously described geometry, the laser scanner observation in the sensor frame can be divided in three components: horizontal angle, vertical angle and Euclidean distance between the intersected point and the laser scanner position. We assume that the laser scanner is perfectly calibrated, so the horizontal and vertical angles remain unchanged. Therefore, the Euclidean distance is the only component that is introduced in the filter as laser scanner observation.
Improving the use of intersections as individual landmarks as in Garcia-Fernandez and Schön (2018), we introduce the plane equation obtained from the geometrical elements used to build the 3D city model as prior knowledge in order to take complex object recognition in the building facades into account. This way, we are able to identify which rays intersect with which plane, and therefore we also know that no matter the amount of noise that affects the observations, the point should belong to the plane and must fulfill the following condition equality: where n is the known normal vector (n = [a b c]), p veh is the vehicle's position in the local topocentric frame (t-frame), C t b is the rotation matrix from the body frame (b-frame) to the t-frame (attitude), C b s and t b s are the leverarm (s-frame to b-frame) parameters obtained from the platform calibration, dist is the measured Euclidean distance between the intersected point and the laser scanner in the s-frame, C(α, ) is the ray direction matrix which contains the fixed horizontal and vertical angles of the rays (α and , respectively), and d completes the set of coefficients for the plane's Hesse normal form.
Eq. 6 represents a hard constraint and the measurement model can be written as: h (x k , l k ) ! = 0.
As it was stated in section 1, the laser scanner measurements show some outliers caused by reflections. At this state of the study, outliers are not considered. In order to compensate this, the number of laser scanner measurements considered in the simulation is considerably lower than as if they would have been imported from a real laser scanner point cloud.

Correction
Step: Since the prediction step depends only on the system model selected, the prediction step remains unaltered. Then, the update step is implemented as in Dang et al. (2009. In order to perform the update step, we need to differentiate between the measurements that fulfill equation 2 and the ones that fulfill equation 7. Therefore, three Jacobian matrices are computed: Afterward, the matrices have to be concatenated in order to be able to perform the update for all measurement types in only one step.
where R LS,k is the covariance matrix for the laser scanner measurements.
Then the K k andx + k are computed as: In order to propagate the covariance states we apply:

Simulation setup
We use the data collected in the i.c.sens measurement campaign  in the simulation (cf. Section 2.1). In this paper, we select a representative scenario that takes place in an urban area, where three vehicles approach an intersection simultaneously. Furthermore, the simulation requires the definition of the measurement noise standard deviation to be used within the filter as well as the power spectral density (PSD) for the process noise estimation (Table 2). These values were intentionally not optimized for the specific measurement setup and trajectory dynamic in order to show the power of the simulation tool to identify critical situations and next help to optimize the parameters.
In order to evaluate the previously described algorithm, two actions were carried out: 1. Run scenarios using different combinations of measurements 2. Evaluate the effect of the selected random vectors used for the measurement noise on the filter by using Monte Carlo simulations. In this paper, four different sensor setups for the scenarios are presented. Each of them run 1000 times using in each run a different but fixed random vector for generating the observation noise. Thus, we can validate the filter and evaluate the effect or the random vector in the noise generation process.

Overall Monte Carlo simulation analysis
In each run, the deviations of the estimated trajectory (x + k ) with respect to the reference trajectory and the standard deviation (σ) obtained from P + k are computed and stored for each epoch. Consequently, the Root Mean Squared Error (RMSE) and mean standard deviation (σ) can be computed for every run in order to analyze the accuracy and precision of the system, respectively.  Table 2) of vehicle 1. Please note the different scales of the plots.

Analysis of the maximum sensor setup
In order to get insights in the effect of the different observations, as well as the effect of the environment characteristics on the mean state and covariance propagation we take a closer look at the recursively estimates of every epoch over time obtained in each run of the Monte Carlo (MC) simulation.
The plots used to display the results from the MC simulation are structured as follows. We depict the 1000 runs of the MC simulation as a raster grid in which the values represented on the x-axis are the epochs and the values represented on the y-axis are the run number. Then, the grid is color coded by deviations with respect to the true trajectory (accuracy). Hence, every row of the grid represents the time series evolution of the parameter used to color code it. This gives information about the state of the runs at each epoch. In addition, we use the right y-axis of the plot to show the mean values of the deviations with respect to the reference and standard deviation of the state parameters. Figure 4 shows the 3D deviation of the estimated trajectory with respect to the reference trajectory for vehicle 1 using sensor setup 1. In addition, the mean 3D deviation (RMSE) over all runs is indicated as red solid line. The overall range of values is between 0 m and 2 m, depending on the specific convergence. The mean RMSE reaches ca. 0.30 m. Figure 4 shows also that every generated time series has a different behavior sometime leading to large deviations (columns containing dark blue tone and yellow  Fig. 2) where variations common to all time series are visible. This is due to a trajectory part in an urban canyon, where the vehicle is turning (compare with Fig.5, where the gradients of the heading and norm of the horizontal velocity are depicted in order to show turning and acceleration/breaking maneuvers) and the positioning estimation is ruled by the motion model used in the prediction step. Afterwards, the filter needs a few epochs to converge again to the previously indicated ca 0.30 m of deviation. This is an expected behavior, shown as a result of the use a constant velocity and constant attitude motion model in the prediction step, vulnerable to breaking and turning maneuvers. The position deviations are different for its three components, which we will analyze next. First, at the planimetric level, the implementation of a plane based update step with the laser scanner lead to a larger uncertainty of the longitudinal direction (forward direction of driving) than in the lateral direction in the urban canyons where the facades are almost parallel to the direction of driving (see Figure 6).
In addition, it is possible to appreciate that the most important decreases in the accuracy of the transverse direction are due to the changes in the heading (ψ) shown in figure 5. This effect is also shown in the heading state parameter estimation (yaw angle in Figure 7) where the heading changes at epochs 187s, and 277-283 s (cf. Figure 5).
The Up coordinate of the estimated position is only updated with GNSS measurements (position and baselines) and velocity measurements. Figure 8 shows that the estimation converges or diverges depending on the error of the measurements. This is due to the DOP value used as standard deviation of the GNSS measurements, showing lower accuracy in covered areas as between epochs 80 -110 s (urban canyon, cf. Fig 2 and Fig. 9).
It is important to note that in figures 4 to 10 (with exception of figures 5 and 9) the grid describe column patterns. This is an indication that changes in the random vector used for the noise generation have almost no effect on the state estimation. The exceptions are figures 4 and 8 also due to the influence of the GNSS geometry.
In figure 10 we show the time series of the Horizontal Dilution of Precision (HDOP) values in relation to the planimetric standard deviations obtained from the estimation. Here, the impact of the laser scanner can be clearly appreciated, since the lower the HDOP value, the more precise is the estimation. It can also be seen that in the places with a suitable measurement geometry for the estimation, the values obtained are in the range of 0.02-0.03 meters.

Analysis of varying sensor setups
The analysis of the different sensor setups together with the selected noise properties allows to identify the impact of different measurement types as well as the noise properties on the results. In the second setup, we run the filter with the same conditions and random vectors as with the sensor setup 1, but we neglect the absolute GNSS positioning measurements for the computation. As it was stated before, the plane based algorithm using laser scanner impacts crucially the planimetric coordinate estimation, but has no effect on the Up coordinate. Thus, the absence of GNSS has a minimum impact on the North and East coordinate estimations (cf. Figure 11). However, it has a significant influence in the Up direction, where the estimation -even under bad GNSS geometry -constraints the overall accuracy of the 3D estimation (cf. Fig. 12).  Figure 12 shows the deviation of the estimated trajectory in the Up coordinate with respect to the reference trajectory for vehicle 1 using sensor setup 2. In addition, the mean deviation (RMSE) over all runs is indicated as red solid line. Here, the individual random vector show a significant influence, specially in the last one hundred epochs where the rows can be found in yellow or blue tones. This can be understood as the convergence capacities of the filter, where if the error becomes too big at some point of the trajectory, it cannot be compensated with the rest of the measurements (considering the standard deviations in table 1). This underlines the need of GNSS measurements and of an adaptation of the filter settings depending on the used sensor setups.
The obtained standard deviations are ruled mainly by the geometry and the laser scanner measurements. Thus the differences with respect to scenario 1 are minimum and thus not explicitly shown here. In absence of laser scanner measurements (sensor setup 3), figure 13 shows the 2D deviation of the estimated trajectory with respect to the reference trajectory for vehicle 1. By comparing this results with Fig. 6 it is possible to appreciate the impact of the laser scanner. As stated before, the main impact of this sensor is in the lateral direction, showing a clear improvement specially in the urban canyons. The laser scanner update shows also a positive effect on the longitudinal direction estimation, although this is not as big as in the lateral direction during the whole trajectory (cf. Fig. 6 and Fig.13, bottom panels). This happens because the urban canyon it is not only formed by planes that lay parallel to the direction of driving. These non-parallel facades normally are smaller than the others in the 3D city model and thus less laser scanner rays are intersected with them. As a result, these measurements are not enough to overcome the sensitivity of the estimation to the accelerating/breaking maneuvers (cf. Figure 5, epoch ca 210 s). This way an increase in accuracy is obtained but not as significant as in the lateral direction. Consequently, the changes on the heading in quite symmetric geometries in areas with low number of efficient laser scanner measurements (cf. Figure 5) produce an increase in the mean deviations.

Impact of collaboration
With the fourth scenario displayed in Table 2, we perform a first rough evaluation of the impact of the collaborative approach with respect to the single vehicle approach by neglecting the V2V baseline measurements. Figure 14 depicts the mean 3D RMSE of the estimated trajectory with respect to the reference trajectory for vehicle 1. Comparing the collaborative setup 1 (green line) with the individual single vehicle case (setup 4, magenta line) the impact is visible.
The collaborative approach improves the accuracy of the system mainly in the Up as well as the standard deviation (not shown

Collaborative vs Single Vehicle Approach 3D
Collaborative mean RMSE Single Vehicle mean RMSE Figure 14. Collaborative approach (Sensor setup 1) VS Single Vehicle approach (Scenario 4). RMSE here). It can be seen from figure 14 that the state estimation, as well as the amount of improvement depends on the GNSS measurement standard deviation (obtained from the GNSS geometry) in relation to the other on-board sensors' standard deviation. In addition, the collaborative approach is translated into a larger number of measurements and a strengthened geometry which increase the precision of the system. Further studies are needed to fully exploit this impact of collaboration.

CONCLUSIONS
The paper introduces a simulation tool for evaluating and analyzing complex collaborative navigation scenarios and finding optimal sensor configurations, observation as well as process noise settings.
The utilization of a C-EKF with implicit constraints is a suitable algorithm for the state estimation of parameters using the configuration described on section 3.1. Here, the most accurate results are shown in the lateral direction to the direction of driving in urban canyons using all available sensors. In addition, laser scanner measurements geometry and the vehicles' turning maneuvers have a strong influence on the accuracy and precision of the estimation.
Under the studied configurations, the GNSS measurements show a large influence on the update of the up direction of the vehicles. The highly precise velocity measurements are not enough to update the estimation in the indicated direction without the GNSS measurements. This highlights the importance of the GNSS measurements in 3D estimations. The absence of the laser scanner measurements produces a significant decrease in the accuracy and precision of the estimation, specially in the North and East coordinates. Finally, collaboration increases the accuracy of the system. The amount of improvement depends on the GNSS measurements standard deviation in relation with the other on-board sensors' standard deviations (that at the same time depends on the GPS PDOP).
Future studies will be directed towards simulations with a higher number of vehicles as well as to the tuning of the processing parameters and noise properties in order to find optimal filter solutions in all setups.