Cooperative Localization Problem Research Articles

A survey and evaluation of iterative optimization algorithms for cooperative localization

Cooperative localization (Co-localization) refers to the establishment of an objective function through relative measurements and obtaining the relative position information by solving the objective function, and is a typical optimization problem. The current research of cooperative localization is mainly the innovative application of specific algorithms in specific problems. It lacks the summary of typical optimization strategies, the comparative analysis of coordinate transformation strategies, and the evaluation criteria of positioning performance. This paper summarizes the design ideas, applicable conditions and detailed procedures of typical algorithms in the cooperative localization problem, and compares the accuracy, convergence speed and computational complexity of different algorithms through simulation experiments; summarizes two methods of coordinate transformation, and analyzes the performance of these methods through simulation experiments; summarizes Cramer–Rao lower bound (CRLB) of co-localization problem under the condition that relative measurement is unbiased estimation, and gives the specific steps to calculate CRLB under different spatial dimensions.

Moving-Landmark Assisted Distributed Learning Based Decentralized Cooperative Localization (DL-DCL) with Fault Tolerance

This paper considers the problem of cooperative localization of multiple robots under uncertainty, communicating over a partially connected, dynamic communication network and assisted by an agile landmark. Each robot owns an IMU and a relative pose sensing suite, which can get faulty due to system or environmental uncertainty, and therefore exhibit large bias in their estimation output. For the robots to localize accurately under sensor failure and system or environmental uncertainty, a novel Distributed Learning based Decentralized Cooperative Localization (DL-DCL) algorithm is proposed that involves real-time learning of an information fusion strategy by each robot for combining pose estimates from its own sensors as well as from those of its neighboring robots, and utilizing the moving landmark's pose information as a feedback to the learning process. Convergence analysis shows that the learning process converges exponentially under certain reasonable assumptions. Simulations involving sensor failures inducing around 40-60 times increase in the nominal bias show DL-DCL's estimation performance to be approximately 40% better than the well-known covariance-based estimate fusion methods. For the evaluation of DL-DCL's implementability and fault-tolerance capability in practice, a high-fidelity simulation is carried out in Gazebo with ROS2.

Distributed decentralized EKF for very large-scale networks with application to satellite mega-constellations navigation

The design of a decentralized and distributed filtering solution for large-scale networks of interconnected systems is addressed considering (i) generic nonlinear dynamics and (ii) generic coupled nonlinear outputs in a generic, possibly time-varying, topology. The local filters, which follow the structure of the extended Kalman filter, are implemented in each system, which estimates its own state exclusively. To be suitable for the heavily restricted implementation to very large-scale systems, a novel algorithm is proposed, which: (i) does not rely on instantaneous data transmission; (ii) allows local communication exclusively; and (iii) requires computational, memory, and data transmission resources for each system that do not scale with the dimension of the network. The scalability of the proposed algorithm allows for its application to the cooperative localization problem of very large-scale systems. In particular, it is applied herein to the on-board position estimation problem of LEO mega-constellations using GNSS featuring numerical simulations for the Starlink constellation.

Fast IPPM for Distributed Cooperative Localization With Hybrid Information

This letter addresses the problem of distributed cooperative localization using the joint range and angle measurements. With the appealing properties of fully distributed implementation and low complexity at each agent, a convex formulation named fast iterative parallel projection method (FastIPPM) is exploited to solve the cooperative localization problem. In pursuit of faster convergence, gradient descent with momentum is utilized. Simulation results validate the superior performance of the proposed algorithm compared with existing ones. After adopting the acceleration techniques, the FastIPPM shows a <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"> <tex-math notation="LaTeX">$2\times$ </tex-math></inline-formula> – <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"> <tex-math notation="LaTeX">$3\times$ </tex-math></inline-formula> speed-up. In addition, positioning accuracy and convergence stability in sparse networks have also been greatly enhanced.

Cooperative Localization in Mobile Robots Using Event-Triggered Mechanism: Theory and Experiments

