Rao-Blackwellized Particle Filter Research Articles

Online as-Built Building Information Model Update for Robotic Monitoring in Construction Sites

Today, automated techniques for the update of as-built Building Information Models (BIM) make use of offline algorithms restricting the update frequency to an extent where continuous monitoring becomes nearly impossible. To address this problem, we propose a new method for robotic monitoring that updates an as-built BIM in real-time by solving a Simultaneous Localization and Mapping (SLAM) problem where the map is represented as a collection of elements from the as-planned BIM. The suggested approach is based on the Rao-Blackwellized Particle Filter (RBPF) which enables explicit injection of prior knowledge from the building’s construction schedule, i.e., from a 4D BIM, or its elements’ spatial relations. In the methods section we describe the benefits of using an exact inverse sensor model that provides a measure for the existence probability of elements while considering the entire probabilistic existence belief map. We continue by outlining robustification techniques that include both geometrical and temporal dimensions and present how we account for common pose and shape mistakes in constructed elements. Additionally, we show that our method reduces to the standard Monte Carlo Localization (MCL) in known areas. We conclude by presenting simulation results of the proposed method and comparing it to adjacent alternatives.

Multiple maneuvering target tracking based on hierarchical Dirichlet process and hidden Markov model

This paper is concerned with multiple maneuvering target tracking problems. Although the interacting multiple model (IMM) algorithm can be used for maneuvering targets tracking, the performance is limited as the motion mode set of the target is fixed in advance. In this paper, we address the problem of multiple maneuvering targets with unknown maneuvering models in cluttered environments. A finite hidden Markov model (HMM) is developed with truncated Hierarchical Dirichlet process (HDP) to recognize the unknown and changing motion modes. Further, the HDP-HMM is introduced into multiple hypothesis tracking (MHT) and probabilistic multiple hypothesis tracking (PMHT) approaches for multiple maneuvering targets tracking with data association uncertainty. To joint estimate the HDP-HMM parameters and target states, the particle learning and variational inference approaches are used, and the solutions of HDP-HMM-MHT and HDP-HMM-PMHT algorithms are obtained based on Rao-Blackwellized particle filter and variational Bayesian methods, respectively. As the changes of maneuver models can be learned using HDP, the multi-target tracking accuracy of the proposed algorithm is improved. Simulation results show that the proposed algorithms outperform the traditional IMM-MHT method with fixed model sets.

Bayesian estimation and reconstruction of marine surface contaminant dispersion

Discharge of hazardous substances into the marine environment poses a substantial risk to both public health and the ecosystem. In such incidents, it is imperative to accurately estimate the release strength of the source and reconstruct the spatio-temporal dispersion of the substances based on the collected measurements. In this study, we propose an integrated estimation framework to tackle this challenge, which can be used in conjunction with a sensor network or a mobile sensor for environment monitoring. We employ the fundamental convection-diffusion partial differential equation (PDE) to represent the general dispersion of a physical quantity in a non-uniform flow field. The PDE model is spatially discretised into a linear state-space model using the dynamic transient finite-element method (FEM) so that the characterisation of time-varying dispersion can be cast into the problem of inferring the model states from sensor measurements. We also consider imperfect sensing phenomena, including miss-detection and signal quantisation, which are frequently encountered when using a sensor network. This complicated sensor process introduces nonlinearity into the Bayesian estimation process. A Rao-Blackwellised particle filter (RBPF) is designed to provide an effective solution by exploiting the linear structure of the state-space model, whereas the nonlinearity of the measurement model can be handled by Monte Carlo approximation with particles. The proposed framework is validated using a simulated oil spill incident in the Baltic sea with real ocean flow data. The results show the efficacy of the developed spatio-temporal dispersion model and estimation schemes in the presence of imperfect measurements. Moreover, the parameter selection process is discussed, along with some comparison studies to illustrate the advantages of the proposed algorithm over existing methods.

An outlier-robust Rao–Blackwellized particle filter for underwater terrain-aided navigation

