In this paper, we address the problem of estimating the rotational extrinsics, as well as the scale factors of two gyroscopes rigidly mounted on the same device. In particular, we formulate the problem as a least-squares minimization and introduce a direct algorithm that computes the estimated quantities without any iterations, hence avoiding local minima and improving efficiency. Furthermore, we show that the rotational extrinsics are observable while the scale factors can be determined up to a global scale for general configurations of the gyroscopes. To this end, we also study special placements of the gyroscopes where a pair, or all, of their axes are parallel and analyze their impact on the scale factors’ observability. Lastly, we evaluate our algorithm in simulation and real-world experiments to assess its performance as a function of key motion and sensor characteristics.

In this paper, we address the problem of estimating dense depth from a sequence of images using deep neural networks. Specifically, we employ a dense-optical-flow network to compute correspondences and then triangulate the point cloud to obtain an initial depth map. Parts of the point cloud, however, may be less accurate than others due to lack of common observations or small parallax. To further increase the triangulation accuracy, we introduce a depth-refinement network (DRN) that optimizes the initial depth map based on the image’s contextual cues. In particular, the DRN contains an iterative refinement module (IRM) that improves the depth accuracy over iterations by refining the deep features. Lastly, the DRN also predicts the uncertainty in the refined depths, which is desirable in applications such as measurement selection for scene reconstruction. We show experimentally that our algorithm outperforms state-of-the-art approaches in terms of depth accuracy, and verify that our predicted uncertainty is highly correlated to the actual depth error.

This paper addresses the problem of learning to complete a scene's depth from sparse depth points and images of indoor scenes. Specifically, we study the case in which the sparse depth is computed from a visual-inertial simultaneous localization and mapping (VI-SLAM) system. The resulting point cloud has low density, it is noisy, and has nonuniform spatial distribution, as compared to the input from active depth sensors, e.g., LiDAR or Kinect. Since the VI-SLAM produces point clouds only over textured areas, we compensate for the missing depth of the low-texture surfaces by leveraging their planar structures and their surface normals which is an important intermediate representation. The pre-trained surface normal network, however, suffers from large performance degradation when there is a significant difference in the viewing direction (especially the roll angle) of the test image as compared to the trained ones. To address this limitation, we use the available gravity estimate from the VI-SLAM to warp the input image to the orientation prevailing in the training dataset. This results in a significant performance gain for the surface normal estimate, and thus the dense depth estimates. Finally, we show that our method outperforms other state-of-the-art approaches both on training (ScanNet [1] and NYUv2 [2]) and testing (collected with Azure Kinect [3]) datasets.

In this paper, we present the RISE-SLAM algorithm for performing visual-inertial simultaneous localization and mapping (SLAM), while improving estimation consistency. Specifically, in order to achieve real-time operation, existing approaches often assume previously-estimated states to be perfectly known, which leads to inconsistent estimates. Instead, based on the idea of the Schmidt-Kalman filter, which has processing cost linear in the size of the state vector but quadratic memory requirements, we derive a new consistent approximate method in the information domain, which has linear memory requirements and adjustable (constant to linear) processing cost. In particular, this method, the resource-aware inverse Schmidt estimator (RISE), allows trading estimation accuracy for computational efficiency. Furthermore, and in order to better address the requirements of a SLAM system during an exploration vs. a relocalization phase, we employ different configurations of RISE (in terms of the number and order of states updated) to maximize accuracy while preserving efficiency. Lastly, we evaluate the proposed RISE-SLAM algorithm on publicly-available datasets and demonstrate its superiority, both in terms of accuracy and efficiency, as compared to alternative visual-inertial SLAM systems.

In this paper, we present a novel approach to shared augmented reality (AR) for mobile devices operating in the same area that does not rely on cloud computing. In particular, each user's device processes the visual and inertial data received from its sensors and almost immediately broadcasts a partial feature map of its surroundings. In parallel, every device localizes against the broadcasted maps by employing feature observations to determine its 4-DOF transformation to each of the gravity-aligned maps. By doing so, virtual content placed by any of the users is quickly and accurately displayed in all other users' views. Furthermore, to reduce the effect of inconsistency introduced by relocalizing against incrementally created and updated maps, their transformations w.r.t. the device are modeled as random processes driven by white noise instead of constant, unknown parameters. Lastly, we assess the accuracy of our approach for the case of two users in a room-scale environment against VICON ground-truth.

One approach to improve the accuracy and robustness of vision-aided inertial navigation systems (VINS) that employ low-cost inertial sensors, is to obtain scale information from stereoscopic vision. Processing images from two cameras, however, is computationally expensive and increases latency. To address this limitation, in this work, a novel two-camera alternating-stereo VINS is presented. Specifically, the proposed system triggers the left-right cameras in an alternating fashion, estimates the poses corresponding to the left camera only, and introduces a linear interpolation model for processing the alternating right camera measurements. Although not a regular stereo system, the alternating visual observations when employing the proposed interpolation scheme, still provide scale information, as shown by analyzing the observability properties of the vision-only corresponding system. Finally, the performance gain, of the proposed algorithm over its monocular and stereo counterparts is assessed using various datasets.

This paper addresses the problem of gait estimation using visual and inertial data, as well as human motion models. Specifically, a batch least-squares (BLS) algorithm is presented that fuses data from a minimal set of sensors [two inertial measurement units (IMUs), one on each foot, and a head-mounted IMU-camera pair] along with motion constraints corresponding to the different walking states, to estimate the person's head and feet poses. Subsequently, gait models are employed to solve for the lower-body's posture and generate its animation. Experimental results against the VICON motion capture system demonstrate the accuracy of the proposed minimal sensors-based system for determining a person's motion.

In this paper, we address the problem of cooperative mapping (CM) using datasets collected by multiple users at different times, when the transformation between the users' starting poses is unknown. Specifically, we formulate CM as a constrained optimization problem, in which each user's independently estimated trajectory and map are merged together by imposing geometric constraints between commonly observed point and line features. Additionally, we provide an algorithm for efficiently solving the CM problem, by taking advantage of its structure. The proposed solution is proven to be batch-least-squares (BLS) optimal over all users' datasets, while it is less memory demanding and lends itself to parallel implementations. In particular, our solution is shown to be faster than the standard BLS solution, when the overlap between the users' data is small. Furthermore, our algorithm is resource-aware as it is able to consistently trade accuracy for lower processing cost, by retaining only an informative subset of the common-feature constraints. Experimental results based on visual and inertial measurements collected from multiple users within large buildings are used to assess the performance of the proposed CM algorithm.

In this work, we present an algebraic solution to the classical perspective-3-point (P3P) problem for determining the position and attitude of a camera from observations of three known reference points. In contrast to previous approaches, we first directly determine the cameras attitude by employing the corresponding geometric constraints to formulate a system of trigonometric equations. This is then efficiently solved, following an algebraic approach, to determine the unknown rotation matrix and subsequently the cameras position. As compared to recent alternatives, our method avoids computing unnecessary (and potentially numerically unstable) intermediate results, and thus achieves higher numerical accuracy and robustness at a lower computational cost. These benefits are validated through extensive Monte-Carlo simulations for both nominal and close-to-singular geometric configurations.

In this paper, we seek to provide consistent, real-time 3D localization capabilities to mobile devices navigating within previously mapped areas. To this end, we introduce the Cholesky-Schmidt-Kalman filter (C-SKF), which explicitly considers the uncertainty of the prior map, by employing the sparse Cholesky factor of the map's Hessian, instead of its dense covariance-as is the case for the Schmidt-Kalman filter. By doing so, the C-SKF has memory requirements typically linear in the size of the map, as opposed to quadratic for storing the map's covariance. Moreover, and in order to bound the processing needs of the C-SKF (between linear and quadratic in the size of the map), we introduce two relaxations of the C-SKF algorithm: (i) The sC-SKF, which operates on the Cholesky factors of independent sub-maps resulting from dividing the map into overlapping segments. (ii) We formulate an efficient method for sparsifying the Cholesky factor by selecting and processing a subset of loop-closure measurements based on their temporal distribution. Lastly, we assess the processing and memory requirements of the proposed algorithms, and compare their positioning accuracy against other inconsistent map-based localization approaches that employ measurement-noise-covariance inflation to compensate for the map's uncertainty.

In this paper, a sliding-window two-camera vision-aided inertial navigation system (VINS) is presented in the square-root inverse domain. The performance of the system is assessed for the cases where feature matches across the two-camera images are processed with or without any stereo constraints (i.e., stereo vs. binocular). To support the comparison results, a theoretical analysis on the information gain when transitioning from binocular to stereo is also presented. Additionally, the advantage of using a two-camera (both stereo and binocular) system over a monocular VINS is assessed. Furthermore, the impact on the achieved accuracy of different image-processing frontends and estimator design choices is quantified. Finally, a thorough evaluation of the algorithm's processing requirements, which runs in real-time on a mobile processor, as well as its achieved accuracy as compared to alternative approaches is provided, for various scenes and motion profiles.

In this paper, we present a vision-aided inertial navigation system (VINS) for localizing wheeled robots. In particular, we prove that VINS has additional unobservable directions, such as the scale, when deployed on a ground vehicle that is constrained to move along straight lines or circular arcs. To address this limitation, we extend VINS to incorporate low-frequency wheel-encoder data, and show that the scale becomes observable. Furthermore, and in order to improve the localization accuracy, we introduce the manifold-(m)VINS that exploits the fact that the vehicle moves on an approximately planar surface. In our experiments, we first show the performance degradation of VINS due to special motions, and then demonstrate that by utilizing the additional sources of information, our system achieves significantly higher positioning accuracy, while operating in real-time on a commercial-grade mobile device.

