Resilient Cooperative Localization Based on Factor Graphs for Multirobot Systems

To improve the accuracy of multiple target tracking in the clutter environment, a new joint probabilistic data association (JPDA) algorithm based on variational Bayesian adaptive moment estimation is proposed. First, considering the existence of measurements, the posterior distribution of the target state in JPDA is composed of two parts of probability weighting, that is, the posterior distribution of the target state that the real measurement exists in the association gate and the posterior distribution of the target state that the real measurement does not exist in the association gate. By combining the conjugate properties of the prior and posterior distributions, the prior distributions of the target state in the two cases are classified to provide more accurate a priori information to filter, so as to improve the accuracy of data association. Second, considering the coupling effect between state estimation and data association process, combined with variational Bayesian inference, the problem of minimizing Kullback–Leibler divergence is transformed into the problem of maximizing the evidence lower bound, thereby effectively measuring the distance between the posterior distribution of target state estimation and the real posterior distribution, so as to improve the accuracy of data association again from the perspective of optimizing nonlinear filter. Finally, the adaptive momentum estimation strategy is introduced to iteratively solve the variable distribution that meets the maximization of the evidence lower bound, and the optimization of the posterior distribution of the target state is completed. Theoretical derivation and simulation experiments are conducted to verify the feasibility and effectiveness of the algorithm.

A single-sensor, single-target mixture reduction (MR) data association algorithm is extended for use in multisensor, multitarget tracking situations. MR is extended for tracking an arbitrary number of targets using an arbitrary number of sensors under the assumption that the sensor measurement errors are independent across sensors. Like the single-sensor, single-target MR algorithm, which gives better performance than the probabilistic data association (PDA) filter, the multisensor, multitarget MR extensions give similar improvements in performance compared to the joint PDA (JPDA) and multisensor JPDA (MSJPDA) algorithms. Further, in the formulations for the multisensor and multitarget MR algorithms, the equations for the calculation of the data association probabilities have been put in the same form as for the JPDA, thus allowing previously developed fast JPDA computational techniques to be applicable. N the problem of tracking targets in random clutter, the optimal Bayesian solution leads to Gaussian mixture distributions that consist of an exponentially increasing number of components.1 In practice, the number of components in each mixture is kept in check by approximating it with fewer components. The nearest neighbor (NN) approximation reduces each target's mixture to its largest component.13 The joint probabilistic data association (JPDA) approximation merges the components of each target's mixture into a single Gaussian component.1'4 However, when there are several significant, well-spaced components in the original mixture, important information may be lost by using the NN or JPDA methods. A more flexible approximation method for single-sensor, single-target tracking, known as mixture reduction (MR), has been proposed by Salmond.5 This MR algorithm preserves the mean and covariance of the original mixture of components, successively merging components until the number of components is reduced to some userspecified limit. In this paper, we first review Salmond's MR algorithm for single-sensor tracking of a single target in clutter. We then derive extensions to Salmond's MR algorithm for use in tracking multiple targets and when there are measurements available from multiple sensors. Just as the single-sensor, single-target MR algorithm gives better performance5 than the probabilistic data association (PDA) filter, the multisensor, multitarget MR extensions are shown to give similar improvements in performance as compared to the JPDA and multisensor JPDA (MSJPDA)6 algorithms. Extension of the MR algorithm for tracking of multiple targets is necessary for use in realistic tracking scenarios where there are usually many targets that need to be tracked. Moreover, many current tracking systems have been designed with multiple sensors, thus requiring the further extension to a multisensor algorithm. The paper is organized as follows. In Sec. II, the target dynamics and measurement equations are defined. We review the singlesensor, single-target MR algorithm5 in Sec. Ill, and we extend the algorithm to the multitarget and multisensor cases in Sec. IV and V, respectively. In Sec. VI, simulation results evaluating the performances of the MR extensions are presented. Finally, concluding remarks are given in Sec. VII.

Algorithms are presented for managing sensor information to reduce the effects of bias when tracking interacting targets. When targets are close enough together that their measurement validation gates overlap, the measurement from one target can be confused with another. Data association algorithms such as the joint probabilistic data association (JPDA) algorithm can effectively continue to track targets under these conditions, but the target estimates may become biased. A modification of the covariance control approach for sensor management can reduce this effect. Sensors are chosen based on their ability to reduce the extent of measurement gate overlap as judged by a set of heuristic parameters derived in this work. Monte Carlo simulation results show that these are effective methods of reducing target estimate bias in the JPDA algorithm when targets are close together. An analysis of the computational demands of these algorithms shows that while they are computationally demanding, they are not prohibitively so.

