Vehicle infrastructure cooperative localization using Factor Graphs

Highly assisted and Autonomous Driving is dependent on the accurate localization of both the vehicle and other targets within the environment. With increasing traffic on roads and wider proliferation of low cost sensors, a vehicle-infrastructure cooperative localization scenario can provide improved performance over traditional mono-platform localization. The paper highlights the various challenges in the process and proposes a solution based on Factor Graphs which utilizes the concept of topology of vehicles. A Factor Graph represents probabilistic graphical model as a bipartite graph. It is used to add the inter-vehicle distance as constraints while localizing the vehicle. The proposed solution is easily scalable for many vehicles without increasing the execution complexity. Finally simulation indicates that incorporating the topology information as a state estimate can improve performance over the traditional Kalman Filter approac

Cooperative localization for mobile agents: a recursive decentralized algorithm based on Kalman filter decoupling

We consider cooperative localization technique for mobile agents with communication and computation capabilities. We start by provide and overview of different decentralization strategies in the literature, with special focus on how these algorithms maintain an account of intrinsic correlations between state estimate of team members. Then, we present a novel decentralized cooperative localization algorithm that is a decentralized implementation of a centralized Extended Kalman Filter for cooperative localization. In this algorithm, instead of propagating cross-covariance terms, each agent propagates new intermediate local variables that can be used in an update stage to create the required propagated cross-covariance terms. Whenever there is a relative measurement in the network, the algorithm declares the agent making this measurement as the interim master. By acquiring information from the interim landmark, the agent the relative measurement is taken from, the interim master can calculate and broadcast a set of intermediate variables which each robot can then use to update its estimates to match that of a centralized Extended Kalman Filter for cooperative localization. Once an update is done, no further communication is needed until the next relative measurement

Split Covariance Intersection Filter Based Visual Localization With Accurate AprilTag Map For Warehouse Robot Navigation

Accurate and efficient localization with conveniently-established map is the fundamental requirement for mobile robot operation in warehouse environments. An accurate AprilTag map can be conveniently established with the help of LiDAR-based SLAM. It is true that a LiDAR-based system is usually not commercially competitive in contrast with a vision-based system, yet fortunately for warehouse applications, only a single LiDAR-based SLAM system is needed to establish an accurate AprilTag map, whereas a large amount of visual localization systems can share this established AprilTag map for their own operations. Therefore, the cost of a LiDAR-based SLAM system is actually shared by the large amount of visual localization systems, and turns to be acceptable and even negligible for practical warehouse applications. Once an accurate AprilTag map is available, visual localization is realized as recursive estimation that fuses AprilTag measurements (i.e. AprilTag detection results) and robot motion data. AprilTag measurements may be nonlinear partial measurements; this can be handled by the well-known extended Kalman filter (EKF) in the spirit of local linearization. AprilTag measurements tend to have temporal correlation as well; however, this cannot be reasonably handled by the EKF. The split covariance intersection filter (Split CIF) is adopted to handle temporal correlation among AprilTag measurements. The Split CIF (in the spirit of local linearization) can also handle AprilTag nonlinear partial measurements. The Split CIF based visual localization system incorporates a measurement adaptive mechanism to handle outliers in AprilTag measurements and adopts a dynamic initialization mechanism to address the kidnapping problem. A comparative study in real warehouse environments demonstrates the potential and advantage of the Split CIF based visual localization solution

A Survey on Multisensor Fusion and Consensus Filtering for Sensor Networks

Multisensor fusion and consensus filtering are two fascinating subjects in the research of sensor networks. In this survey, we will cover both classic results and recent advances developed in these two topics. First, we recall some important results in the development ofmultisensor fusion technology. Particularly, we pay great attention to the fusion with unknown correlations, which ubiquitously exist in most of distributed filtering problems. Next, we give a systematic review on several widely used consensus filtering approaches. Furthermore, some latest progress on multisensor fusion and consensus filtering is also presented. Finally, conclusions are drawn and several potential future research directions are outlined.the Royal Society of the UK, the National Natural Science Foundation of China under Grants 61329301, 61374039, 61304010, 11301118, and 61573246, the Hujiang Foundation of China under Grants C14002 and D15009, the Alexander von Humboldt Foundation of Germany, and the Innovation Fund Project for Graduate Student of Shanghai under Grant JWCXSL140

A multi-hypothesis approach for range-only simultaneous localization and mapping with aerial robots