Terrain-aided navigation (TAN) is one of the key techniques for autonomous navigation and positioning of AUVs. This paper develops a novel integrated navigation system that combines the inertial navigation system (INS) and TAN into one filter, in contrast to the conventional INS/TAN model. This method not only provides a position estimate for the carrier but also the INS error, which increases the filtering dimension significantly. Thus, the Rao-Blackwellized particle filter (RBPF) technique is introduced to address the “dimensional disaster” problem. A three-dimensional (3-D) model is established to estimate the horizontal position while compensating for the tidal inaccuracy by extending the tidal depth bias to the state space. Furthermore, the variable and complex underwater environment can potentially result in outlier interference in depth measurements, which significantly degrades the performance of the RBPF-based estimation algorithm. This paper thus presents a 3-D adaptive RBPF method based on the maximum correntropy criterion (MCC) to suppress the influence of measurement outliers on positioning precision. Simulations and experimental tests verify the effectiveness of the proposed algorithm. The results demonstrated that the 3-D model-based AMCCRBPF method can effectively improve robustness to outliers and provide an accurate estimate of the tidal depth bias.

Optimal Trajectory Planning Method for the Navigation of WIP Vehicles in Unknown Environments: Theory and Experiment.

Navigation of underactuated wheeled inverted pendulum (WIP) vehicles in unknown environments is still facing great difficulties, especially when the optimal motion is required. This article proposes an optimal trajectory planning method for the navigation of WIP vehicles in unknown environments, where various performance demands, such as security, smoothness, efficiency, etc., are all considered. First, a map-building algorithm based on the improved Rao-Blackwellized particle filter is applied for the WIP vehicle to construct the environmental map. Then, a multiobjective optimization using the genetic algorithm is performed to find an optimized path between the given start and target point with path length, path curvature, and safe distance being taken into consideration simultaneously. Moreover, on the basis of kinematical and dynamical analysis, velocity, and acceleration constraints are parameterized with a path parameter, and the minimum-time trajectory along the optimized path is further planned with a sequence of maximum acceleration and deceleration trajectories. Finally, a WIP vehicle platform based on the robot operating system is designed, and related experiments in a real obstacle environment are conducted to validate the feasibility of the proposed method.

Shaped-Based Tightly Coupled IMU/Camera Object-Level SLAM.

Object-level simultaneous localization and mapping (SLAM) has gained popularity in recent years since it can provide a means for intelligent robot-to-environment interactions. However, most of these methods assume that the distribution of the errors is Gaussian. This assumption is not valid under many circumstances. Further, these methods use a delayed initialization of the objects in the map. During this delayed period, the solution relies on the motion model provided by an inertial measurement unit (IMU). Unfortunately, the errors tend to accumulate quickly due to the dead-reckoning nature of these motion models. Finally, the current solutions depend on a set of salient features on the object's surface and not the object's shape. This research proposes an accurate object-level solution to the SLAM problem with a 4.1 to 13.1 cm error in the position (0.005 to 0.021 of the total path). The developed solution is based on Rao-Blackwellized Particle Filtering (RBPF) that does not assume any predefined error distribution for the parameters. Further, the solution relies on the shape and thus can be used for objects that lack texture on their surface. Finally, the developed tightly coupled IMU/camera solution is based on an undelayed initialization of the objects in the map.

Modeling orientation perception adaptation to altered gravity environments with memory of past sensorimotor states.

Transitioning between gravitational environments results in a central reinterpretation of sensory information, producing an adapted sensorimotor state suitable for motor actions and perceptions in the new environment. Critically, this central adaptation is not instantaneous, and complete adaptation may require weeks of prolonged exposure to novel environments. To mitigate risks associated with the lagging time course of adaptation (e.g., spatial orientation misperceptions, alterations in locomotor and postural control, and motion sickness), it is critical that we better understand sensorimotor states during adaptation. Recently, efforts have emerged to model human perception of orientation and self-motion during sensorimotor adaptation to new gravity stimuli. While these nascent computational frameworks are well suited for modeling exposure to novel gravitational stimuli, they have yet to distinguish how the central nervous system (CNS) reinterprets sensory information from familiar environmental stimuli (i.e., readaptation). Here, we present a theoretical framework and resulting computational model of vestibular adaptation to gravity transitions which captures the role of implicit memory. This advancement enables faster readaptation to familiar gravitational stimuli, which has been observed in repeat flyers, by considering vestibular signals dependent on the new gravity environment, through Bayesian inference. The evolution and weighting of hypotheses considered by the CNS is modeled via a Rao-Blackwellized particle filter algorithm. Sensorimotor adaptation learning is facilitated by retaining a memory of past harmonious states, represented by a conditional state transition probability density function, which allows the model to consider previously experienced gravity levels (while also dynamically learning new states) when formulating new alternative hypotheses of gravity. In order to demonstrate our theoretical framework and motivate future experiments, we perform a variety of simulations. These simulations demonstrate the effectiveness of this model and its potential to advance our understanding of transitory states during which central reinterpretation occurs, ultimately mitigating the risks associated with the lagging time course of adaptation to gravitational environments.