This article addresses a new cooperative localization problem for a team of mobile robots subject to limited communication resources. First, we develop a decentralized event-triggered cooperative localization (DECL) algorithm for multirobot system such that each robot localizes itself with minimum communication exchange between robots. Then, using an event-triggered mechanism we propose an optimization framework to achieve a balance between estimation performance and communication rate. Simulation results show the main benefits of the event-triggered mechanism. Also, experimental results using four e-puck2 mobile robots demonstrate the effectiveness of the proposed method. Note to Practitioners—Multiple mobile robots are able to implement certain tasks that are beyond the capabilities of individual robots. In multirobot system, the accurate localization of each robot in the team is essential for a successful operation. Existing cooperative localization approaches neglect some realistic limitations of mobile robots, such as battery capacity and communication bandwidth. Especially, this issue is important when the number of sensors, actuators, and robots in the team increases. This article was motivated by these realistic limitations of mobile robots and it suggests a new approach for cooperative localization based on event-triggered mechanism. Motived by the aforementioned discussion, our objective is to design and implement the event-triggered cooperative localization for a group of e-puck2 robots. Our theoretical analysis and experimental results show that we achieve a tradeoff between localization accuracy and communication resources.

Cooperative Localization Based on Augmented State Belief Propagation for Mobile Agent Networks

Belief propagation (BP) is widely used to solve the cooperative localization problem due to its excellent performance and natural distributed structure of implementation. For a mobile agent network, its factor graph inevitably encounters loops. In this case, the BP algorithm becomes iterative and can only provide an approximate marginal probability density function of the estimate with finite iterations. We propose an augmented-state BP algorithm for mobile agent networks to alleviate the effect of loops. By performing state augmentation, the messages in the factor graph will actually be allowed to be backward propagated, which reduces the number of loops in the factor graph, increases the available information of agents, and thus, benefits the localization. Experimental results demonstrate the better performance of the proposed algorithm over the original BP method.

Simplify Belief Propagation and Variation Expectation Maximization for Distributed Cooperative Localization

Only a specific location can make sensor data useful. The paper presents an simplify belief propagation and variation expectation maximization (SBPVEM) algorithm to achieve node localization by cooperating with another target node while lowering communication costs in a challenging environment where the anchor is sparse. A simplified belief propagation algorithm is proposed as the overall reasoning framework by modeling the cooperative localization problem as a graph model. The high-aggregation sampling and variation expectation–maximization algorithm is applied to sample and fit the complicated distribution. Experiments show that SBPVEM can obtain accurate node localization equal to NBP and SPAWN in a challenging environment while reducing bandwidth requirements. In addition, the SBPVEM has a better expressive ability than PVSPA, for SBPVEM is efficient in challenging environments.

Fault Tolerant Multi-Robot Cooperative Localization Based on Covariance Union

This paper studies the multi-robot cooperative localization (CL) problem, a challenging scenario in which robots may receive spurious sensor data, potentially causing inconsistent state estimates. To address this problem, this paper presents a fully decentralized CL algorithm based on covariance union (CU), referred to as DCL-CU. The proposed approach is fault-tolerant and supports generic measurement models. Extensive Monte Carlo simulations and a group of real-world experiments were conducted to verify the performance of the proposed DCL-CU approach. The results show that the DCL-CU approach can efficiently deal with spurious sensor data.

Cooperative Localization of 3D Mobile Networks via Relative Distance and Velocity Measurement

This letter mainly considers the cooperative localization problem of 3D mobile cluster network. With the development of Doppler frequency shift measurement technique, high-precision relative velocity measurement has become possible. In this letter, we first incorporate the relative velocity information into the 3D cooperative localization framework. Then the quasi-Newton algorithm BFGS is used to solve the framework, and the vBFGS algorithm suitable for solving the relative position of the mobile cluster network is constructed. Simulation experiments show that using relative velocity measurement to assist positioning can significantly improve positioning accuracy. Compared with the state-of-the-art algorithm, vBFGS has higher positioning accuracy and faster convergence rate for low-connectivity networks, and has better numerical stability and robustness for high-speed moving models.