Los sistemas de Range-only SLAM (o RO-SLAM) tienen como objetivo la construcción de un mapa formado por la posición de un conjunto de sensores de distancia y la localización simultánea del robot con respecto a dicho mapa, utilizando únicamente para ello medidas de distancia. Los sensores de distancia son dispositivos capaces de medir la distancia relativa entre cada par de dispositivos. Estos sensores son especialmente interesantes para su applicación a vehículos aéreos debido a su reducido tamaño y peso. Además, estos dispositivos son capaces de operar en interiores o zonas con carencia de señal GPS y no requieren de una línea de visión directa entre cada par de dispositivos a diferencia de otros sensores como cámaras o sensores laser, permitiendo así obtener una lectura de datos continuada sin oclusiones. Sin embargo, estos sensores presentan un modelo de observación no lineal con una deficiencia de rango debido a la carencia de información de orientación relativa entre cada par de sensores. Además, cuando se incrementa la dimensionalidad del problema de 2D a 3D para su aplicación a vehículos aéreos, el número de variables ocultas del modelo aumenta haciendo el problema más costoso computacionalmente especialmente ante implementaciones multi-hipótesis. Esta tesis estudia y propone diferentes métodos que permitan la aplicación eficiente de estos sistemas RO-SLAM con vehículos terrestres o aéreos en entornos reales. Para ello se estudia la escalabilidad del sistema en relación al número de variables ocultas y el número de dispositivos a posicionar en el mapa. A diferencia de otros métodos descritos en la literatura de RO-SLAM, los algoritmos propuestos en esta tesis tienen en cuenta las correlaciones existentes entre cada par de dispositivos especialmente para la integración de medidas estÃa˛ticas entre pares de sensores del mapa. Además, esta tesis estudia el ruido y las medidas espúreas que puedan generar los sensores de distancia para mejorar la robustez de los algoritmos propuestos con técnicas de detección y filtración. También se proponen métodos de integración de medidas de otros sensores como cámaras, altímetros o GPS para refinar las estimaciones realizadas por el sistema RO-SLAM. Otros capítulos estudian y proponen técnicas para la integración de los algoritmos RO-SLAM presentados a sistemas con múltiples robots, así como el uso de técnicas de percepción activa que permitan reducir la incertidumbre del sistema ante trayectorias con carencia de trilateración entre el robot y los sensores de destancia estáticos del mapa. Todos los métodos propuestos han sido validados mediante simulaciones y experimentos con sistemas reales detallados en esta tesis. Además, todos los sistemas software implementados, así como los conjuntos de datos registrados durante la experimentación han sido publicados y documentados para su uso en la comunidad científica

A Brief Tutorial On Recursive Estimation With Examples From Intelligent Vehicle Applications (Part I): Basic Spirit And Utilities

Estimation is an indispensable process for an ocean of applications, which are rooted in various domains including engineering, economy, medicine, etc. Recursive estimation is an important type of estimation, especially for online or real-time applications. In this brief tutorial, we will explain the utilities, the basic spirit, and some common methods of recursive estimation, with concrete examples from intelligent vehicle applications

Cooperative Perception for Social Driving in Connected Vehicle Traffic

The development of autonomous vehicle technology has moved to the center of automotive research in recent decades. In the foreseeable future, road vehicles at all levels of automation and connectivity will be required to operate safely in a hybrid traffic where human operated vehicles (HOVs) and fully and semi-autonomous vehicles (AVs) coexist. Having an accurate and reliable perception of the road is an important requirement for achieving this objective. This dissertation addresses some of the associated challenges via developing a human-like social driver model and devising a decentralized cooperative perception framework. A human-like driver model can aid the development of AVs by building an understanding of interactions among human drivers and AVs in a hybrid traffic, therefore facilitating an efficient and safe integration. The presented social driver model categorizes and defines the driver\u27s psychological decision factors in mathematical representations (target force, object force, and lane force). A model predictive control (MPC) is then employed for the motion planning by evaluating the prevailing social forces and considering the kinematics of the controlled vehicle as well as other operating constraints to ensure a safe maneuver in a way that mimics the predictive nature of the human driver\u27s decision making process. A hierarchical model predictive control structure is also proposed, where an additional upper level controller aggregates the social forces over a longer prediction horizon upon the availability of an extended perception of the upcoming traffic via vehicular networking. Based on the prediction of the upper level controller, a sequence of reference lanes is passed to a lower level controller to track while avoiding local obstacles. This hierarchical scheme helps reduce unnecessary lane changes resulting in smoother maneuvers. The dynamic vehicular communication environment requires a robust framework that must consistently evaluate and exploit the set of communicated information for the purpose of improving the perception of a participating vehicle beyond the limitations. This dissertation presents a decentralized cooperative perception framework that considers uncertainties in traffic measurements and allows scalability (for various settings of traffic density, participation rate, etc.). The framework utilizes a Bhattacharyya distance filter (BDF) for data association and a fast covariance intersection fusion scheme (FCI) for the data fusion processes. The conservatism of the covariance intersection fusion scheme is investigated in comparison to the traditional Kalman filter (KF), and two different fusion architectures: sensor-to-sensor and sensor-to-system track fusion are evaluated. The performance of the overall proposed framework is demonstrated via Monte Carlo simulations with a set of empirical communications models and traffic microsimulations where each connected vehicle asynchronously broadcasts its local perception consisting of estimates of the motion states of self and neighboring vehicles along with the corresponding uncertainty measures of the estimates. The evaluated framework includes a vehicle-to-vehicle (V2V) communication model that considers intermittent communications as well as a model that takes into account dynamic changes in an individual vehicle’s sensors’ FoV in accordance with the prevailing traffic conditions. The results show the presence of optimality in participation rate, where increasing participation rate beyond a certain level adversely affects the delay in packet delivery and the computational complexity in data association and fusion processes increase without a significant improvement in the achieved accuracy via the cooperative perception. In a highly dense traffic environment, the vehicular network can often be congested leading to limited bandwidth availability at high participation rates of the connected vehicles in the cooperative perception scheme. To alleviate the bandwidth utilization issues, an information-value discriminating networking scheme is proposed, where each sender broadcasts selectively chosen perception data based on the novelty-value of information. The potential benefits of these approaches include, but are not limited to, the reduction of bandwidth bottle-necking and the minimization of the computational cost of data association and fusion post processing of the shared perception data at receiving nodes. It is argued that the proposed information-value discriminating communication scheme can alleviate these adverse effects without sacrificing the fidelity of the perception