In this paper, we address the problem of cooperative mapping (CM) using datasets collected by multiple users at different times, when the transformation between the users' starting poses is unknown. Specifically, we formulate CM as a constrained optimization problem, where each user's independently estimated trajectory and map are combined in a single map by imposing geometric constraints between commonly-observed point and line features. Furthermore, our formulation allows for modularity since new/old maps (or parts of them) can be easily added/removed with no impact on the remaining ones. Additionally, the proposed CM algorithm lends itself, for the most part, to parallel implementations, hence gaining in speed. Experimental results based on visual and inertial measurements collected from four users within two large buildings are used to assess the performance of the proposed CM algorithm.

In this paper, we introduce an Iterative Kalman Smoother (IKS) for tracking the 3D motion of a mobile device in real-time using visual and inertial measurements. In contrast to existing Extended Kalman Filter (EKF)-based approaches, smoothing can better approximate the underlying nonlinear system and measurement models by re-linearizing them. Additionally, by iteratively optimizing over all measurements available, the IKS increases the convergence rate of critical parameters (e.g., IMU-camera clock drift) and improves the positioning accuracy during challenging conditions (e.g., scarcity of visual features). Furthermore, and in contrast to existing inverse filters, the proposed IKS's numerical stability allows for efficient 32-bit implementations on resource-constrained devices, such as cell phones and wearables. We validate the IKS for performing vision-aided inertial navigation on Google Glass, a wearable device with limited sensing and processing, and demonstrate positioning accuracy comparable to that achieved on cell phones. To the best of our knowledge, this work presents the first proof-of-concept real-time 3D indoor localization system on a commercial-grade wearable computer.

Nonlinear estimation problems, such as range-only and bearing-only target tracking, are often addressed using linearized estimators, e.g., the extended Kalman filter (EKF). These estimators generally suffer from linearization errors as well as the inability to track multimodal probability density functions. In this paper, we propose a bank of batch maximum a posteriori (MAP) estimators as a general estimation framework that provides relinearization of the entire state trajectory, multihypothesis tracking, and an efficient hypothesis generation scheme. Each estimator in the bank is initialized using a locally optimal state estimate for the current time step. Every time a new measurement becomes available, we relax the original batch-MAP problem and solve it incrementally. More specifically, we convert the relaxed one-step-ahead cost function into polynomial or rational form and compute all the local minima analytically. These local minima generate highly probable hypotheses for the target's trajectory and hence greatly improve the quality of the overall MAP estimate. Additionally, pruning of least probable hypotheses and marginalization of old states are employed to control the computational cost. Monte Carlo simulation and real-world experimental results show that the proposed approach significantly outperforms the standard EKF, the batch-MAP estimator, and the particle filter.

In this paper, we present C-KLAM, a Maximum A Posteriori (MAP) estimator-based keyframe approach for SLAM. Instead of discarding information from non-keyframes for reducing the computational complexity, the proposed C-KLAM presents a novel, elegant, and computationally-efficient technique for incorporating most of this information in a consistent manner, resulting in improved estimation accuracy. To achieve this, C-KLAM projects both proprioceptive and exteroceptive information from the non-keyframes to the keyframes, using marginalization, while maintaining the sparse structure of the associated information matrix, resulting in fast and efficient solutions. The performance of C-KLAM has been tested in experiments, using visual and inertial measurements, to demonstrate that it achieves performance comparable to that of the computationally-intensive batch MAP-based 3D SLAM, that uses all available measurement information.

In this paper, we study estimator inconsistency in vision-aided inertial navigation systems (VINS) from the standpoint of system's observability. We postulate that a leading cause of inconsistency is the gain of spurious information along unobservable directions, which results in smaller uncertainties, larger estimation errors, and divergence. We develop an observability constrained VINS (OC-VINS), which explicitly enforces the unobservable directions of the system, hence preventing spurious information gain and reducing inconsistency. This framework is applicable to several variants of the VINS problem such as visual simultaneous localization and mapping (V-SLAM), as well as visual-inertial odometry using the multi-state constraint Kalman filter (MSC-KF). Our analysis, along with the proposed method to reduce inconsistency, are extensively validated with simulation trials and real-world experimentation.

In this paper, we introduce an efficient maximum a posteriori (MAP) estimation algorithm, which effectively tracks multiple most probable hypotheses. In particular, due to multimodal distributions arising in most nonlinear problems, we employ a bank of MAP to track these modes (hypotheses). The key idea is that we analytically determine all the posterior modes for the current state at each time step, which are used to generate highly probable hypotheses for the entire trajectory. Moreover, since it is expensive to solve the MAP problem sequentially over time by an iterative method such as Gauss-Newton, in order to speed up its solution, we reuse the previous computations and incrementally update the square-root informationmatrix at every time step, while batch relinearization is performed only periodically or as needed.

We consider the problem of joint-state estimation for mobile wireless sensor networks (WSN) using noisy analog observations from spatially-distributed sensors. Due to communication bandwidth constraints, sensors can transmit only quantized observations. As opposed to existing estimators that process either only quantized or only analog observations, we develop a Maximum A Posteriori-based hybrid estimation framework that enables each sensor to utilize its own local analog observations as well as quantized observations received from other sensors to improve estimation accuracy.

In this paper, we address the problem of extrinsically calibrating an inertial measurement unit (IMU) with respect to an RGBD sensor. In particular, we study the observability of the nonlinear IMU-RGBD calibration system and prove that the calibration parameters are observable given observations to a single point feature. Moreover, we show that the system has four unobservable directions corresponding to the global translation and rotations about the gravity vector. Based on the results of the observability analysis, we design a consistency-improved, observability constrained (OC) extended Kalman filter (EKF)-based estimator for calibrating the sensor pair while at the same time tracking its pose and creating a 3D map of the environment. Finally, we validate the key findings of the observability analysis and assess the performance of the OC-EKF estimator both in simulation and experimentally.

In this paper, we present an elegant solution to the 2D LIDAR-camera extrinsic calibration problem. Specifically, we develop a simple method for establishing correspondences between a line-scan (2D) LIDAR and a camera using a small calibration target that only contains a straight line. Moreover, we formulate the nonlinear least-squares problem for finding the unknown 6 degree-of-freedom (dof) transformation between the two sensors, and solve it analytically to determine its global minimum. Additionally, we examine the conditions under which the unknown transformation becomes unobservable, which can be used for avoiding ill-conditioned configurations. Finally, we present extensive simulation and experimental results for assessing the performance of the proposed algorithm as compared to alternative analytical approaches.

Particle filtering (PF) is a popular nonlinear estimation technique and has been widely used in a variety of applications such as target tracking. Within the PF framework, one critical design choice that greatly affects the filter's performance is the selection of the proposal distribution from which particles are drawn. In this paper, we advocate the proposal distribution to be a Gaussian-mixture-based approximation of the posterior probability density function (pdf) after taking into account the most recent measurement. The novelty of our approach is that each Gaussian in the mixture is determined analytically to match the modes of the underlying unknown posterior pdf. As a result, particles are sampled along the most probable regions of the state space, hence reducing the probability of particle depletion. We adapt this proposal distribution into a new PF, termed Analytically-Guided-Sampling (AGS)-PF, and apply it to the particular problem of range-only target tracking. Both Monte-Carlo simulation and real-world experimental results validate the superior performance of the proposed AGS-PF over other state-of-the-art PF algorithms.

This paper addresses the problem of estimating the state of a vehicle moving in 3D based on inertial measurements and visual observations of lines. In particular, we investigate the observability properties of the corresponding vision-aided inertial navigation system (VINS) and prove that it has five (four) unobservable degrees of freedom when one (two or more) line(s) is (are) detected. Additionally, we leverage this result to improve the consistency of the extended Kalman filter (EKF) estimator introduced for efficiently processing line observations over a sliding time-window at cost only linear in the number of line features. Finally, we validate the proposed algorithm experimentally using a miniature-size camera and a micro-electromechanical systems (MEMS)-quality inertial measurement unit (IMU).

In this paper, we present a Covariance Intersection (CI)-based algorithm for reducing the processing and communication complexity of multi-robot Cooperative Localization (CL). Specifically, for a team of N robots, our proposed approximate CI-based CL approach has processing and communication complexity only linear, O(N), in the number of robots. Moreover, and in contrast to alternative approximate methods, our approach is provably consistent, can handle asynchronous communication, and does not place any restriction on the robots' motion. We test the performance of our proposed approach in both simulations and experimentally, and show that it outperforms the existing linear-complexity split CI-based CL method.

In this paper, we present a linear-complexity 3D inertial navigation algorithm using both point and plane features observed from an RGBD camera. In particular, we study the system's observability properties, and prove that: (i) When observing a single plane feature of known direction, the IMU gyroscope bias is observable. (ii) By observing a single point feature, as well as a single plane of known direction but not perpendicular to gravity, all degrees of freedom of the IMU-RGBD navigation system become observable, up to global translations. Next, based on the results of the observability analysis, we design a consistency-improved, observability-constrained (OC) extended Kalman filter (EKF)-based estimator for the IMU-RGBD camera navigation system. Finally, we experimentally validate the superiority of our proposed algorithm compared to alternative methods in urban scenes.