Parametric Variational Sum-Product Algorithm for Cooperative Localization in Wireless Sensor Networks

Communication overhead is a key task for the large-scale wireless sensor networks (WSNs) in the cooperative localization problems. This paper is aimed to propose a low-overhead algorithm without sacrificing the performance of localization, which is more practical for energy-saving WSNs. In this paper, the cooperative localization problem is formulated as a variational reference problem on a factor graph (FG). Combining the high performance of particle-based algorithms and the overhead advantage of parametric ones, a parametric variational sum-product algorithm (PVSPA) is proposed for the cooperative localization of WSNs. In prediction operation, to ensure high accuracy, messages referenced inside sensor nodes are particle-based. In correction operation, Gaussian parametric representation is adopted to the observation message and the communication overhead is obviously decreased. Simulation results show that the localization accuracy of PVSPA equally matches the classic particle-based algorithms, while the communication overhead of it is far lower.

Cooperative Navigation Using Pairwise Communication with Ranging and Magnetic Anomaly Measurements

The problem of cooperative localization for a small group of unmanned aerial vehicles (UAVs) in a Global Navigation Satellite System-denied environment is addressed in this paper. The presented approach contains two sequential steps: first, an algorithm called cooperative ranging localization, formulated as an extended Kalman filter, estimates each UAV’s relative pose inside the group using intervehicle ranging measurements; second, an algorithm named cooperative magnetic localization, formulated as a particle filter, estimates each UAV’s global pose through matching the group’s magnetic anomaly measurements to a given magnetic anomaly map. In this study, each UAV is assumed to only perform a ranging measurement and data exchange with one other UAV at any point in time. A simulator is developed to evaluate the algorithms with magnetic anomaly maps acquired from airborne geophysical survey. The simulation results show that the average estimated position error of a group of 16 UAVs is approximately 20 m after flying about 180 km in 1 h. Sensitivity analysis shows that the algorithms can tolerate large variations of velocity, yaw rate, and magnetic anomaly measurement noises. Additionally, the UAV group shows improved position estimation robustness with both high- and low-resolution maps as more UAVs are added into the group.

Cooperative Multiple Target Nodes Localization Using TOA in Mixed LOS/NLOS Environments

In this paper, the time-of-arrival based cooperative localization problem under mixed line-of-sight/non-line-of-sight (LOS/NLOS) conditions is addressed. Under the conditions of unknown statistics of NLOS errors and unknown path status, we formulate a non-convex robust weighted least squares (RWLS) problem to locate the multiple stationary target nodes. In this problem, we introduce some “balancing parameters” to alleviate the possible improper upper bounds problem of the existing robust localization methods, and jointly estimate the target node positions and the balancing parameters. With the aid of the semidefinite relaxation technique, the RWLS problem is transformed into a convex mixed semidefinite and second order cone programming problem, which can be solved efficiently. The idea is then extended to the moving target nodes localization problem, where the velocity is assumed to be fixed within a short time period. Both simulated and real experimental data are used to verify the performance of the proposed method. The results show that the proposed method works well for both the sparse and dense NLOS environments, and also confirm the superior performance of the proposed method over the existing methods.

Cooperative Localization for Multiple Soccer Agents Using Factor Graphs and Sequential Monte Carlo

This paper addresses the cooperative localization problem for a multiagent system in the framework of belief propagation. In particular, we consider the RoboCup 3D Soccer Simulation scenario, in which the networked agents are able to obtain simulated measurements of the distance and bearing to both known landmarks and teammates as well as the direction of arrival (DOA) of messages received from allies around the field. There are, however, severe communication restrictions between the agents, which limit the size and periodicity of the information that can be exchanged between them. We factorize the joint probability density function of the state of the robots conditioned on all measurements in the network in order to derive the corresponding factor graph representation of the cooperative localization problem. Then we apply the sum-product-algorithm (SPA) and introduce suitable implementations thereof using hybrid Gaussian-Mixture Model (GMM) / Sequential Monte Carlo (SMC) representations of the individual messages that are passed at each network location. Simulated results show that the cooperative estimates for position and orientation converge faster and present smaller errors when compared to the non-cooperative estimates in situations where agents do not observe landmarks for a long period.

