We present closed-form expressions for marginalizing and conditioning Gaussians onto linear manifolds, and demonstrate how to apply these expressions to smooth non-linear manifolds through linearization. Although marginalization and conditioning onto axis-aligned manifolds are well-established procedures, doing so onto non-axis-aligned manifolds is not as well understood. We demonstrate the utility of our expressions through three applications: 1) approximation of the projected normal distribution, where the quality of our linearized approximation increases as problem non-linearity decreases; 2) covariance extraction in Koopman SLAM, where our covariances are shown to be consistent on a real-world dataset; and 3) covariance extraction in constrained GTSAM, where our covariances are shown to be consistent in simulation.

This paper introduces a set of customizable and novel cost functions that enable the user to easily specify desirable robot formations, such as a "high-coverage" infrastructure-inspection formation, while maintaining high relative pose estimation accuracy. The overall cost function balances the need for the robots to be close together for good ranging-based relative localization accuracy and the need for the robots to achieve specific tasks, such as minimizing the time taken to inspect a given area. The formations found by minimizing the aggregated cost function are evaluated in a coverage path planning task in simulation and experiment, where the robots localize themselves and unknown landmarks using a simultaneous localization and mapping algorithm based on the extended Kalman filter. Compared to an optimal formation that maximizes ranging-based relative localization accuracy, these formations significantly reduce the time to cover a given area with minimal impact on relative pose estimation accuracy.

In this article, we present a framework for model-free batch localization and simultaneous localization and mapping (SLAM). We use lifting functions to map a control-affine system into a high-dimensional space, where both the process model and the measurement model are rendered bilinear. During training, we solve a least-squares problem using groundtruth data to compute the high-dimensional model matrices associated with the lifted system purely from data. At inference time, we solve for the unknown robot trajectory and landmarks through an optimization problem, where constraints are introduced to keep the solution on the manifold of the lifting functions. The problem is efficiently solved using a sequential quadratic program (SQP), where the complexity of an SQP iteration scales linearly with the number of timesteps. Our algorithms, called reduced constrained Koopman linearization localization (RCKL-Loc) and reduced constrained Koopman linearization SLAM (RCKL-SLAM), are validated experimentally in simulation and on two datasets: one with an indoor mobile robot equipped with a laser rangefinder that measures range to cylindrical landmarks, and one on a golf cart equipped with radio-frequency identification (RFID) range sensors. We compare RCKL-Loc and RCKL-SLAM with classic model-based nonlinear batch estimation. While RCKL-Loc and RCKL-SLAM have a similar performance compared to their model-based counterparts, they outperform the model-based approaches when the prior model is imperfect, showing the potential benefit of the proposed data-driven technique.

Many problems in robotics, such asestimating the state from noisy sensor data or aligning two point clouds, can be posed and solved as least-squares problems. Unfortunately, vanilla nonminimal solvers for least-squares problems are notoriously sensitive to outliers and initialization errors. The conventional approach to outlier rejection is to use a robust loss function, which is typically selected and tuned a priori. A newly developed approach to handle large initialization errors is graduated nonconvexity (GNC), which is defined for a particular choice of a robust loss function. The main contribution of this article is to combine these two approaches by using an adaptive kernel within a GNC optimization scheme. This brings a solution to least-squares problems that is robust to both outliers and initialization errors, without the need for model selection and tuning. Simulations and experiments demonstrate that the proposed method is more robust compared to non-GNC counterparts and performs on par with other GNC-tailored loss functions.

Ultra-wideband (UWB) systems are becoming increasingly popular as a means of inter-robot ranging and communication. A major constraint associated with UWB is that only one pair of UWB transceivers can range at a time to avoid interference, hence hindering the scalability of UWBbased localization. In this paper, a ranging protocol is proposed that allows all robots to passively listen on neighbouring communicating robots without any hierarchical restrictions on the role of the robots. This is utilized to allow each robot to obtain more range measurements and to broadcast preintegrated inertial measurement unit (IMU) measurements for relative extended pose state estimation directly on$SE_{2}(3)$. Consequently, a simultaneous clock-synchronization and relative-pose estimator (CSRPE) is formulated using an on-manifold extended Kalman filter (EKF) and is evaluated in simulation using Monte-Carlo runs for up to 7 robots. The ranging protocol is implemented in C on custom-made UWB boards fitted to 3 quadcopters, and the proposed filter is evaluated over multiple experimental trials, yielding up to 48% improvement in localization accuracy.