In this paper, we study the problem of hovering (i.e., absence of translational motion) detection and compensation in Vision-aided Inertial Navigation Systems (VINS). We examine the system's unobservable directions for two common hovering conditions (with and without rotational motion) and propose a robust motion-classification algorithm, based on both visual and inertial measurements. By leveraging our observability analysis and the proposed motion classifier, we modify existing state-of-the-art filtering algorithms, so as to ensure that the number of the system's unobservable directions is minimized. Finally, we validate experimentally the proposed modified sliding window filter, by demonstrating its robustness on a quadrotor with rapid transitions between hovering and forward motions, within an indoor environment.

This paper presents hybrid Minimum Mean Squared Error-based estimators for wireless sensor networks with time-varying communication-bandwidth constraints, focusing on the particular application of multi-robot Cooperative Localization. When sensor nodes (e.g., robots) communicate only a quantized version of their analog measurements to the team, our proposed hybrid filters enable robots to process all available information, i.e., local analog measurements (recorded by its own sensors) as well as remote quantized measurements (collected and communicated by other sensors). Moreover, these filters are resource-aware and can utilize additional bandwidth, whenever available, to maximize estimation accuracy. Specifically, in this paper, we present two filters, the Hybrid Batch-Quantized Kalman filter (H-BQKF) and the Hybrid Iteratively-Quantized Kalman filter (H-IQKF), that can process local analog measurements along with remote measurements quantized to any number of bits. We test our proposed filters in simulations and experimentally, and demonstrate that they achieve performance comparable to the standard Kalman filter.

In this paper, we present an observability analysis of a vision-aided inertial navigation system (VINS) in which the camera is downward looking and observes a single point feature on the ground. In our analysis, the full INS parameter vector (including position, velocity, rotation, and inertial sensor biases) as well as the 3D position of the observed point feature are considered as state variables. In particular, we prove that the system has only three unobservable directions corresponding to global translations along the x and y axes, and rotations around the gravity vector. Hence, compared to general VINS, an advantage of using only ground features is that the vertical translation becomes observable. The findings of the theoretical analysis are validated through real-world experiments.

This paper addresses two key limitations of the unscented Kalman filter (UKF) when applied to the simultaneous localization and mapping (SLAM) problem: the cubic computational complexity in the number of states and the inconsistency of the state estimates. To address the first issue, we introduce a new sampling strategy for the UKF, which has constant computational complexity. As a result, the overall computational complexity of UKF-based SLAM becomes of the same order as that of the extended Kalman filter (EKF)-based SLAM, i.e., quadratic in the size of the state vector. Furthermore, we investigate the inconsistency issue by analyzing the observability properties of the linear-regression-based model employed by the UKF. Based on this analysis, we propose a new algorithm, termed observability-constrained (OC)-UKF, which ensures the unobservable subspace of the UKF’s linear-regression-based system model is of the same dimension as that of the nonlinear SLAM system. This results in substantial improvement in the accuracy and consistency of the state estimates. The superior performance of the OC-UKF over other state-of-the-art SLAM algorithms is validated by both Monte-Carlo simulations and real-world experiments.

In this paper, we address the problem of motion-induced 3-D robot-to-robot extrinsic calibration that is based on ego-motion estimates and combinations of interrobot measurements (i.e., distance and/or bearing observations from either or both of the two robots, recorded across multiple time steps). In particular, we focus on solving minimal problems, where the unknown 6-degree-of-freedom (DOF) transformation between the two robots is determined based on the minimum number of measurements necessary to find a finite set of solutions. In order to address the very large number of possible combinations of interrobot observations, we identify symmetries in the measurement sequence and use them to prove that any extrinsic robot-to-robot calibration problem can be solved based on the solutions of only 14 (base) minimal problems. Moreover, we provide algebraic (closed-form) and efficient symbolic-numerical (analytical) solution methods to these minimal problems. Finally, we evaluate the performance of our proposed solvers through extensive simulations and experiments.

In order to fuse camera and odometer measurements, we first need to estimate their relative transformation through the so-called odometer-camera extrinsic calibration. In this paper, we present a two-step analytical least-squares solution for the extrinsic odometer-camera calibration that (i) is not iterative and finds the least-squares optimal solution without any initialization, and (ii) does not require any special hardware or the presence of known landmarks in the scene. Specifically, in the first step, we estimate a subset of the 3D relative rotation parameters by analytically minimizing a least-squares cost function. We then back-substitute these estimates in the measurement constraints, and determine the rest of the 3D transformation parameters by analytically minimizing a second least-squares cost function. Simulation and experimental results are presented that validate the efficiency and accuracy of the proposed algorithm.

In this paper, we study the problem of Cooperative Localization (CL) for two robots, each equipped with an Inertial Measurement Unit (IMU) and a camera. We present an algorithm that enables the robots to exploit common features, observed over a sliding-window time horizon, in order to improve the localization accuracy of both vehicles. In contrast to existing CL methods, which require robot-to-robot distance and/or bearing measurements to resolve the robots' relative position and orientation (pose), our approach recovers the relative pose through indirect information from the commonly observed features. Moreover, we analyze the system observability properties to determine how many degrees of freedom (d.o.f.) of the relative transformation can be computed under different measurement scenarios. Lastly, we present simulation results to evaluate the performance of the proposed method.

This paper focuses on reducing the computational complexity of the extended Kalman filter (EKF)-based multi-robot cooperative localization (CL) by taking advantage of the sparse structure of the measurement Jacobian matrix H. In contrast to the standard EKF update, whose complexity is up to O(N4) (N is the number of robots in a team), we introduce a Modified Householder QR algorithm which fully exploits the sparse structure of the matrix H, and prove that the overall complexity of the EKF update, based on our QR factorization scheme, reduces to O(N3). Finally, we validate the Modified Householder QR algorithm through extensive simulations, and demonstrate its superior performance both in terms of accuracy and CPU runtime, as compared to the current state-of-the-art QR decomposition algorithm for sparse matrices.

This paper presents two new, efficient solutions to the two-view, relative pose problem from three image point correspondences and one common reference direction. This three-plus-one problem can be used either as a substitute for the classic five-point algorithm, using a vanishing point for the reference direction, or to make use of an inertial measurement unit commonly available on robots and mobile devices where the gravity vector becomes the reference direction. We provide a simple, closed-form solution and a solution based on algebraic geometry which offers numerical advantages. In addition, we introduce a new method for computing visual odometry with RANSAC and four point correspondences per hypothesis. In a set of real experiments, we demonstrate the power of our approach by comparing it to the five-point method in a hypothesize-and-test visual odometry setting.

In this work, we present a Direct Least-Squares (DLS) method for computing all solutions of the perspective-n-point camera pose determination (PnP) problem in the general case (n ≥ 3). Specifically, based on the camera measurement equations, we formulate a nonlinear least-squares cost function whose optimality conditions constitute a system of three third-order polynomials. Subsequently, we employ the multiplication matrix to determine all the roots of the system analytically, and hence all minima of the LS, without requiring iterations or an initial guess of the parameters. A key advantage of our method is scalability, since the order of the polynomial system that we solve is independent of the number of points. We compare the performance of our algorithm with the leading PnP approaches, both in simulation and experimentally, and demonstrate that DLS consistently achieves accuracy close to the Maximum-Likelihood Estimator (MLE).

In this paper, we present an analytical method for computing the globally optimal estimates of orthogonal vanishing points in a “Manhattan world” with a calibrated camera. We formulate this as constrained least-squares problem whose optimality conditions form a multivariate polynomial system. We solve this system analytically to compute all the critical points of the least-squares cost function, and hence the global minimum, i.e., the globally optimal estimate for the orthogonal vanishing points. The same optimal estimator is used in conjunction with RANSAC to generate orthogonal-vanishing-point hypotheses (from triplets of lines) and thus classify lines into parallel and mutually orthogonal groups. The proposed method is validated experimentally on the York Urban Database.

Nonlinear estimation problems, such as bearingonly tracking, are often addressed using linearized estimators, e.g., the extended Kalman filter (EKF). These estimators generally suffer from linearization errors as well as the inability to track multimodal probability density functions (pdfs). In this paper, we propose a bank of batch maximum a posteriori (MAP) estimators as a general estimation framework that provides relinearization of the entire state history, multi-hypothesis tracking, and an efficient hypothesis generation scheme. Each estimator in the bank is initialized using a locally optimal state estimate for the current time step. Every time a new measurement becomes available, we convert the nonlinear cost function corresponding to this relaxed one-step subproblem into polynomial form, allowing to analytically and efficiently compute all stationary points. This local optimization generates highly probable hypotheses for the target trajectory and greatly improves the quality of the overall MAP estimate. Additionally, pruning and marginalization are employed to control the computational cost. Monte Carlo simulations and real-world experiments show that the proposed approach significantly outperforms the EKF, the standard batch MAP estimator, and the particle filter (PF), in terms of accuracy and consistency.

We present a new strategy for active vision-based localization and navigation of a mobile robot in a visual memory, i.e., within a previously-visited area represented as a large collection of images. Vision-based localization in such a large and dynamic visual map is intrinsically ambiguous, since more than one map-locations can exhibit the same visual appearance as the current image observed by the robot. Most existing approaches are passive, i.e., they do not devise any strategy to resolve this ambiguity. In this work, we present an active vision-based localization and navigation strategy that can disambiguate the true initial location among possible hypotheses by controlling the mobile observer across a sequence of highly distinctive images, while concurrently navigating towards the target image. The performance of our active localization and navigation algorithm is demonstrated experimentally on a robot moving within a large outdoor environment.