Online Stochastic Variational Gaussian Process Mapping for Large-Scale Bathymetric SLAM in Real Time

Rao-Blackwellized particle filter (RBPF) SLAM solutions with Gaussian Process (GP) maps can both maintain multiple hypotheses of a vehicle pose estimate and perform implicit data association for loop closure detection in continuous terrain representations. Both qualities are of particular interest for SLAM with autonomous underwater vehicles (AUVs) in the open sea, where distinguishable features are scarce. However, the applicability of GP regression to parallel, real-time mapping in an RBPF framework remains limited by the size of the area to survey and the computational cost of the GP training. To overcome these constraints, in this letter we propose the adaption of Stochastic Variational GP (SVGP) regression to online mapping in combination with a novel, efficient particle trajectory storing in the RBPF. We show how the resulting RBPF-SVGP framework can achieve real-time performance in an embedded platform on two AUV surveys containing millions of points. We further test the framework on a live mission on an AUV and we make the implementation publicly available.

Rao-Blackwellized Particle Filter Algorithm Integrated with Neural Network Sensor Model Using Laser Distance Sensor

Commonly, simultaneous localization and mapping (SLAM) algorithm is developed using high-end sensors. Alternatively, some researchers use low-end sensors due to the lower cost of the robot. However, the low-end sensor produces noisy sensor measurements that can affect the SLAM algorithm, which is prone to error. Therefore, in this paper, a SLAM algorithm, which is a Rao-Blackwellized particle filter (RBPF) integrated with artificial neural networks (ANN) sensor model, is introduced to improve the measurement accuracy of a low-end laser distance sensor (LDS) and subsequently improve the performance of SLAM. The RBPF integrated with the ANN sensor model is experimented with by using the Turtlebot3 mobile robot in simulation and real-world experiments. The experiment is validated by comparing the occupancy grid maps estimated by RBPF integrated with the ANN sensor model and RBPF without ANN. Both the results in simulation and real-world experiments show that the SLAM performance of RBPF integrated with the ANN sensor model is better than the RBPF without ANN. In the real-world experiment results, the performance of the occupied cells integrated with the ANN sensor model is increased by 107.59%. In conclusion, the SLAM algorithm integrated with the ANN sensor model is able to improve the accuracy of the map estimate for mobile robots using low-end LDS sensors.

The Effect of Artificial Neural Network Towards the Number of Particles of Rao-Blackwellized Particle Filter using Laser Distance Sensor

Rao-Blackwellized particle filter (RBPF) algorithm aims to solve the Simultaneous Localization and Mapping (SLAM) problem. The performance of RBPF is based on the number of particles. The higher the number of particles, the better the performance of RBPF. However, higher number of particles required high memory and computational cost. Nevertheless, the number of particles can be reduced by using high-end sensor. By using high-end sensor, high performance of RBPF can be achieved and reduced the number of particles. But the development of the robot came at a high cost. A robot can be equipped with low-cost sensor in order to reduce the overall cost of the robot. However, low-cost sensor presented challenges of creating good map accuracy due to the low accuracy of the sensor measurement. For that reason, RBPF is integrated with artificial neural network (ANN) to interpret noisy sensor measurements and achieved better accuracy in SLAM. In this paper, RBPF integrated with ANN is experimented by using Turtlebot3 in real-world experiment. The experiment is evaluated by comparing the resulting maps estimated by RBPF with ANN and RBPF without ANN. The results show that RBPF with ANN has increased the performance of SLAM by 25.17% and achieved 10 out of 10 trials of closed loop map by using only 30 particles compared to RBPF without ANN that needs 400 particles to achieve closed loop map. In conclusion, it shows that, SLAM performance can be improved by integrating RBPF algorithm with ANN and reduces the number of particles.