Cloud-Assisted Cooperative Localization for Vehicle Platoons: A Turbo Approach

Due to the high resolution of angles of arrivals (AoAs) provided by the massive MIMO base station in 5 G wireless systems, it is promising to integrate 5G-based localization technology into autonomous driving to improve the accuracy and robustness of vehicle localization. In this paper, we investigate the problem of 5G cloud-assisted cooperative localization for vehicle platoons. The existing 5G-based localization algorithms focused on single-user localization and are not efficient for the localization of vehicle platoon where the positions of the vehicles are highly correlated. To the best of our knowledge, cloud-assisted cooperative localization tailored to vehicle platoons has not been studied before. To address this challenging problem, we first propose a Gamma-Markov-Group-Sparse (GMGS) model to capture the joint distribution of the vehicle positions in a vehicle platoon. Then we formulate the vehicle platoon cooperative localization as a sparse Bayesian inference (SBI) problem. The existing standard SBI algorithms such as variational Bayesian inference (VBI) and approximate message passing (AMP) cannot be applied to our platoon localization problem due to the complicated GMGS prior and the ill-conditioned measurement matrix. As such, we propose a novel turbo vehicle platoon cooperative localization (Turbo-VPCL) algorithm to fully exploit the correlations of the vehicle positions (as captured by the GMGS prior) under the ill-conditioned measurement matrix. Simulation results verify that the proposed Turbo-VPCL can achieve significant gain over the-state-of-art SBI algorithms.

Fault-tolerant Covariance Intersection for localizing robot swarms

This paper examines the important problem of cooperative localization in robot swarms, in the presence of unmodeled errors experienced by real sensors in hardware platforms. Many existing methods for cooperative swarm localization rely on approximate distance metric heuristics based on properties of the communication graph. We present a new cooperative localization method that is based on a rigorous and scalable treatment of estimation errors generated by peer-to-peer sharing of relative robot pose information. Our approach blends Covariance Intersection and Covariance Union techniques from distributed sensor fusion theory in a novel way, in order to maintain statistical estimation consistency for cooperative localization errors. Experimental validation results show that this approach provides both reliable and accurate state estimation results for Droplet swarms in scenarios where other existing swarm localization methods cannot.

A Bayesian Algorithm for Distributed Network Localization Using Distance and Direction Data

A reliable, accurate, and affordable positioning service is highly required in wireless networks. High-resolution estimates of distance and direction data are available in most current and emerging wireless systems. Combining these two sensing modalities can improve the estimation performance and identifiability of the cooperative localization problem, and reduce its sensitivity to the geometry of anchor nodes, i.e., the reference nodes with known locations. However, this is still an open and challenging research problem. In this paper, the novel Message Passing Hybrid Localization (MPHL) algorithm is proposed to solve the problem of cooperative distributed localization using joint distance and direction estimates without any prior information. A statistical model is formulated for the problem, and approximate minimum mean square error (MMSE) estimates of the node locations are computed. The proposed MPHL algorithm is a distributed technique based on belief propagation and Markov chain Monte Carlo sampling. Numerical results are presented showing that the average localization error is significantly reduced in almost every simulation scenario, about 50% in most cases, compared to the state of the art. This improvement in localization performance is due to close approximation of a statistically optimal MMSE estimator.

Cooperative Localization Based on Efficient Covariance Intersection