Correspondences between 2D lines in an image and 3D lines in the surrounding environment can be exploited to determine the camera's position and attitude (pose). In this paper, we introduce a novel approach to estimate the camera's pose by directly solving the corresponding least-squares problem algebraically. Specifically, the optimality conditions of the least-squares problem form a system of polynomial equations, which we efficiently solve through the eigendecomposition of a so-called multiplication matrix. Contrary to existing methods, the proposed algorithm (i) is guaranteed to find the globally optimal estimate in the least-squares sense, (ii) does not require initialization, and (iii) has computational cost only linear in the number of measurements. The superior performance of the proposed algorithm compared to previous approaches is demonstrated through extensive simulations and experiments.

In this paper, we address the problem of motion induced 3D robot-to-robot extrinsic calibration based on different combinations of inter-robot measurements (i.e., distance and/or bearing observations) and ego-motion estimates, recorded across multiple time steps. In particular, we focus on solving minimal problems where the unknown 6-degree of-freedom transformation between two robots is determined based on the minimum number of measurements necessary for finding a discrete set of solutions. In our previous work [1], we have shown that only 14 base systems need to be solved, and provided closed-form solutions for three of them. This paper considers the remaining systems and provides closed-form solutions to most of them, while for some of the most challenging problems, we introduce efficient symbolic-numerical solution methods. Finally, we evaluate the performance of our proposed solvers through extensive simulations.

A sliding window filter (SWF) is an appealing smoothing algorithm for nonlinear estimation problems such as simultaneous localization and mapping (SLAM), since it is resource-adaptive by controlling the size of the sliding window, and can better address the nonlinearity of the problem by relinearizing available measurements. However, due to the marginalization employed to discard old states from the sliding window, the standard SWF has different parameter observability properties from the optimal batch maximum-a-posterior (MAP) estimator. Specifically, the nullspace of the Fisher information matrix (or Hessian) has lower dimension than that of the batch MAP estimator. This implies that the standard SWF acquires spurious information, which can lead to inconsistency. To address this problem, we propose an observability-constrained (OC)-SWF where the linearization points are selected so as to ensure the correct dimension of the nullspace of the Hessian, as well as minimize the linearization errors. We present both Monte Carlo simulations and real-world experimental results which show that the OC-SWF's performance is superior to the standard SWF, in terms of both accuracy and consistency.

In this paper, we consider the problem of multi-centralized Cooperative Localization (CL) under severe communication constraints, i.e., when each robot can communicate only a single bit per real-valued (analog) measurement. Existing approaches, such as those based on the Sign-of-Innovation Kalman filter (SOI-KF) and its variants, require each robot to process quantized versions of both its local (i.e., recorded by its own sensors) and remote (i.e., collected by other robots) measurements. This results in suboptimal performance since each robot has to discard information that is available in its own analog measurements. To address this limitation, we introduce a novel hybrid estimation scheme that enables each robot to process both quantized (from remote sensors) and analog (from its own sensors) measurements. Specifically, we first present the hybrid (H)-SOI-KF, a direct extension of the SOI-KF, for processing both types of measurements. Secondly, we introduce the modified (M)H-SOI-KF, that uses an asymmetric encoding/decoding scheme to incorporate additional information during quantization (based on the hybrid estimates locally available to each robot), resulting in substantial accuracy improvement. Lastly, we present extensive simulations which demonstrate that both hybrid estimators not only outperform the SOI-KF, but also achieve accuracy comparable to that of the standard (analog) centralized Kalman filter.

This paper addresses the problem of determining the optimal robot trajectory for localizing a robot follower in a leader-follower formation using robot-to-robot distance or bearing measurements. In particular, maintaining a perfect formation has been shown to reduce the localization accuracy (as compared to moving randomly), or even leads to loss of observability when only distance or bearing measurements are available and the robots move on parallel straight lines. To address this limitation, we allow the follower to slightly deviate from its desired formation-imposed position and seek to find the next best location where it should move to in order to minimize the uncertainty about its relative, with respect to the leader, position and orientation estimates. We formulate and solve this non-convex optimization problem analytically and show, through extensive simulations, that the proposed optimized motion strategy leads to significant localization accuracy improvement as compared to competing approaches.

In this paper, we study the problem of optimal trajectory generation for a team of heterogeneous robots moving in a plane and tracking a moving target by processing relative observations, i.e., distance and/or bearing. Contrary to previous approaches, we explicitly consider limits on the robots' speed and impose constraints on the minimum distance at which the robots are allowed to approach the target. We first address the case of a single tracking sensor and seek the next sensing location in order to minimize the uncertainty about the target's position. We show that although the corresponding optimization problem involves a nonconvex objective function and a nonconvex constraint, its global optimal solution can be determined analytically. We then extend the approach to the case of multiple sensors and propose an iterative algorithm, i.e., the Gauss-Seidel relaxation (GSR), to determine the next best sensing location for each sensor. Extensive simulation results demonstrate that the GSR algorithm, whose computational complexity is linear in the number of sensors, achieves higher tracking accuracy than gradient descent methods and has performance that is indistinguishable from that of a grid-based exhaustive search, whose cost is exponential in the number of sensors. Finally, through experiments, we demonstrate that the proposed GSR algorithm is robust and applicable to real systems.

A distributed set-membership-constrained particle filter (SMCPF) is developed for decentralized tracking applications using wireless sensor networks. Unlike existing PF alternatives, SMC-PF offers reduced overhead for inter-sensor communications because it requires only particle weights to be exchanged among sensors, instead of raw measurements or parameters of a Gaussian mixture model. SMC-PF relies on a novel distributed adaptation scheme based on successive set intersections that can afford reduced number of particles without sacrificing performance. Conditions are provided to quantify the variance reduction of the SMC-PF-based state estimator. Simulations corroborate the ability of the SMCPF to considerably outperform the bootstrap PF for a fixed number of particles.

This paper presents a novel 3D indoor Laser-aided Inertial Navigation System (L-INS) for the visually impaired. An Extended Kalman Filter (EKF) fuses information from an Inertial Measurement Unit (IMU) and a 2D laser scanner, to concurrently estimate the six degree-of-freedom (d.o.f.) position and orientation (pose) of the person and a 3D map of the environment. The IMU measurements are integrated to obtain pose estimates, which are subsequently corrected using line-to-plane correspondences between linear segments in the laser-scan data and orthogonal structural planes of the building. Exploiting the orthogonal building planes ensures fast and efficient initialization and estimation of the map features while providing human-interpretable layout of the environment. The L-INS is experimentally validated by a person traversing a multistory building, and the results demonstrate the reliability and accuracy of the proposed method for indoor localization and mapping.

In this paper, we address the problem of determining the relative position and orientation (pose) of two robots navigating in 2D, based on known egomotion and noisy robot-to-robot distance measurements. We formulate this as a weighted Least Squares (WLS) estimation problem, and determine the exact global optimum by directly solving the multivariate polynomial system resulting from the first-order optimality conditions. Given the poor scalability of the original WLS problem, we propose an alternative formulation of the WLS problem in terms of squared distance measurements (squared distances WLS or SD-WLS). Using a hybrid algebraic-numeric technique, we are able to solve the corresponding first-order optimality conditions of the SD-WLS in 125 ms in Matlab. Both methods solve the minimal (3 distance measurements) as well as the overdetermined problem (more than 3 measurements) in a unified fashion. Simulation and experimental results show that the SD-WLS achieves performance virtually indistinguishable from the maximum likelihood estimator, and significantly outperforms current algebraic methods.

This paper presents a strategy for descending-stair detection, approach, and traversal using inertial sensing and a monocular camera mounted on an autonomous tracked vehicle. At the core of our algorithm are vision modules that exploit texture energy, optical flow, and scene geometry (lines) in order to robustly detect descending stairwells during both far- and near-approaches. As the robot navigates down the stairs, it estimates its three-degrees-of-freedom (d.o.f.) attitude by fusing rotational velocity measurements from an on-board tri-axial gyroscope with line observations of the stair edges detected by its camera. We employ a centering controller, derived based on a linearized dynamical model of our system, in order to steer the robot along safe trajectories. A real-time implementation of the described algorithm was developed for an iRobot Packbot, and results from real-world experiments are presented.

This paper presents a generalized framework for inter-robot information-transfer schemes in Multi-Centralized Cooperative Localization (MC-CL) under asynchronous communication, i.e., when the communication graph associated with the mobile robot network is time-varying and intermittently disconnected. Specifically, two information-transfer schemes, which differ based on their communication bandwidth requirements per link, are discussed. Even under asynchronous communication constraints, these schemes enable robots to compute pose estimates identical to those generated using the centralized CL framework, albeit delayed. For each of these schemes, necessary and sufficient conditions for the communication-graph connectivity, that enable each robot to generate the centralized estimates, are developed. Moreover, detailed description of these schemes, along with their communication-complexity analysis and analytical results for the expected time delay in obtaining these estimates, are presented. Lastly, simulation results are used to validate the performance (the trade-off between communication link bandwidth and accuracy/delay) of these information-transfer schemes.

In this paper, we address the problem of motioninduced 3D extrinsic calibration based on different combinations of inter-robot measurements (i.e., distance and/or bearing observations from either or both of the two robots, recorded across multiple time steps) and ego-motion estimates. In particular, we focus on solving minimal problems where the unknown 6-degree-of-freedom transformation between the two robots is determined based on the minimum number of measurements necessary for finding a discrete set of, in general, multiple solutions. In order to address the very large number of possible combinations of inter-robot observations, we identify symmetries in these problems and use them to prove that any of the possible extrinsic robot-to-robot calibration problems can be solved based on the solution of only 14 (base) minimal problems. Finally, we derive analytical solutions to three of these base problems, and evaluate their performance through extensive simulations.