Wheel-SLAM: Simultaneous Localization and Terrain Mapping Using One Wheel-Mounted IMU

A reliable pose estimator robust to environmental disturbances is desirable for mobile robots. To this end, inertial measurement units (IMUs) play an important role because they can perceive the full motion state of the vehicle independently. However, it suffers from accumulative error due to inherent noise and bias instability, especially for low-cost sensors. In our previous studies on Wheel-INS [1], [2], we proposed to limit the error drift of the pure inertial navigation system (INS) by mounting an IMU to the wheel of the robot to take advantage of rotation modulation. However, Wheel-INS still drifted over a long period of time due to the lack of external correction signals. In this letter, we propose to exploit the environmental perception ability of Wheel-INS to achieve simultaneous localization and mapping (SLAM) with only one IMU. To be specific, we use the road bank angles (mirrored by the robot roll angles estimated by Wheel-INS) as terrain features to enable the loop closure with a Rao-Blackwellized particle filter. The road bank angle is sampled and stored according to the robot position in the grid maps maintained by the particles. The weights of the particles are updated according to the difference between the currently estimated roll sequence and the terrain map. Field experiments suggest the feasibility of the idea to perform SLAM in Wheel-INS using the robot roll angle estimates. In addition, the positioning accuracy is improved significantly (more than 30%) over Wheel-INS.

Simultaneous Localization and Calibration (SLAC) Methods for a Train-Mounted Magnetometer

<h3>Summary</h3> Magnetic field localization is based on the fact that the Earth’s magnetic field is distorted in the vicinity of ferromagnetic objects. When ferromagnetic objects are in fixed positions, the distortions are also fixed and, thus, contain location information. In our prior work, we proposed a simultaneous localization and calibration (SLAC) algorithm based on a Rao-Blackwellized particle filter that enables magnetic train localization using only uncalibrated magnetometer measurements. In this paper, a lower-complexity version of the SLAC algorithm is proposed that only estimates a subset of calibration parameters. An evaluation compares the full and reduced SLAC approach to a particle filter in which the magnetometer is pre-calibrated with a fixed set of parameters. The results show a clear advantage for both SLAC approaches and that the SLAC algorithm with a reduced set of calibration parameters achieves the same performance as the one with a full set of parameters.

Multisensor Fusion SLAM Research Based on Improved RBPF‐SLAM Algorithm

Simultaneous localization and map construction (SLAM) technology provides the foundation for indoor robots to realize autonomous path planning. The Rao‐Blackwellized particle filtering (RBPF) algorithm is widely used to obtain information and perform map construction in unknown environments. This paper proposes a multisensor fusion algorithm that improves the RBPF‐SLAM algorithm by addressing the issues of particle distribution errors and degradation. To achieve this, the extended Kalman filter (EKF) is first adopted to effectively fuse odometry and inertial navigation (inertial measurement unit (IMU)) data as the initial positional information. Then, a high‐precision light detection and ranging (LIDAR) observation model is integrated to calculate the proposed distribution, and a threshold is introduced to determine the number of valid particles to simplify the resampling step. Finally, raster map construction experiments were carried out on both the Gazebo simulation environment and Robot Operating System (ROS)‐based mobile robot Keepbot platform in different scenarios. The experimental results demonstrate that the optimized RBPF‐SLAM algorithm generates a clearer and more complete map outline, which can effectively improve the accuracy of robot pose estimation, acquire a reliable 2D raster map with a reduced number of particles, and greatly reduces the computational effort.

A Tightly Coupled Integration Approach for Cooperative Positioning Enhancement in DSRC Vehicular Networks