This paper proposes a cooperative positioning (CP) algorithm based on distributed modified sigma point belief propagation. Range measurement collected from the raw global navigation satellite systems (GNSSs) and ultra wide-band (UWB) data can be used for vehicle localization in urban areas under non-light-of-sight (NLOS) conditions. In order to alleviate the drawbacks of previous methods in handling non-Gaussian noise in NLOS and particularly in GNSS challenging environments, a newly range measurement error model is developed. The designed range measurement error model is based on a well-tailored non-Gaussian distribution for representing noise error in NLOS conditions. The CP problem is transformed into Bayesian inference on factor graph, which relies on a novel version of sigma point belief propagation (SPBP). The proposed CP algorithm is referred to hybrid cooperative non-Gaussian randomized sigma-point belief propagation (HC-ngR-SPBP). By using a nonlinear approximation based on randomized sigma points and stochastic integration rule (SIR), the HC-ngR-SPBP can: reduce the number of sigma point and solve a low computational cost the nonlinear measurement function exhibited by UWB and GNSS devices. The mean and the covariance calculated by the SIR are used to generate the non-Gaussian ranging probability density functions via the asymmetric generalized Gaussian mixture model. Simulation results conducted using various iterations with with real-time position information and range errors of 2 and 4 m demonstrate that the proposed algorithm outperforms the standard SPBP and nonparametric belief propagation and is very well suited for vehicle localization.

Multiple target tracking (MTT) has received much attention recently for various applications in the military as well as the Strategic Defense Initiative areas. Data association is one of the critical computation in MTT problems, because erroneous data associations often result in lost tracks. The joint probabilistic data association (JPDA) algorithm is a good approach to solve the data association problem. However, the computation complexity of this algorithm increases rapidly with the number of targets and radar returns. Neural networks have been considered to approximate the JPDA and ease the computation burden through the parallel processing. In this paper, we propose a neural network data association (NNDA) algorithm for the solution of the data association problems. Simulation results show the following three controversial issues: first, NNDA can track multiple targets with performance compatible with JPDA. Second, when the prediction filter can not provide an enough accurate prediction data ( This sometimes occurs when the prediction model can not match the tracking environment precisely enough), the neural computation provides better performace than JPDA. Third, the performance of NNDA is not affected by the number while that of JPDA degrades with the increase of target number. As a whole, this paper presents a neural network which not only possesses the intrinsic ability of parallel computation but also provides conditionally better tracking performance than JPDA.

It is a challenging task to develop an effective multi-target tracking algorithm for camera networks due to the factors, such as spurious measurement, limited field of view, complexity of data association algorithm, and so on. In a real-life environment, the system model for camera networks is usually nonlinear, and hence, a Kalman filter may be inappropriate for modeling this system. Besides, the main drawback of traditional joint probabilistic data association (JPDA) is prone to raise the combinatorial explosion problem when the association probabilities have to be calculated. To solve these problems, a multi-target square-root cubature information weighted consensus filter (MTSCF) combined with a K-best joint probabilistic data association algorithm is proposed in this paper. The proposed K-MTSCF algorithm can not only reduce the effect of data association uncertainty stemming from the ambiguity of measurements and computation complexity of data association algorithm by K-best JPDA, but also increase tracking accuracy and stability using MTSCF algorithm. The experimental results demonstrate that the proposed approach performs favorably against the state-of-the-art methods in terms of accuracy and stability for tracking multiple targets in camera networks.