In this paper, we provide a study of motion-induced 3-D extrinsic calibration based on robot-to-robot sensor measurements. In particular, we introduce algebraic methods to compute the relative translation and rotation between two robots using known robot motion and robot-to-robot 1) distance and bearing, 2) bearing-only, and 3) distance-only measurements. We further conduct a nonlinear observability analysis and provide sufficient conditions for the 3-D relative position and orientation (pose) to become locally weakly observable. Finally, we present a nonlinear weighted least-squares estimator to refine the algebraic pose estimate in the presence of noise. We use simulations to evaluate the performance of our methods in terms of accuracy and robustness.

This paper addresses two key limitations of the unscented Kalman filter (UKF) when applied to the simultaneous localization and mapping (SLAM) problem: the cubic, in the number of states, computational complexity, and the inconsistency of the state estimates. In particular, we introduce a new sampling strategy that minimizes the linearization error and whose computational complexity is constant (i.e., independent of the size of the state vector). As a result, the overall computational complexity of UKF-based SLAM becomes of the same order as that of the extended Kalman filter (EKF) when applied to SLAM. Furthermore, we investigate the observability properties of the linear-regression-based model employed by the UKF, and propose a new algorithm, termed the observability-constrained (OC)-UKF, that improves the consistency of the state estimates. The superior performance of the OC-UKF compared to the standard UKF and its robustness to large linearization errors are validated by extensive simulations.

This paper presents a distributed Maximum A Posteriori (MAP) estimator for multi-robot Cooperative Localization (CL). As opposed to centralized MAP-based CL, the proposed algorithm reduces the memory and processing requirements by distributing data and computations amongst the robots. Specifically, a distributed data-allocation scheme is presented that enables robots to simultaneously process and update their local data. Additionally, a distributed Conjugate Gradient algorithm is employed that reduces the cost of computing the MAP estimates, while utilizing all available resources in the team and increasing robustness to single-point failures. Finally, a computationally efficient distributed marginalization of past robot poses is introduced for limiting the size of the optimization problem. The communication and computational complexity of the proposed algorithm is described in detail, while extensive simulation studies are presented for validating the performance of the distributed MAP estimator and comparing its accuracy to that of existing approaches.

This paper addresses the problem of cooperative localization (CL) under severe communication constraints. Specifically, we present minimum mean square error (MMSE) and maximum a posteriori (MAP) estimators that can process measurements quantized with as little as one bit per measurement. During CL, each robot quantizes and broadcasts its measurements and receives the quantized observations of its teammates. The quantization process is based on the appropriate selection of thresholds, computed using the current state estimates, that minimize the estimation error metric considered. Extensive simulations demonstrate that the proposed Iteratively-Quantized Extended Kalman filter (IQEKF) and the Iteratively Quantized MAP (IQMAP) estimator achieve performance indistinguishable of that of their real-valued counterparts (EKF and MAP, respectively) when using as few as 4 bits for quantizing each robot measurement.

This paper presents an indoor localization system for the visually impaired. The basis of our system is an Extended Kalman Filter (EKF) for six degree-of-freedom (d.o.f.) position and orientation (pose) estimation. The sensing platform consists of an Inertial Measurement Unit (IMU) and a 2D laser scanner. The IMU measurements are integrated to obtain pose estimates which are subsequently corrected using line-to-plane correspondences between linear segments in the laser-scan data and known 3D structural planes of the building. Furthermore, we utilize Lie derivatives to show that the system is observable when at least three planes are detected by the laser scanner. Experimental results are presented that demonstrate the reliability of the proposed method for accurate and realtime indoor localization.

In this paper, we study the problem of optimal trajectory generation for a team of mobile robots tracking a moving target using distance and bearing measurements. Contrary to previous approaches, we explicitly consider limits on the robots' speed and impose constraints on the minimum distance at which the robots are allowed to approach the target. We first address the case of a single sensor and show that although this problem is non-convex with non-convex constraints, in general, its optimal solution can be determined analytically. Moreover, we extend this approach to the case of multiple sensors and propose an iterative algorithm, Gauss-Seidel-relaxation (GSR), for determining the set of feasible locations that each sensor should move to in order to minimize the uncertainty about the position of the target. Extensive simulation results are presented demonstrating that the performance of the GSR algorithm, whose computational complexity is linear in the number of sensors, is indistinguishable of that of a grid-based exhaustive search, with cost exponential in the number of sensors, and significantly better than that of a random, towards the target, motion strategy.

In this paper, we present the vision-aided inertial navigation (VISINAV) algorithm that enables precision planetary landing. The vision front-end of the VISINAV system extracts 2-D-to-3-D correspondences between descent images and a surface map (mapped landmarks), as well as 2-D-to-2-D feature tracks through a sequence of descent images (opportunistic features). An extended Kalman filter (EKF) tightly integrates both types of visual feature observations with measurements from an inertial measurement unit. The filter computes accurate estimates of the lander's terrain-relative position, attitude, and velocity, in a resource-adaptive and hence real-time capable fashion. In addition to the technical analysis of the algorithm, the paper presents validation results from a sounding-rocket test flight, showing estimation errors of only 0.16 m/s for velocity and 6.4 m for position at touchdown. These results vastly improve current state of the art for terminal descent navigation without visual updates, and meet the requirements of future planetary exploration missions.

We consider state estimation of a Markov stochastic process using an ad hoc wireless sensor network (WSN) based on noisy linear observations. Due to power and bandwidth constraints present in resource- limited WSNs, the observations are quantized before transmission. We derive a distributed recursive mean-square error (MSE) optimal quantizer-estimator based on the quantized observations. The resultant Kalman-like algorithm based on quantized observations exhibits MSE performance and computational complexity comparable to the Kalman filter based on un-quantized observations even for 2-3 bits of quantization per observation.

In this work, we study the inconsistency of EKF-based SLAM from the perspective of observability. We analytically prove that when the Jacobians of the state and measurement models are evaluated at the latest state estimates during every time step, the linearized error-state system model of the EKF-based SLAM has observable subspace of dimension higher than that of the actual, nonlinear, SLAM system. As a result, the covariance estimates of the EKF undergo reduction in directions of the state space where no information is available, which is a primary cause of inconsistency. To address this issue, a new “First Estimates Jacobian” (FEJ) EKF is proposed, which is shown to perform better in terms of consistency. In the FEJ-EKF, the filter Jacobians are calculated using the first-ever available estimates for each state variable, which insures that the observable subspace of the error-state system model is of the same dimension as that of the underlying nonlinear SLAM system. The theoretical analysis is validated through extensive simulations.

This paper presents a method for estimating the six-degrees-of-freedom transformation between a camera and the body of the robot on which it is rigidly attached. The robot maneuvers in front of a planar mirror, allowing the camera to observe fiducial features on the robot from several vantage points. Exploiting these measurements, we form a maximum-likelihood estimate of the camera-to-body transformation, without assuming prior knowledge of the robot motion or of the mirror configuration. Additionally, we estimate the mirror configuration with respect to the camera for each image. We validate the accuracy and correctness of our method with simulations and real-world experiments.

Vision-aided inertial navigation systems (V-INSs) canprovide precise state estimates for the 3-D motion of a vehicle when no external references (e.g., GPS) are available. This is achieved bycombining inertial measurements from an inertial measurement unit (IMU) with visual observations from a camera under the assumption that the rigid transformation between the two sensors is known. Errors in the IMU-camera extrinsic calibration process cause biases that reduce the estimation accuracy and can even lead to divergence of any estimator processing the measurements from both sensors. In this paper, we present an extended Kalman filter for precisely determining the unknown transformation between a camera and an IMU. Contrary to previous approaches, we explicitly account for the time correlation of the IMU measurements and provide a figure of merit (covariance) for the estimated transformation. The proposed method does not require any special hardware (such as spin table or 3-D laser scanner) except a calibration target. Furthermore, we employ the observability rank criterion based on Lie derivatives and prove that the nonlinear system describing the IMU-camera calibration process is observable. Simulation and experimental results are presented that validate the proposed method and quantify its accuracy.

In this paper, we study the problem of optimal trajectory generation for a team of mobile sensors tracking a moving target using distance-only measurements. This problem is shown to be NP-hard, in general, when constraints are imposed on the speed of the sensors. We propose two algorithms, modified Gauss-Seidel relaxation and linear programming (LP) relaxation, for determining the set of feasible locations that each sensor should move to in order to collect the most informative measurements; i.e., distance measurements that minimize the uncertainty about the position of the target. These algorithms are applicable regardless of the process model that is employed for describing the motion of the target, while their computational complexity is linear in the number of sensors. Extensive simulation results are presented demonstrating that the performance attained with the proposed methods is comparable to that obtained with grid-based exhaustive search, whose computational cost is exponential in the number of sensors, and significantly better than that of a random, toward the target, motion strategy.

In this paper, we address the problem of determining the 2-D relative pose of pairs of communicating robots from (1) robot-to-robot distance measurements and (2) displacement estimates expressed in each robot's reference frame. Specifically, we prove that for nonsingular configurations, the minimum number of distance measurements required for determining all six possible solutions for the 3 degree-of-freedom (3-DOF) robot-to-robot transformation is 3. Additionally, we show that given four distance measurements, the maximum number of solutions is 4, while five distance measurements are sufficient for uniquely determining the robot-to-robot transformation. Furthermore, we present an efficient algorithm for computing the unique solution in closed form and describe an iterative least-squares process for improving its accuracy. Finally, we derive necessary and sufficient observability conditions based on Lie derivatives and evaluate the performance of the proposed estimation algorithms both in simulation and via experiments.