Intelligent transportation system significantly relies on accurate positioning information of land vehicles for both safety and non-safety related applications, such as hard-braking ahead warning and red-light violation warning. However, existing Global Navigation Satellite System (GNSS) based solutions suffer from positioning performance degradation in challenging environments, such as urban canyons and tunnels. In this paper, we focus on the positioning performance enhancement of land vehicles via cooperative positioning under a partial GNSS environment in a Vehicular Ad-hoc NETwork (VANET). The availability of Time-of-Flight (ToF) based inter-vehicle or vehicle-to-infrastructure ranges is verified via 5.9 GHz Dedicated Short-Range Communication (DSRC) vehicle-to-everything communication with RTS/CTS unicast mechanism. An inertial navigation sensor aided, tightly coupled integration approach for land vehicle cooperative positioning using DSRC ToF ranges and carrier frequency offset range-rates is proposed, where a digital map is used to constrain the position estimates. If available, the GNSS pseudorange and Doppler shift under partial GNSS environment can also be incorporated. A Rao–Blackwellized particle filter is utilized to estimate the unknown variables allowing for reduced computational complexity in comparison with the conventional particle filter. The posterior Cramer–Rao lower bound is also derived to give a theoretical performance guideline. Both simulation and experimental results show the validity of our proposed approach.

Semi-symbolic inference for efficient streaming probabilistic programming

A streaming probabilistic program receives a stream of observations and produces a stream of distributions that are conditioned on these observations. Efficient inference is often possible in a streaming context using Rao-Blackwellized particle filters (RBPFs), which exactly solve inference problems when possible and fall back on sampling approximations when necessary. While RBPFs can be implemented by hand to provide efficient inference, the goal of streaming probabilistic programming is to automatically generate such efficient inference implementations given input probabilistic programs. In this work, we propose semi-symbolic inference, a technique for executing probabilistic programs using a runtime inference system that automatically implements Rao-Blackwellized particle filtering. To perform exact and approximate inference together, the semi-symbolic inference system manipulates symbolic distributions to perform exact inference when possible and falls back on approximate sampling when necessary. This approach enables the system to implement the same RBPF a developer would write by hand. To ensure this, we identify closed families of distributions – such as linear-Gaussian and finite discrete models – on which the inference system guarantees exact inference. We have implemented the runtime inference system in the ProbZelus streaming probabilistic programming language. Despite an average 1.6× slowdown compared to the state of the art on existing benchmarks, our evaluation shows that speedups of 3×-87× are obtainable on a new set of challenging benchmarks we have designed to exploit closed families.

PMBM-Based SLAM Filters in 5G mmWave Vehicular Networks

Radio-based vehicular simultaneous localization and mapping (SLAM) aims to localize vehicles while mapping the landmarks in the environment. We propose a sequence of three Poisson multi-Bernoulli mixture (PMBM) based SLAM filters, which handle the entire SLAM problem in a theoretically optimal manner. The complexity of the three proposed SLAM filters is progressively reduced while sustaining high accuracy by deriving SLAM density approximation with the marginalization of nuisance parameters (either vehicle state or data association). Firstly, the PMBM SLAM filter serves as the foundation, for which we provide the first complete description based on a Rao-Blackwellized particle filter. Secondly, the Poisson multi-Bernoulli (PMB) SLAM filter is based on the standard reduction from PMBM to PMB, but involves a novel interpretation based on auxiliary variables and a relation to Bethe free energy. Finally, using the same auxiliary variable argument, we derive a marginalized PMB SLAM filter, which avoids particles and is instead implemented with a low-complexity cubature Kalman filter. We evaluate the three proposed SLAM filters in comparison with the probability hypothesis density (PHD) SLAM filter in 5G mmWave vehicular networks and show the computation-performance trade-off between them.

An Extended Kalman Filter for Magnetic Field SLAM Using Gaussian Process Regression.

We present a computationally efficient algorithm for using variations in the ambient magnetic field to compensate for position drift in integrated odometry measurements (dead-reckoning estimates) through simultaneous localization and mapping (SLAM). When the magnetic field map is represented with a reduced-rank Gaussian process (GP) using Laplace basis functions defined in a cubical domain, analytic expressions of the gradient of the learned magnetic field become available. An existing approach for magnetic field SLAM with reduced-rank GP regression uses a Rao-Blackwellized particle filter (RBPF). For each incoming measurement, training of the magnetic field map using an RBPF has a computational complexity per time step of , where is the number of particles, and is the number of basis functions used to approximate the Gaussian process. Contrary to the existing particle filter-based approach, we propose applying an extended Kalman filter based on the gradients of our learned magnetic field map for simultaneous localization and mapping. Our proposed algorithm only requires training a single map. It, therefore, has a computational complexity at each time step of . We demonstrate the workings of the extended Kalman filter for magnetic field SLAM on an open-source data set from a foot-mounted sensor and magnetic field measurements collected onboard a model ship in an indoor pool. We observe that the drift compensating abilities of our algorithm are comparable to what has previously been demonstrated for magnetic field SLAM with an RBPF.