The recent development of high-precision subsea optical scanners allows for 3D keypoint detectors and feature descriptors to be leveraged on point cloud scans from subsea environments. However, the literature lacks a comprehensive survey to identify the best combination of detectors and descriptors to be used in these challenging and novel environments. This paper aims to identify the best detector/descriptor pair using a challenging field dataset collected using a commercial underwater laser scanner. Furthermore, studies have shown that incorporating texture information to extend geometric features adds robustness to feature matching on synthetic datasets. This paper also proposes a novel method of fusing images with underwater laser scans to produce coloured point clouds, which are used to study the effectiveness of 6D point cloud descriptors.

Ultra-Wideband (UWB) systems are becoming increasingly popular for indoor localization, where range measurements are obtained by measuring the time-of-flight of radio signals. However, the range measurements typically suffer from a systematic error or bias that must be corrected for high-accuracy localization. In this paper, a ranging protocol is proposed alongside a robust and scalable antenna-delay calibration procedure to accurately and efficiently calibrate antenna delays for many UWB tags. Additionally, the bias and uncertainty of the measurements are modelled as a function of the received-signal power. The full calibration procedure is presented using experimental training data of 3 aerial robots fitted with 2 UWB tags each, and then evaluated on 2 test experiments. A localization problem is then formulated on the experimental test data, and the calibrated measurements and their modelled uncertainty are fed into an extended Kalman filter (EKF). The proposed calibration is shown to yield an average of 46% improvement in localization accuracy. Lastly, the paper is accompanied by an open-source UWB-calibration Python library, which can be found at https://github.com/decargroup/uwb_calibration.

The ability to rapidly test a variety of algorithms for an arbitrary state estimation task is valuable in the prototyping phase of navigation systems. Lie group theory is now mainstream in the robotics community, and hence estimation prototyping tools should allow state definitions that belong to manifolds. A new package, called navlie, provides a framework that allows a user to model a large class of problems by implementing a set of classes complying with a generic interface. Once accomplished, navlie provides a variety of on-manifold estimation algorithms that can run directly on these classes. The package also provides a built-in library of common models, as well as many useful utilities. The open-source project can be found at https://github.com/decargroup/navlie

Indoor magnetic fields are a combination of Earth's magnetic field and disruptions induced by ferromag-netic objects, such as steel structural components in buildings. As a result of these disruptions, pervasive in indoor spaces, mag-netic field data is often omitted from navigation algorithms in indoor environments. This paper leverages the spatially-varying disruptions to Earth's magnetic field to extract positional information for use in indoor navigation algorithms. The algorithm uses a rate gyro and an array of four magnetometers to estimate the robot's pose. Additionally, the magnetometer array is used to compute attitude-invariant measurements associated with the magnetic field and its gradient. These measurements are used to detect loop closure points. Experimental results indicate that the proposed approach can estimate the pose of a ground robot in an indoor environment within meter accuracy.

Accurate, self-consistent bathymetric maps are needed to monitor changes in subsea environments and infrastructure. These maps are increasingly collected by underwater vehicles, and mapping requires an accurate vehicle navigation solution. Commercial off-the-shelf (COTS) navigation solutions for underwater vehicles often rely on external acoustic sensors for localization; however, survey-grade acoustic sensors are expensive to deploy and limit the range of the vehicle. Techniques from the field of simultaneous localization and mapping, particularly loop closures, can improve the quality of the navigation solution over dead reckoning, but are difficult to integrate into COTS navigation systems. This article presents a method to improve the self-consistency of bathymetric maps by smoothly integrating loop-closure measurements into the state estimate produced by a commercial subsea navigation system. Integration is done using a white-noise-on-acceleration motion prior, without access to raw sensor measurements or proprietary models. Improvements in map self-consistency are shown for both simulated and experimental datasets, including a 3-D scan of an underwater shipwreck in Wiarton, ON, Canada.

In multi-robot missions, relative position and attitude information between robots is valuable for a variety of tasks such as mapping, planning, and formation control. In this paper, the problem of estimating relative poses from a set of inter-robot range measurements is investigated. Specifically, it is shown that the estimation accuracy is highly dependent on the true relative poses themselves, which prompts the desire to find multi-robot formations that provide the best estimation performance. By direct maximization of Fischer information, it is shown in simulation and experiment that large improvements in estimation accuracy can be obtained by optimizing the formation geometry of a team of robots.

This paper considers the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose. The measurement model consisting of the position of one receiver and the relative position between the two receivers is left invariant, enabling the use of the invariant extended Kalman filter (IEKF) framework. The IEKF possesses various advantages over the standard multiplicative extended Kalman filter, such as state-estimate-independent Jacobians. Monte Carlo simulations demonstrate that the two-receiver IEKF approach yields improved estimates over a two-receiver multiplicative extended Kalman filter (MEKF) and a singlereceiver IEKF approach. An experiment further validates the proposed approach, confirming that the two-receiver IEKF has improved performance over the other filters considered.