This paper presents an indoor human localization system for the visually impaired. A prototype portable device has been implemented, consisting of a pedometer and a standard white cane, on which a laser range finder and a 3-axis gyroscope have been mounted. A novel pose estimation algorithm has been developed for robustly estimating the heading and position of a person navigating in a known building. The basis of our estimation scheme is a two-layered extended Kalman filter (EKF) for attitude and position estimation. The first layer maintains an attitude estimate of the white cane, which is subsequently provided to the second layer where a position estimate of the user is generated. Experimental results are presented that demonstrate the reliability of the proposed method for accurate, real-time human localization.

In this paper, we study the accuracy of cooperative localization and target tracking (CLATT) in a team of mobile robots, and derive analytical upper bounds for the position uncertainty. The obtained bounds provide a description of the asymptotic positioning performance of the robots and the targets as a function of the sensor characteristics and the structure of the graph of relative position measurements. By employing an extended Kalman filter (EKF) formulation for data fusion, two key asymptotic results are derived. The first provides the guaranteed worst-case positioning accuracy, whereas the second determines an upper bound on the expected covariance of the estimates. We investigate the effects of jointly estimating the targets' and the robots' position, and demonstrate that it results in better accuracy for the robots' position estimates. The theoretical results are confirmed both in simulation and experimentally.

In this paper, we present an extended Kalman filter (EKF)-based algorithm for real-time vision-aided inertial navigation. The primary contribution of this work is the derivation of a measurement model that is able to express the geometric constraints that arise when a static feature is observed from multiple camera poses. This measurement model does not require including the 3D feature position in the state vector of the EKF and is optimal, up to linearization errors. The vision-aided inertial navigation algorithm we propose has computational complexity only linear in the number of features, and is capable of high-precision pose estimation in large-scale real-world environments. The performance of the algorithm is demonstrated in extensive experimental results, involving a camera/IMU system localizing within an urban area.

In this paper we address the problem of determining the relative pose of pairs robots that move on a plane while measuring the distance to each other. We show that the minimum number of distance measurements required for the 3 degrees of freedom robot-to-robot transformation to become locally observable is 3. Furthermore, we prove that the maximum number of possible solutions in this case is 6, while a minimum of 5 distance measurements is necessary in order to uniquely determine the robots' relative pose. Finally, we present efficient algorithms for computing all possible solutions and evaluate the validity of our theoretical results both in simulation and experimentally.

Vision-aided Inertial Navigation Systems (V-INS) can provide precise state estimates for the 3D motion of a vehicle when no external references (e.g., GPS) are available. This is achieved by combining inertial measurements from an IMU with visual observations from a camera under the assumption that the rigid transformation between the two sensors is known. Errors in the IMU-camera calibration process causes biases that reduce the accuracy of the estimation process and can even lead to divergence. In this paper, we present a Kalman filter-based algorithm for precisely determining the unknown transformation between a camera and an IMU. Contrary to previous approaches, we explicitly account for the time correlations of the IMU measurements and provide a figure of merit (covariance) for the estimated transformation. The proposed method does not require any special hardware (such as spin table or 3D laser scanner) except a calibration target. Simulation and experimental results are presented that validate the proposed method and quantify its accuracy.

In this paper, we present an extended Kalman filter (EKF)-based estimator for simultaneous localization and mapping (SLAM) with processing requirements that are linear in the number of features in the map. The proposed algorithm is based on three key ideas. Firstly, by introducing the global-map postponement method, approximations necessary for ensuring linear computational complexity are delayed over many time steps. Then by employing the power method, only the most informative of the Kalman vectors, generated during the postponement phase, are retained for updating the covariance matrix. This in effect minimizes the information loss during each approximation epoch. Finally, linear-complexity, rank-2 updates, which minimize the trace of the covariance matrix, are applied to increase the speed of convergence of the estimator. In addition to being consistent, the resulting estimator has processing requirements that can be adjusted to the availability of computational resources. Simulation results are presented that demonstrate the accuracy of the proposed algorithm (Power-SLAM) when compared to the quadratic computational cost standard EKF-based SLAM, and two linear- complexity competing alternatives.

In this paper, we develop an algorithm for determining the relative position and attitude of two robots moving in 3D, using only dead-reckoning and inter-robot distance measurements. From the mechanical analogue of parallel manipulators, it is known that this problem has 40 solutions when six distance measurements are available. These general solutions are not known in closed-form, and existing closed-form solutions require additional bearing sensors, or impose strong constraints on the geometric structure of the manipulator (in our case, the robots’ trajectories). This paper presents, for the first time, an efficient, algebraic algorithm to solve the relative pose using 10 distance measurements, without imposing any constraints on the robots’ motion. We further present a weighted leastsquares refinement step, and validate our algorithm in various simulations, demonstrating its efficiency and accuracy.

This paper presents a new method to optimally combine motion measurements provided by proprioceptive sensors, with relative-state estimates inferred from feature-based matching. Two key challenges arise in such pose tracking problems: 1) the displacement estimates relate the state of the robot at two different time instants and 2) the same exteroceptive measurements are often used for computing consecutive displacement estimates, a process that renders the errors in these correlated. We present a novel stochastic cloning Kalman filtering (SC-KF) estimation algorithm that successfully addresses these challenges, while still allowing for efficient calculation of the filter gains and covariances. The proposed algorithm is not intended to compete with simultaneous localization and mapping (SLAM) approaches. Instead, it can be merged with any extended-Kalman- filter-based SLAM algorithm to increase its precision. In this respect, the SC-KF provides a robust framework for leveraging additional motion information extracted from dense point features that most SLAM algorithms do not treat as landmarks. Extensive experimental and simulation results are presented to verify the validity of the proposed method and to demonstrate that its performance is superior to that of alternative position-tracking approaches.

We derive and analyze distributed state estimators of dynamical stochastic processes, whereby low communication cost is effected by requiring the transmission of a single bit per observation. Following a Kalman filtering (KF) approach, we develop recursive algorithms for distributed state estimation based on the sign of innovations (SOI). Even though SOI-KF can afford minimal communication overhead, we prove that in terms of performance and complexity it comes very close to the clairvoyant KF which is based on the analog-amplitude observations. Reinforcing our conclusions, we show that the SOI-KF applied to distributed target tracking based on distance only observations yields accurate estimates at low communication cost

In this paper, we study the problem of localization using relative-state estimates. It is shown, that when the same exteroceptive sensor measurement is processed for the computation of two consecutive displacement estimates (both forward and backward in time) these estimates are correlated, and an analysis of the exact structure of the correlations is performed. This analysis is utilized in the design of data fusion algorithms, that correctly account for the existing correlations. We examine two cases: i) pose propagation based exclusively on inferred displacement measurements, and ii) the fusion of proprioceptive sensor information with relative-state measurements. For the latter case, an efficient EKF-based estimation algorithm is proposed, that extends the approach of S.I. Roumeliotis and J. Burdick, 2002. Extensive simulation and experimental results are presented, that verify the validity of the presented method

Bearing-only SLAM describes the process of simultaneously localizing a mobile robot while building a map of the unknown surroundings, using bearing measurements to landmarks as the only available exteroceptive sensor information. Commonly, the position of map features is estimated along with the robot pose. However, consistent initialization of these positions is a difficult problem in bearing-only SLAM, in particular for distant landmarks. In previous approaches, measurements to remote landmarks often had to be discarded, thus losing valuable orientation information. In this paper, we present for the first time a unifying framework allowing for non-delayed initialization of both nearby and distant features. This is made possible by a four-element landmark parametrization, combined with a constraint-based inferred measurement

This paper presents a new approach to the multi-robot map-alignment problem that enables teams of robots to build joint maps without initial knowledge of their relative poses. The key contribution of this work is an optimal algorithm for merging (not necessarily overlapping) maps that are created by different robots independently. Relative pose measurements between pairs of robots are processed to compute the coordinate transformation between any two maps. Noise in the robot-to-robot observations, propagated through the map-alignment process, increases the error in the position estimates of the transformed landmarks, and reduces the overall accuracy of the merged map. When there is overlap between the two maps, landmarks that appear twice provide additional information, in the form of constraints, which increases the alignment accuracy. Landmark duplicates are identified through a fast nearest-neighbor matching algorithm. In order to reduce the computational complexity of this search process, a kd-tree is used to represent the landmarks in the original map. The criterion employed for matching any two landmarks is the Mahalanobis distance. As a means of validation, we present experimental results obtained from two robots mapping an area of 4,800 m2

This paper studies the accuracy of position estimation for groups of mobile robots performing cooperative localization. We consider the case of teams comprised of possibly heterogeneous robots and provide analytical expressions for the upper bound on their expected positioning uncertainty. This bound is determined as a function of the sensors' noise covariance and the eigenvalues of the relative position measurement graph (RPMG), i.e., the weighted directed graph which represents the network of robot-to-robot exteroceptive measurements. The RPMG is employed as a key element in this analysis, and its properties are related to the localization performance of the team. It is shown that, for a robot group of a certain size, the maximum expected rate of uncertainty increase is independent of the accuracy and number of relative position measurements and depends only on the accuracy of the proprioceptive and orientation sensors on the robots. Additionally, the effects of changes in the topology of the RPMG are studied, and it is shown that, at steady-state, these reconfigurations do not inflict any loss in localization precision. Experimental data, as well as simulation results that validate the theoretical analysis, are presented