A Massively Parallel Implementation of Gaussian Process Regression for Real Time Bathymetric Modeling and Simultaneous Localization and Mapping

A Gaussian process regression (GPR) can be used as a stochastic method for modeling underwater terrain using multibeam sonar data. A GPR model can improve the effective resolution of a terrain model over traditional gridding methods and quantify uncertainty with an estimate of model variance over its entire domain. However, GPR solutions are extremely computationally expensive and generally reserved for post-processing applications. To make GPR viable for real-time applications, we developed massively parallel GPR (MP-GPR) to run on a graphical processing unit (GPU). MP-GPR is first used to process real-time multibeam data when assuming accurate navigation from a high precision position, heading, and attitude source. In underwater environments, however, we are denied the luxury of high precision position sensors and typically rely on dead reckoning. Therefore, MP-GPR was used as a terrain model for a featureless, Rao-Blackwellized particle filter based, bathymetric particle-filter simultaneous localization and mapping (BPSLAM) algorithm. Our GPU-based extension of BPSLAM (GP-BPSLAM) estimates many possible vehicle trajectories and MP-GPR predicts a possible map for each. By comparing the recent multibeam observations against the model for each possible trajectory, unlikely trajectories can be identified and removed. GP-BPSLAM is able to process data in real time and generate a navigation solution that is more accurate than simple dead reckoning.

Design and implementation of an autonomous service robot based on cyber physical modeling systems

An autonomous robot is designed and implemented to support the service works. Many technologies were adapted, namely human-robot interaction order system, path planning and mapping in indoor, Robot Operating System (ROS), and Cyber Physical System (CPS). Many subsystems are developed and constructed based on ROS to implement a human-interaction action in completing service robot tasks. A human-robot interaction order system is designed for the convenient human servicing purpose. Therefore, the customer quickly finds the commodity information through the internet. This system recommends the commodity according to the content of the database. In addition, the customer makes an order by touch screen interface, and then it sends a command to control the robot to deliver the commodity. In the second subsystem, it includes path planning and mapping generation. The well-known simultaneous localization and mapping (SLAM) concept with a Rao-Blackwellized particle filter (RBPF) addresses the appropriated maps after completing the exploration of the indoor environment. In order to reach better robustness and agility, the hybrid path planning algorithm includes a global A* algorithm and local dynamic window approach (DWA) to navigation. In the third subsystem, the Gazebo makes the perfect construction of CPS design. Finally, the simulation and implementation of dual-arm robot navigation with the interactive order situation to efficiently support servicing actions of the robot and finish the required tasks.

Indoor Environment Mapping of Mobile Robot Based on Efficient DSM-PF Method

A mobile robot uses an odometer system for localization, and uses a lidar sensor to obtain environmental information, to complete the process of Simultaneous Localization and Mapping (SLAM). This paper proposes an efficient Double Simultaneous Majorization Particle Filter (DSM-PF) algorithm, which is based on the majorization of the Rao-Blackwellized Particle Filtering (RBPF) method. The main purpose of the algorithm is to improve the accuracy of particle pose and maintain the weight of particles, to improve the quality of the results. For the motion distortion generated by the mobile robot, a particle pose majorization algorithm is proposed to improve the localization accuracy of the robot while in motion. At the same time, a weight majorization algorithm is proposed to maintain the weight of each particle, slow down the degradation of particles and improve the accuracy of the map carried by particles. In addition, for different particles that require resampling, an adaptive hierarchical resampling method is performed to maintain particles with higher weights. In this paper, the feasibility of the proposed algorithm is verified in different indoor environments. The experimental results show that the algorithm can slow down the degradation of particles and prevent the number of particles from declining. What is more, the experimental results of particle resampling show that the proposed algorithm has better mapping ability.