Evaluating an EKF Simulation tool for Collaborative Navigation Systems
Nicolas Garcia Fernandez and Steffen Schön, Insitut für Erdmessung, Leibniz Universität Hannover, Germany
In the advent of autonomous navigation, the development of new techniques for positioning becomes a hot topic in the research community. Within these new techniques, Collaborative Positioning (CP) is a challenging positioning technique, in which a group of dynamic nodes (pedestrians, vehicles, etc.) equipped with different (time synchronized) sensors can increase the quality of the Positioning, Navigation and Timing (PNT) information by exchanging navigation information as well as performing measurements between nodes or to elements of the environment (urban furniture, buildings, etc.). The robustness of positioning is supposed to increase, describing an improvement in accuracy, integrity, continuity and availability terms compared to single node positioning, like e.g. standalone GNSS or tightly coupled GNSS + IMU solutions. On the one hand side, such a configuration can be considered as a dynamic sensor network in which some of the nodes are changing their position and in which the links between nodes are defined by measurements carried out with additional sensors (V2X measurements). On the other hand, using the findings from geodetic network analysis and optimization the whole picture of the sensor network quality can be evaluated. However, the traditional concepts and methodologies developed for geodetic network optimization do not consider the possibility that a node could move describing a trajectory. Thus, new concepts for dynamic sensor network optimization and evaluation have to be developed.
In this paper, we describe the development of a realistic simulation tool for collaborative 3D navigation systems and its validation with real data. Satellite navigation, inertial navigation, laser scanner and photogrammetry techniques are combined. This way, algorithms capable of fusing different sensor measurements for localization or positioning purposes are provided, which enables evaluating the correlations and dependencies of estimated parameters and the localizability in the network. In addition, critical or highly redundant observations can be detected, and the network improvement can be evaluated by adding dedicate observations. In addition, a simulation tool adds flexibility to pre-analyze specific scenarios to invent new test drives or to analyze a specific phenomenon not present in the set of real data.
The fact that several multi-sensor systems are included in the navigation situation, brings the necessity of a sensor measurement fusion algorithm. Here, the simulation algorithm is implemented as an Extended Kalman Filter (EKF), since it allows us to model the system dynamics (velocity, acceleration, attitude, angular rates, etc.) described by continuous-time differential equations, defining the control and measurement signals (laser scanner, stereo-camera, etc.) as discrete time equations. This allows the time discrete representation of the dynamic network. The snapshot in every epoch is treated with the traditional concepts for geodetic network analysis and optimization (namely datum problem, First Order Design problem, weight problem). The response of the existing methodologies will be evaluated, providing the path to follow for the development of new techniques for geometry and topology optimization in dynamic sensor networks.
Methodologies for vehicle detection/modeling and landmark detection (V2V and V2I measurements) using laser scanner and stereo cameras are implemented. The landmarks will be represented as planes (namely facades of buildings, walls, etc.). Subsequently, the parameter estimation is extended by the respective plane equation. As a result, dynamic node parameters and the landmarks are simultaneously estimated in a special type of collaborative SLAM algorithm.
Furthermore, 3D city model with Level of Detail 2 (LoD2), freely available for Hannover, is used in the EKF to improve the robustness of the adjustment. The building models are provided in WGS84/UTM Zone 32 projection with an estimated accuracy for the vertex coordinates of around 0.5m. In the first steps of the implementation, the plane equations can be extracted from the LoD2 building models, providing us with the features target for the line to plane intersection of the V2I measurement simulation with laser scanner. In addition, a realistic GNSS visibility for the simulation is derived.
A correct modeling of the different observation error sources plays a crucial role in the simulation design. The different relative observation weights regulate the impact of each type of observation in the fusion filter. In the simulation, typical performance classes are considered for every sensor type, based on product specifications.
The current 2D implementation of our simulation algorithm allows specification of various vehicles equipped with different sensors. Realistic trajectories are generated based on attitude and forward velocity in the body frame. As input serves digitalized waypoints. During the simulation run, the following performance metrics are computed: the geometric strength of the positioning problem is evaluated by DOP values, the changes in precision and accuracy when combining different sensors is treated by coordinate standard deviation as well as RMS obtained from simulated observation noise. The ability of the network to detect outliers and thus deliver more robust estimation is expressed in terms of minimum detectable biases.
In the current paper we will show the results when extending the simulation to true 3D. The convergence in accuracy and precision of the state parameters under a collaboration situation, the minimum number of V2V measurements and/or V2I necessary to produce this situation, the increase in the reliability (integrity) and the minimum set of sensors needed to analyze a specific target are some of the large spectrum of findings using the approach proposed in this paper.
Finally, the simulation tool is validated using real data from a measurement campaign carried out in the framework of the research training group ICSens. On it, simultaneous measurements were carried out with different sensors (Javad GNSS receivers, Novatel SPAN SE receiver, iMAR and MicroStrain IMUs, Velodyne laser scanner and GrassHopper 3.0. stereo cameras) rigidly mounted on three moving vehicles in the area of Hannover, describing different collaborative navigation scenarios. Exemplary, we will focus in a scenario in which the three vehicles describe a trajectory of around 1.5 Km in an urban area, repeating the same circuit four times (around 20 minutes’ duration). On it, the vehicles were forced to meet each other several times in an intermediate intersection (area of interest for collaboration purposes) but collecting useful and continuous data during the whole time of the scenario. This data is also used to evaluate the integrity, precision, accuracy and reliability of the system.
A better understanding of dynamic networks will improve the integrity, precision and accuracy of the existing navigation algorithms, showing the most of its power in GNSS challenging or GNSS denied areas.