This paper addresses the problem of resource allocation in formations of mobile robots localizing as a group. Each robot receives measurements from various sensors that provide relative (robot-to-robot) and absolute positioning information. Constraints on the sensors' bandwidth, as well as communication and processing requirements, limit the number of measurements that are available or can be processed at each time step. The localization uncertainty of the group, determined by the covariance matrix of the equivalent continuous-time system at steady state, is expressed as a function of the sensor measurements' frequencies. The trace of the weighted covariance matrix is selected as the optimization criterion, under linear constraints on the measuring frequency of each sensor and the cumulative rate of the extended Kalman filter updates. This formulation leads to a convex optimization problem (semidefinite program) whose solution provides the sensing frequencies, for each sensor on every robot, required in order to maximize the positioning accuracy of the group. Simulation and experimental results are presented that demonstrate the applicability of this method and provide insight into the properties of the resource-constrained cooperative localization problem

This paper addresses the problem of estimating the parameters of the advection-diffusion equation, which describes the propagation of an instantaneously released gas. A team of mobile robots, equipped with appropriate sensing devices, are used for collecting gas concentration measurements. In addition, each of the robots has sensitive anemometric vanes for determining the velocity and the direction of the wind. The selection of the sequence of locations, where the robot should go to collect the gas concentration measurements, is performed in real-time and is based on the minimization of the uncertainty of the estimated parameters and on reducing the time to convergence of this estimation problem. Simulation results are presented in order to confirm the described approach, which has significantly lower computational requirements than other well-known techniques based on exhaustive global search.

This paper presents a new approach for estimating in real-time the parameters of the advection-diffusion equation that describes the propagation of an instantaneously released gas. A mobile robot equipped with an appropriate sensing device collects measurements in order to estimate the parameters of this equation. The selection of the set of locations where chemical concentration measurements should be recorded, is performed in real-time with the objective of maximizing the accuracy of the parameter estimates and reducing the time to convergence of this estimation problem. Simulation results are presented that validate the described approach, which has significantly lower computational requirements compared to alternative motion strategies based on exhaustive global search.

This paper studies the effects of the geometry of a mobile robot formation on the accuracy of the robots’ localization. The general case of heterogeneous (in terms of sensor accuracy) robot teams performing Cooperative Localization is considered. An analysis of the time evolution of the covariance matrix of the position estimates allows us to express the steady-state positioning uncertainty of the robots as an analytic function of the relative positions of the robots in the formation. This metric encapsulates the effect of formation geometry on the information content of the exteroceptive measurements, as well as the effect of the influx of uncertainty due to the errors in the robots’ odometry. Thus, by minimizing the trace of the steady state covariance matrix with respect to the positions of the robots, the optimal robot configuration can be determined. Numerical experiments are presented, which indicate that it is possible to derive a practical rule for determining optimal formations, without the need to resort to extensive simulations, or experimentation.

A system that enables continuous slip compensation for a Mars rover has been designed, implemented, and field-tested. This system is composed of several components that allow the rover to accurately and continuously follow a designated path, compensate for slippage, and reach intended goals in high-slip environments. These components include: visual odometry, vehicle kinematics, a Kalman filter pose estimator, and a slip compensation/path follower. Visual odometry tracks distinctive scene features in stereo imagery to estimate rover motion between successively acquired stereo image pairs. The vehicle kinematics for a rocker-bogie suspension system estimates motion by measuring wheel rates, and rocker, bogie, and steering angles. The Kalman filter merges data from an inertial measurement unit (IMU) and visual odometry. This merged estimate is then compared to the kinematic estimate to determine how much slippage has occurred, taking into account estimate uncertainties. If slippage has occurred then a slip vector is calculated by differencing the current Kalman filter estimate from the kinematic estimate. This slip vector is then used to determine the necessary wheel velocities and steering angles to compensate for slip and follow the desired path.

This work studies the localization accuracy of a team of mobile robots that perform cooperative localization. We provide an analytical expression for the upper bound in positioning accuracy as a function of a weighted connectivity graph for the network of relative position measurements in the robot group. This network has a time-varying topology determined by the availability of relative position measurements between pairs of robots. The weights of the connectivity graph depend on (i) the odometric and orientation accuracy of each robot, and (ii) the accuracy of the robot tracker on each member of the team that measures its relative position with respect to other robots in the group. The theoretical results are validated by extensive simulations.

This paper compares the performances of several algorithms that address the problem of Simultaneous Localization and Mapping (SLAM) for the case of very small, resource-limited robots. These robots have poor odometry and can typically only carry a single monocular camera. These algorithms do not make the typical SLAM assumption that metric distance/bearing information to landmarks is available. Instead, the robot registers a distinctive sensor "signature", based on its current location, which is used to match robot positions. The performances of a physics-inspired maximum likelihood (ML) estimator, the iterated form of the Extended Kalman Filter (IEKF), and a batch-processed linearized ML estimator are compared under various odometric noise models.

This paper studies the time evolution of the covariance of the position estimates in single-robot simultaneous localization and mapping (SLAM). A closed-form expression is derived, that establishes a functional relation between the noise parameters of the robot's proprioceptive and exteroceptive sensors, the number of features being mapped, and the attainable accuracy of SLAM. Furthermore, it is demonstrated how prior information about the spatial density of landmarks can be utilized in order to compute a tight upper bound on the expected covariance of the positioning errors. The derived closed-form expressions enable the prediction of SLAM positioning performance, without resorting to extensive simulations, and thus offer an analytical tool for determining the sensor characteristics required to achieve a desired degree of accuracy. Simulation experiments are conducted, that corroborate the presented theoretical analysis.

This paper presents an algorithm to find the line-based map that best fits sets of two-dimensional range scan data. To construct the map, we first provide an accurate means to fit a line segment to a set of uncertain points via maximum likelihood formalism. This scheme weights each point's influence on the fit according to its uncertainty, which is derived from sensor noise models. We also provide closed-form formulas for the covariance of the line fit, along with methods to transform line coordinates and covariances across robot poses. A Chi-squared based criterion for "knitting" together sufficiently similar lines can be used to merge lines directly (as we demonstrate) or as part of the framework for a line-based SLAM implementation. Experiments using a Sick LMS-200 laser scanner and a Nomad 200 mobile robot illustrate the effectiveness of the algorithm.

This paper deals with the problem of cooperative localization for the case of large groups of mobile robots. A Kalman filter estimator is implemented and tested for this purpose. The focus of this paper is to examine the effect on localization accuracy of the number N of participating robots and the accuracy of the sensors employed. More specifically, we investigate the improvement in localization accuracy per additional robot as the size of the team increases. Furthermore, we provide an analytical expression for the upper bound on the positioning uncertainty increase rate for a team of N robots as a function of N, the odometric and orientation uncertainty for each robot, and the accuracy of a robot tracker measuring relative positions between pairs of robots. The analytical results derived in this paper are validated in simulation for different test cases.

This paper addresses the problem of simultaneous localization and mapping (SLAM) for the case of very small, resource-limited robots which have poor odometry and can typically only carry a single monocular camera. We propose a modification to the standard SLAM algorithm in which the assumption that the robots can obtain metric distance/bearing information to landmarks is relaxed. Instead, the robot registers a distinctive sensor "signature", based on its current location, which is used to match robot positions. In our formulation of this non-linear estimation problem, we infer implicit position measurements from an image recognition algorithm. The iterated form of the extended Kalman filter (IEKF) is employed to process all measurements.

Tracked mobile robots in the 20 kg size class are under development for applications in urban reconnaissance. For efficient deployment, it is desirable for teams of robots to be able to automatically execute path following behaviors, with one or more followers tracking the path taken by a leader. The key challenges to enabling such a capability are (1) to develop sensor packages for such small robots that can accurately determine the path of the leader and (2) to develop path following algorithms for the subsequent robots. To date, we have integrated gyros, accelerometers, compass/inclinometers, odometry, and differential GPS into an effective sensing package. The paper describes the sensor package, sensor processing algorithm and path tracking algorithm we have developed for the leader/follower problem in small robots and shows the results of performance characterization of the system. We also document pragmatic lessons learned about design, construction, and electromagnetic interference issues particular to the performance of state sensors on small robots.

Introduces a "weighted" matching algorithm to estimate a robot's planar displacement by matching two-dimensional range scans. The influence of each scan point on the overall matching error is weighted according to its uncertainty. We develop uncertainty models that account for effects such as measurement noise, sensor incidence angle, and correspondence error. Based on models of expected sensor uncertainty, our algorithm computes the appropriate weighting for each measurement so as to optimally estimate the displacement between two consecutive poses. By explicitly modeling the various noise sources, we can also calculate the actual covariance of the displacement estimates instead of a statistical approximation of it. A realistic covariance estimate is necessary for further combining the pose displacement estimates with additional odometric and/or inertial measurements within a localization framework. Experiments using a Nomad 200 mobile robot and a Sick LMS-200 laser range finder illustrate that the method is more accurate than prior techniques.

Introduces a generalized framework, termed "stochastic cloning," for processing relative state measurements within a Kalman filter estimator. The main motivation and application for this methodology is the problem of fusing displacement measurements with position estimates for mobile robot localization. Previous approaches have ignored the developed interdependencies (cross-correlation terms) between state estimates of the same quantities at different time instants. By directly expressing relative state measurements in terms of previous and current state estimates, the effect of these crosscorrelation terms on the estimation process is analyzed and considered during updates. Simulation and experimental results validate this approach.

Numerous upcoming NASA missions need to land safely and precisely on planetary bodies. Accurate and robust state estimation during the descent phase is necessary. Towards this end, we have developed an approach for improved state estimation by augmenting traditional inertial navigation techniques with image-based motion estimation (IBME). A Kalman filter that processes rotational velocity and linear acceleration measurements provided from an inertial measurement unit has been enhanced to accommodate relative pose measurements from the IBME. In addition to increased state estimation accuracy, IBME convergence time is reduced while robustness of the overall approach is improved. The methodology is described in detail and experimental results with a 5 DOF gantry testbed are presented.