The availability of global navigation satellite systems (GNSS) on consumer devices has caused a dramatic change in every-day life and human behaviour globally. Although GNSS generally performs well outdoors, unavailability, intentional and unintentional threats, and reliability issues still remain. This has motivated the deployment of other complementary sensors in such a way that enables reliable positioning, even in GNSS-challenged environments. Besides sensor integration on a single platform to remedy the lack of GNSS, data sharing between platforms, such as in collaborative positioning, offers further performance improvements for positioning. An essential element of this approach is the availability of internode measurements, which brings in the strength of a geometric network. There are many sensors that can support ranging between platforms, such as LiDAR, camera, radar, and many RF technologies, including UWB, LoRA, 5G, etc. In this paper, to demonstrate the potential of the collaborative positioning technique, we use ultra-wide band (UWB) transceivers and vision data to compensate for the unavailability of GNSS in a terrestrial vehicle urban scenario. In particular, a cooperative positioning approach exploiting both vehicle-to-infrastructure (V2I) and vehicle-to-vehicle (V2V) UWB measurements have been developed and tested in an experiment involving four cars. The results show that UWB ranging can be effectively used to determine distances between vehicles (at sub-meter level), and their relative positions, especially when vision data or a sufficient number of V2V ranges are available. The presence of NLOS observations is one of the principal factors causing a decrease in the UWB ranging performance, but modern machine learning tools have shown to be effective in partially eliminating NLOS observations. According to the obtained results, UWB V2I can achieve sub-meter level of accuracy in 2D positioning when GNSS is not available. Combining UWB V2I and GNSS as well V2V ranging may lead to similar results in cooperative positioning. Absolute cooperative positioning of a group of vehicles requires stable V2V ranging and that a certain number of vehicles in the group are provided with V2I ranging data. Results show that meter-level accuracy is achieved when at least two vehicles in the network have V2I data or reliable GNSS measurements, and usually when vehicles lack V2I data but receive V2V ranging to 2–3 vehicles. These working conditions typically ensure the robustness of the solution against undefined rotations. The integration of UWB with vision led to relative positioning results at sub-meter level of accuracy, an improvement of the absolute positioning cooperative results, and a reduction in the number of vehicles required to be provided with V2I or GNSS data to one.

In order to solve the problem that it is difficult to achieve better multi-robot coordination operation using the original control system in multi-robot cooperative systems, this paper conducts system modeling, control framework design and software development and verification experiments for multi-robot cooperative systems. We firstly model the system composed of multiple robots and operated objects. The coordination between any two robots can be divided into two categories: loose coordination and tight coordination according to the constraint relationship between the robots and the objects, based on which the kinematics and dynamics analysis of the multi-robot system is carried out. A multi-robot cooperative control framework is designed, which contains modules for task planning, force and position assignment, sensor information acquisition, position/force control and synchronization clock. On the basis of the multi-robot cooperative control system, the robot cooperative control software is designed and developed, and the software system contains, with excellent characteristics. Finally, an experimental environment containing three large-load EFORT industrial robots was established and experiments were conducted using the developed multi-robot cooperative control software to verify the usability and reliability of the software system.

Cooperative positioning (CP), which fuses the position-related information of a group of vehicles, is a promising technique to enhance the performance of Global Navigation Satellite Systems (GNSS)-based methods for vehicular positioning in urban environments. With the development of communication techniques, more vehicles can be accessed to build the CP cluster. However, current CP techniques are mainly concentrated on the positioning methods and lack analysis of cooperator selection, and existing node selection methods only consider the ranging information between nodes. In this paper, to build a multi-vehicle CP cluster considering GNSS information, the Cramer-Rao lower bound (CRLB) based on differential GNSS and Ultra-Wide Band (UWB) CP framework is derived, and we demonstrate that the satellite observations have an important impact on CP performance. In order to comprehensively consider the impact of GNSS and UWB on CP cluster, we extend the Greedy search and generalized Breiman, Friedman, Oishen, and Stone (GBFOS) algorithm to select cooperators. Simulation results show that a CP cluster with better performance can be built, compared to only considering the ground ranging.

An important step in multitarget tracking (MTT) is the assignment of measurement/target pairing probabilities, i.e., the probability that a certain measurement is associated with a particular target. This is one of the more computationally intensive steps of the Joint Probabilistic Data Association (JPDA) and other MTT algorithms. Determining such assignment probabilities in the conventional JPDA algorithm involves the generation and analysis of all possible binary feasibility matrices, where each feasibility matrix represents the feasible event that each measurement is due to only one target (or clutter) and that each target causes at most one measurement. We discuss a new and most efficient technique for obtaining the measurement/target pairing probabilities directly (without first producing the feasibility matrices) and its implementation on a new analog optical computer.

The data fusion can come down to a process that combines the state vectors from different sources to obtain a more accurate result. Compare to the achieved results that depend on single source, the method has gained an improved performance and reduced the computational complexity and bandwidth of transmission as well. This paper makes use of Probabilistic Data Association (PDA) algorithm and Joint Probabilistic Data Association (JPDA) algorithm to track the Multi-target for each local sensor in a clutter environment. Furthermore, a method based on statistical double-threshold association algorithm and covariance-weighted fusion algorithm is proposed in this paper. Meanwhile, the simulation result shows that the performance has been improved significantly in multi-sensor and multi-target tracking progress with the proposed method in the paper.