This work proposes a geometric covariance intersection (CI)-based algorithm for efficiently solving the decentralized cooperative localization (CL) problem with multiple TOA measurements. Unlike existing algorithms that only process one measurement at a time, our proposed algorithm processes multiple measurements in a batch style, which improves the precision performance by considering the correlation among these measurements. An efficient way to get the optimal solution is given in our algorithm, which reduces the computational complexity to $\mathcal {O}(N \log N)$ . Furthermore, the sparsity of the optimal solution is proved, which enables a heuristic state buffering approach to reduce the usage of ranging link resources and improve measurement efficiency. The simulation results show that our algorithm outperforms the conventional methods.

Bayesian Cooperative Localization Using Received Signal Strength With Unknown Path Loss Exponent: Message Passing Approaches

We propose a Bayesian framework for the received-signal-strength-based cooperative localization problem with unknown path loss exponent. Our purpose is to infer the marginal posterior of each unknown parameter: the position or the path loss exponent. This probabilistic inference problem is solved using message passing algorithms that update messages and beliefs iteratively. To enable the numerical tractability, we combine the variable discretization and Monte-Carlo-based numerical approximation schemes. To further improve computational efficiency, we develop an auxiliary importance sampler that updates the beliefs with the help of an auxiliary variable. To sample from a normalized likelihood function, which is an important ingredient of the proposed auxiliary importance sampler, we develop a stochastic sampling strategy that mathematically interprets and corrects an existing heuristic strategy. The proposed message passing algorithms are analyzed systematically in terms of computational complexity, demonstrating the computational efficiency of the proposed auxiliary importance sampler. Various simulations are conducted to validate the overall good performance of the proposed algorithms.

Cloud-based map alignment strategies for multi-robot FastSLAM 2.0

The cooperative simultaneous localization and mapping problem has acquired growing attention over the years. Even though mapping of very large environments is theoretically quicker than a single robot simultaneous localization and mapping, it has several additional challenges such as the map alignment and the merging processes, network latency, administering various coordinate systems and assuring synchronized and updated data from all robots and also it demands massive computation. This article proposes an efficient architecture for cloud-based cooperative simultaneous localization and mapping to parallelize its complex steps via the multiprocessor (computing nodes) and free the robots from all of the computation efforts. Furthermore, this work improves the map alignment part using hybrid combination strategies, random sample consensus, and inter-robot observations to exploit fully their advantages. The results show that the proposed approach increases mapping performance with less response time.

Cooperative Localization in Hybrid Infrared/Visible Light Networks: Theoretical Limits and Distributed Algorithms

Light emitting diode (LED) based visible light positioning networks can provide accurate location information in environments where the global positioning system (GPS) suffers from severe signal degradation and/or cannot achieve high precision, such as indoor scenarios. In this paper, we propose to employ cooperative localization for hybrid infrared/visible light networks that involve multiple LED transmitters having known locations (e.g., on the ceiling) and visible light communication (VLC) units equipped with both LEDs and photodetectors (PDs) for the purpose of cooperation. In the considered scenario, downlink transmissions from LEDs on the ceiling to VLC units occur via visible light signals, while the infrared spectrum is utilized for device-to-device communications among VLC units. First, we derive the Cramér-Rao lower bound and the maximum likelihood estimator (MLE) for the localization of VLC units in the proposed cooperative scenario. To tackle the nonconvex structure of the MLE, we adopt a set-theoretic approach by formulating the problem of cooperative localization as a quasiconvex feasibility problem, where the aim is to find a point inside the intersection of convex constraint sets constructed as the sublevel sets of quasiconvex functions resulting from the Lambertian formula. Next, we devise two feasibility-seeking algorithms based on iterative gradient projections to solve the feasibility problem. Both algorithms are amenable to distributed implementation, thereby avoiding high-complexity centralized approaches. Capitalizing on the concept of quasi-Fejér convergent sequences, we carry out a formal convergence analysis to prove that the proposed algorithms converge to a solution of the feasibility problem in the consistent case. Numerical examples illustrate the improvements in localization performance achieved via cooperation among VLC units and evidence the convergence of the proposed algorithms to true VLC unit locations in both the consistent and inconsistent cases.