Small, tracked mobile robots designed for general urban mobility have been developed for the purpose of reconnaissance and/or search and rescue missions in buildings and cities. Autonomous stair climbing is a significant capability required for many of these missions. In this paper we present the design and implementation of a new set of estimation and control algorithms that increase the speed and effectiveness of stair climbing. We have developed: (1) a Kalman filter that fuses visual/laser data with inertial measurements and provides attitude estimates of improved accuracy at a high rate; and (2) a physics based controller that minimizes the heading error and maximizes the effective velocity of the vehicle during stair climbing. Experimental results using a tracked vehicle validate the improved performance of this control and estimation scheme over previous approaches.

In this paper, we present a new approach to the problem of simultaneously localizing a group of mobile robots capable of sensing one another. Each of the robots collects sensor data regarding its own motion and shares this information with the rest of the team during the update cycles. A single estimator, in the form of a Kalman filter, processes the available positioning information from all the members of the team and produces a pose estimate for every one of them. The equations for this centralized estimator can be written in a decentralized form, therefore allowing this single Kalman filter to be decomposed into a number of smaller communicating filters. Each of these filters processes the sensor data collected by its host robot. Exchange of information between the individual filters is necessary only when two robots detect each other and measure their relative pose. The resulting decentralized estimation schema, which we call collective localization, constitutes a unique means for fusing measurements collected from a variety of sensors with minimal communication and processing requirements. The distributed localization algorithm is applied to a group of three robots and the improvement in localization accuracy is presented. Finally, a comparison to the equivalent decentralized information filter is provided.

We propose a method to detect and identify faults in wheeled mobile robots. The idea behind the method is to use adaptive estimation to predict the outcome of several faults, and to learn them collectively as a failure pattern. Models of the system behavior under each type of fault are embedded in multiple parallel Kalman filter (KF) estimators. Each KF is tuned to a particular fault and predicts, using its embedded model, the expected values for the sensor readings. The residual, the difference between the predicted readings (based on certain assumptions for the system model and the sensor models) and the actual sensor readings, is used as an indicator of how well each filter is performing. A backpropagation neural network processes this set of residuals as a pattern and decides which fault has occurred, that is, which filter is better tuned to the correct state of the mobile robot. The technique has been implemented on a physical robot and results from experiments are discussed.

This paper presents a new approach to the cooperative localization problem, namely collective localization. A group of M robots is viewed as a single system composed of robots that carry, in general, different sensors and have different positioning capabilities. A single Kalman filter is formulated to estimate the position and orientation of all the members of the group. This centralized schema is capable of fusing information provided by the sensors distributed on the individual robots while accommodating independencies and interdependencies among the collected data. In order to allow for distributed processing, the equations of the centralized Kalman filter are treated so that this filter can be decomposed in M modified Kalman filters each running on a separate robot. The collective localization algorithm is applied to a group of 3 robots and the improvement in localization accuracy is presented.

Decision and estimation theory are closely related topics in applied probability. In this paper, Bayesian hypothesis testing is combined with Kalman filtering to merge two different approaches to map-based mobile robot localization; namely Markov localization and pose tracking. A robot carries proprioceptive sensors that monitor its motion and allow it to estimate its trajectory as it moves away from a known location. A single Kalman filter is used for tracking the pose displacements of the robot between different areas. The robot is also equipped with exteroceptive sensors that seek for landmarks in the environment. Simple feature extraction algorithms process the incoming signals and suggest potential corresponding locations on the map. Bayesian hypothesis testing is applied in order to combine the continuous Kalman filter displacement estimates with the discrete landmark pose measurement events. Within this framework, also known as multiple hypothesis tracking, multimodal probability distribution functions can be represented and this inherent limitation of the Kalman filter is overcome.

A layered algorithm for extracting features in an indoor environment from planar range data is presented. At the lower (signal processing) level, the SEGMENTS algorithm exploits the speed and accuracy of two extended Kalman filters working in tandem and processes sequentially the distance measurements within each scan. The first Kalman filter is responsible for initiating straight line segments and detecting clutter while the second estimates the parameters of each line segment (such as distance and orientation) and determines when this is interrupted. The dual filter combination is capable of detecting edges and straight line segments within the scene in front of the robot. At the higher (post-processing) level, the identified segments are combined to form more complex features such as extended walls, corners (concave and convex), doors and corridors. During the composition cycle, SEGMENTS makes use of (i) the parametric representation of the straight line segments and (ii) the prespecific topological models of the features this algorithm seeks. A list of the identified features along with their location with respect to the laser sensor is finally available to the user. The cluttered regions in the scene are also marked on a polar representation of the environment. The presented algorithm has been tested on a Pioneer 2 DX mobile robot equipped with a SICK LMS 200 proximity laser scanner performing map-based localization. Low computational requirements, accuracy and robustness to uncertainty and noise characterize the performance of this new method for feature extraction.

The mobile robot localization problem is treated as a two-stage iterative estimation process. The attitude is estimated first and is then available for position estimation. The indirect (error state) form of the Kalman filter is developed for attitude estimation when applying gyro modeling. The main benefit of this choice is that combined dynamic modeling of the mobile robot and its interaction with the environment is avoided. The filter optimally combines the attitude rate information from the gyro and the absolute orientation measurements. The proposed implementation is independent of the structure of the vehicle or the morphology of the ground. The method can easily be transferred to another mobile platform provided it carries an equivalent set of sensors. The 2D case is studied in detail first. Results of extending the approach to the 3D case are presented. In both cases the results demonstrate the efficacy of the proposed method.

The mobile robot localization problem is decomposed into two stages; attitude estimation followed by position estimation. The innovation of our method is the use of a smoother, in the attitude estimation loop that outperforms other Kalman filter based techniques in estimate accuracy. The smoother exploits the special nature of the data fused; high frequency inertial sensor (gyroscope) data and low frequency absolute orientation data (from a compass or sun sensor). Two Kalman filters form the smoother. During each time interval one of them propagates the attitude estimate forward in time until it is updated by an absolute orientation sensor. At this time, the second filter propagates the recently renewed estimate back in time. The smoother optimally exploits the limited observability of the system by combining the outcome of the two filters. The system model uses gyro modeling which relies on integrating the kinematic equations to propagate the attitude estimates and obviates the need for complex dynamic modeling. The indirect (error state) form of the Kalman filter is developed for both parts of the smoother. The proposed approach is independent of the robot structure and the morphology of the ground. It can easily be transferred to another robot which has an equivalent set of sensors. Quaternions are used for the 3D attitude representation mainly for practical reasons discussed in the paper. Proposed innovative algorithm is tested in simulation and the overall improvement in position estimation is demonstrated.

A low cost strategy based on well calibrated odometry is presented for localizing mobile robots. The paper describes a two-step process for correction of 'systematic errors' in encoder measurements followed by fusion of the calibrated odometry with a gyroscope and GPS resulting in a robust localization scheme. A Kalman filter operating on data from the sensors is used for estimating position and orientation of the robot. Experimental results are presented that show an improvement of at least one order of magnitude in accuracy compared to the un-calibrated, un-filtered case. Our method is systematic, simple and yields very good results. We show that this strategy proves useful when the robot is using GPS to localize itself as well as when GPS becomes unavailable for some time. As a result robot can move in and out of enclosed spaces, such as buildings, while keeping track of its position on the fly.

Presents a technique to accurately estimate the state of a robot helicopter using a combination of gyroscopes, accelerometers, inclinometers and GPS. Simulation results of state estimation of the helicopter are presented using Kalman filtering based on sensor modeling. The number of estimated states of helicopter is nine : three attitudes(/spl theta/,/spl phi/,/spl psi/) from the gyroscopes, three accelerations(x/spl I.oarr/,y/spl I.oarr/,z/spl I.oarr/) and three positions (x, y, z) from the accelerometers. Two Kalman filters were used, one for the gyroscope data and the other for the accelerometer data. Our approach is unique because it explicitly avoids dynamic modeling of the system and allows for can elegant combination of sensor data available at different frequencies. We also describe the larger context in which this work is embedded, namely the design and implementation of an autonomous robot helicopter.

This paper introduces a method to detect and identify faults in wheeled mobile robots. The idea behind the method is to use adaptive estimation to predict (in parallel) the outcome of several faults. Models of the system behavior under each type of fault are embedded in the various parallel estimators (each of which is a Kalman filter). Each filter is thus tuned to a particular fault. Using its embedded model each filter predicts values for the sensor readings. The residual (the difference between the predicted and actual sensor reading) is an indicator of how well the filter is performing. A fault detection and identification module is responsible for processing the residual to decide which fault has occurred. As an example the method is implemented successfully on a Pioneer I robot. The paper concludes with a discussion of future work.

Multiple model adaptive estimation (MMAE) is used to detect and identify sensor failures in a mobile robot. Each estimator is a Kalman filter with a specific embedded failure model. The filter bank also contains one filter which has the nominal model embedded within it. The filter residuals are postprocessed to produce a probabilistic interpretation of the operation of the system. The output of the system at any given time is the confidence in the correctness of the various embedded models. As an additional feature the standard assumption that the measurements are available at a constant, common frequency, is relaxed. Measurements are assumed to be asynchronous and of varying frequency. The particularly difficult case of 'soft' sensor failure is also handled successfully. A system architecture is presented for the general problem of failure detection and identification in mobile robots. As an example, the MMAE algorithm is demonstrated on a Pioneer I robot in the case of three different sensor failures.