The global navigation satellite system (GNSS) positioning accuracy is highly degraded due to signal reflections from buildings. A stand-alone receiver only has limited measurement capabilities, thus resulting in limited performance. With the rapid development of communications techniques, sharing measurements between receivers have become possible. Therefore, this study proposed a novel 3D mapping aided (3DMA) GNSS-based cooperative positioning method that makes use of all the available surrounding receivers’ measurements for acquiring better positioning solutions. By complementarily integrating the GNSS ray-tracing algorithm and the double difference technique, the uncorrelated errors are mitigated while eliminating the correlated errors between users. As a result, a more accurate relative positioning solution is achieved, which can further improve the absolute positioning accuracy of the degraded receiver. To improve the robustness of the cooperative positioning algorithm, factor graph optimization is employed to obtain an overall optimal positioning solution among multiple receivers’ solutions. By further integrating the 3DMA GNSS cooperative positioning with factor graph optimization, the positioning accuracy and robustness are improved, and the method achieves a root mean square error of less than 10 meters for most receivers in a dense urban area.

In urban road environments, global navigation satellite system (GNSS) signals may be interrupted due to occlusion by buildings and obstacles, resulting in reduced accuracy and discontinuity of combined GNSS/inertial navigation system (INS) positioning. Improving the accuracy and robustness of combined GNSS/INS positioning systems for land vehicles in the presence of GNSS interruptions is a challenging task. The main objective of this paper is to develop a method for predicting GNSS information during GNSS outages based on a long short-term memory (LSTM) neural network to assist in factor graph-based combined GNSS/INS localization, which can provide a reliable combined localization solution during GNSS signal outages. In an environment with good GNSS signals, a factor graph fusion algorithm is used for data fusion of the combined positioning system, and an LSTM neural network prediction model is trained, and model parameters are determined using the INS velocity, inertial measurement unit (IMU) output, and GNSS position incremental data. In an environment with interrupted GNSS signals, the LSTM model is used to predict the GNSS positional increments and generate the pseudo-GNSS information and the solved results of INS for combined localization. In order to verify the performance and effectiveness of the proposed method, we conducted real-world road test experiments on land vehicles installed with GNSS receivers and inertial sensors. The experimental results show that, compared with the traditional combined GNSS/INS factor graph localization method, the proposed method can provide more accurate and robust localization results even in environments with frequent GNSS signal loss.

A foundational aspect of space domain awareness is the ability to identify and track space objects, including space object discovery and custody. This paper demonstrates the power of combining an efficient multiple hypothesis joint probabilistic data association (MH-JPDA) algorithm with a fixed-interval smoother to simultaneously track multiple space objects. For newly discovered objects, statistical initial orbit determination (SIOD) is possible with a single short optical tracklet, but results in large initial uncertainties. Combining these uncertainties with closely spaced objects can result in highly ambiguous data associations, which can lead to poor state estimates and even filter divergence. This paper invokes MH-JPDA to probabilistically update multiple tracks with multiple simultaneous observations in a sequential filter, while avoiding assigning one-to-one associations. Once sufficient information has been collected, the space objects become uniquely distinguishable among each other. Subsequently, the smoother is applied to achieve improved association of the prior observations. MH-JPDA allows for immediate track formation (using SIOD) and sequential processing of incoming observations, providing statistically rigorous real-time state estimates, whereas smoothing produces a more-refined, higher-confidence overall track estimate at user-defined intervals. This paper demonstrates this approach within the Constrained Admissible Region, Multiple Hypothesis Filter (CAR-MHF) software by tracking a simulated break-up scenario.

Multi-target tracking is a difficult task in radar data processing field. To track multiple targets with the video data collected by a X-band surface surveillance radar, a two-level cascade detection algorithm is designed at first. We combine the motion model and appearance model of the targets together for multi-target tracking and propose a multi-target tracking algorithm based on scale adaptive kernelized correlation filter (SAKCF) and Kalman filter (KF) in this paper. Firstly, the extended targets are detected by the designed tow-level cascade detection algorithm and the centroids of the extended targets are extracted by centroid algorithm too. Then, a SAKCF tracker is assigned to each target. KF is utilized to predict the position of each target and each SAKCF tracker searches the region near the predicted position to determine the real position and scale of each target. The association between tracks and trace points is performed based on Hungary algorithm. Finally, the abnormal tracks are deleted. Experimental results based on the radar video data show that compared with the classical simple online real-time target tracking (SORT) algorithm and joint probabilistic data association (JPDA) algorithm, the proposed algorithm improves multi-object tracking precision (MOTP) and multi-object tracking accuracy (MOTA).