A coordinate system is proposed that replaces the usual three-dimensional Cartesian x, y, z position coordinates, for use in robotic localization applications. Range, azimuth, and elevation measurement models become greatly simplified, and, unlike spherical coordinates, the proposed coordinates do not suffer from the same kinematic singularities and angle wraparound. When compared to Cartesian coordinates, the proposed coordinate system results in a significantly enhanced ability to represent the true distribution of robot positions, ultimately leading to large improvements in state estimation consistency.

Autonomous train navigation using only a low-cost MEMS IMU and a track map is considered in this paper. The approach is designed for urban rail or subway environments where GNSS measurements are unreliable or unavailable, and is intended as a baseline against which more complex sensor fusion approaches can be compared to ensure the consistency of the estimates. The estimator exploits the track motion constraint and information about position and velocity derived from centripetal acceleration and angular velocity measurements to improve the dead-reckoning solution and keep error and uncertainty bounded. In experimental validation over a 6 km run of a subway train during commuter service, the proposed approach had a maximum error of 6.0 m, validating the approach as an independent estimator.

Point cloud registration is a means of achieving loop closure correction within a simultaneous localization and mapping (SLAM) algorithm. Data association is a critical component in point cloud registration, and can be very challenging in feature-depleted environments such as seabed. This paper presents a point cloud registration pipeline for performing loop closure correction in feature-depleted subsea environments using data collected from an optical scanner. The pipeline uses Gaussian process regression to extract keypoint sets, and a weighted network alignment algorithm to propose point correspondences. A variant of the iterative closest point (ICP) registration algorithm is used to perform fine alignment, with point correspondences informed by the mappings determined following the network alignment step. The developed registration pipeline is deployed with success on a challenging section of field data containing topography that cannot be resolved using conventional imaging sonar.

This paper presents the modeling and control of a passively-coupled tilt-rotor vertical takeoff and landing aircraft. The aircraft consists of a quadrotor frame attached to a fixed-wing aircraft by an unactuated hinged mechanism. The platform is capable of smooth transitions from hover to forward flight without the use of tilting actuators. The transition from hover to forward flight is made possible by differential thrust between the fore and aft propellers of the quadrotor frame. In this paper, the coupled dynamics between the quadrotor frame and the aircraft frame are modeled as a constrained multi-body system. The equations of motion are established using a constrained Lagrangian approach and the model developed is used to build a realistic simulation environment for control design purpose. A cascaded control architecture based on P/PID controllers is proposed to achieve inner-loop attitude, height and forward velocity control. Simulated and experimental results are obtained with a close match for hover, transitions, forward flight, and banked turn maneuvers.

Interactive control of self-balancing, physically simulated humanoids is a long standing problem in the field of real-time character animation. While physical simulation guarantees realistic interactions in the virtual world, simulated characters can appear unnatural if they perform unusual movements in order to maintain balance. Therefore, obtaining a high level of responsiveness to user control, runtime performance, and diversity has often been overlooked in exchange for motion quality. Recent work in the field of deep reinforcement learning has shown that training physically simulated characters to follow motion capture clips can yield high quality tracking results. We propose a two-step approach for building responsive simulated character controllers from unstructured motion capture data. First, meaningful features from the data such as movement direction, heading direction, speed, and locomotion style, are interactively specified and drive a kinematic character controller implemented using motion matching. Second, reinforcement learning is used to train a simulated character controller that is general enough to track the entire distribution of motion that can be generated by the kinematic controller. Our design emphasizes responsiveness to user input, visual quality, and low runtime cost for application in video-games.

This paper considers modeling and control of a planar flexible two-link telescoping manipulator. The telescoping interface between the deploying and non-deploying portions of the links is discussed in detail. The equations of motion are derived using Lagrange's equation and constrained using a projection method that eliminates the need to compute Lagrange multipliers explicitly. Passivity-based control of the telescoping manipulator is also investigated. It is shown that several passive input-output maps exist. In particular, by a suitable redefinition of the inputs and outputs, a modified tip-based control is made possible. The model is validated through numerical simulation, and the tip-based control is compared with joint-based control. Simulation results show that tip-based control has better closed-loop performance compared with joint-based control.

This paper investigates the dynamic modeling and passivity-based control of a planar cable-actuated system. This system is modeled using a lumped-mass method that explicitly considers the change in cable stiffness and winch inertia that occurs when the cables are wound around their respective winches. In order to simplify the modeling process, each cable is modeled individually and then constrained to the other cables. Exploiting the fact that the payload is much more massive than the cables allows the definition of a modified output called the μ -tip rate. Coupling the μ-tip rate with a modified input realizes the definition of a passive input-output map. The two degrees of freedom of the system are controlled by four winches. This overactuation is simplified by employing a set of load-sharing parameters that effectively reduce four inputs to two. The performance and robustness of the controllers are evaluated in the simulation.