James Richard Forbes

Steven Dahdah, James Richard Forbes

This paper proposes a method to identify a Koopman model of a feedback-controlled system given a known controller. The Koopman operator allows a nonlinear system to be rewritten as an infinite-dimensional linear system by viewing it in terms of an infinite set of lifting functions. A finite-dimensional approximation of the Koopman operator can be identified from data by choosing a finite subset of lifting functions and solving a regression problem in the lifted space. Existing methods are designed to identify open-loop systems. However, it is impractical or impossible to run experiments on some systems, such as unstable systems, in an open-loop fashion. The proposed method leverages the linearity of the Koopman operator, along with knowledge of the controller and the structure of the closed-loop system, to simultaneously identify the closed-loop and plant systems. The advantages of the proposed closed-loop Koopman operator approximation method are demonstrated in simulation using a Duffing oscillator and experimentally using a rotary inverted pendulum system. An open-source software implementation of the proposed method is publicly available, along with the experimental dataset generated for this paper.

David Evan Zlotnik, James Richard Forbes

Nonlinear observer design for systems whose state space evolves on Lie groups is considered. The proposed method is similar to previously developed nonlinear observers in that it involves propagating the state estimate using a process model and corrects the propagated state estimate using an innovation term on the tangent space of the Lie group. In the proposed method, the innovation term is constructed by passing the gradient of an invariant cost function, resolved in a basis of the tangent space, through a linear time-invariant system. The introduction of the linear system completes the extension of linear complementary filters to nonlinear Lie group observers by allowing higher-order filtering. In practice, the proposed method allows for greater design freedom and, with the appropriate selection of the linear filter, the ability to filter bias and noise over specific bandwidths. A disturbance observer that accounts for constant and harmonic disturbances in group velocity measurements is also considered. Local asymptotic stability about the desired equilibrium point is demonstrated. A numerical example that demonstrates the desirable properties of the observer is presented in the context of pose estimation.

Kyungmin Jung, Thomas Hitchcox, James Richard Forbes

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.

Kyle Biron-Gricken, James Richard Forbes

Multi-robot localization that is accurate and consistent is imperative for downstream tasks such as planning and control. Centralized filtering approaches optimally fuse all available sensor measurements of the team. However, a centralized solution is rarely implementable due to hardware, communication, and computational constraints. Distributed approaches deploy a filter on each robot to estimate their own state and neighbours' states using inter-robot communication. This paper proposes a consistent, communication-efficient, and consensus-based distributed filtering framework that shares both preintegrated odometry and relevant shared states among communicating robots. The proposed method is validated in simulated and experimental scenarios, showing near centralized performance in accuracy, and especially in consistency, compared to the current state-of-the-art decentralized approach.

Kyungmin Jung, Angad Bajwa, Junha Yoo et al.

This paper presents a novel acoustic-visual-inertial odometry solution leveraging a continuous-time trajectory estimation framework for unmanned underwater vehicles. Underwater environments present unique challenges for visual localization and mapping, such as light attenuation, illumination variance, and the presence of particulate matter. This motivates the use of additional sensing modalities and a visual tracking pipeline that is robust to diverse subsea conditions. The proposed system is the first continuous-time trajectory estimation framework based on Gaussian processes to fuse asynchronous measurements from a Doppler velocity log, a stereo camera, and an inertial measurement unit. Additionally, a novel visual frontend is proposed, incorporating learning-based feature extraction and matching that is robust to the specific challenges that subsea environments present. The proposed framework enables seamless integration of additional sensor modalities in continuous-time and is adaptable to different environments without reconfiguration. The proposed system is extensively tested on real-world underwater inspection datasets, where it outperforms state-of-the-art visual-inertial and acoustic-visual-inertial SLAM algorithms in accuracy, robustness, and trajectory coverage. Notably, the proposed system outperforms the state-of-the-art despite only forming short-term visual data associations.

Timothy Everett Adams, James Richard Forbes

This paper proposes a linear input-output observer design methodology for a population of systems in which each observer uses knowledge of the linear time-invariant dynamics of the particular device. Observers are typically composed of a known model of the system and a correction mechanism to produce an estimate of the state. The proposed design procedure characterizes the variation within the population in the frequency domain and synthesizes a single robust correction filter. The correction filter is compatible with all system models that satisfy the variation characterization such that a given level of estimation performance is guaranteed. This is accomplished by posing a robust performance problem using the observer error dynamics and solving it using $DK$-iteration. The design procedure is experimentally demonstrated on a flexible joint robotic manipulator with varied joint stiffnesses. It is shown that the proposed method that uses a single correction filter achieves comparable estimation performance to a method that uses a correction gain tailored toward each joint stiffness configuration.

Jonathan Eid, Ashley Meagher, Dmitry Rimorov et al.

This paper presents an $\mathcal{H}_\infty$ model matching control-based approach to the problem of power hardware-in-the-loop (PHIL) interfacing. The objective is to interconnect a grid simulation and a physical device via an interface in a way that is stable and accurate. Conventional approaches include the ideal transformer method (ITM) and its impedance-based variants, which trade accuracy for stability, as well as some $\mathcal{H}_\infty$ control-based approaches, which do not make use of all the available information in their optimization for accuracy. Designing for transparency, as opposed to accuracy as existing approaches do, would achieve both accuracy and stability, while making use of all the dynamical information present in the idealized interconnection of the grid and device. The approach proposed in this paper employs model matching to formulate the PHIL problem as an $\mathcal{H}_\infty$ control problem using transparency as the explicit frequency-domain control objective. The approach is experimentally validated in a real-time resistive-load PHIL setup, and is found to achieve accuracy levels that are comparable or superior to those of an ITM-based interface.

Mitchell Cohen, Vassili Korotkine, James Richard Forbes

This paper presents an analysis of the observability and consistency properties of filtering-based visual-inertial navigation systems (VINS) that utilize anchored feature representations. The unobservable subspace of VINS with anchored landmark parameterizations is shown to be independent of the estimated landmark state, which leads to improved estimator consistency properties without any additional modifications. However, the unobservable subspace is still found to depend on the estimated navigation state, necessitating additional consistency-enforcing techniques. Two methods to improve the consistency of VINS with anchored feature representations are presented. Simulation results showcase that all estimators employing anchored feature paramterizations exhibit improved consistency properties compared to algorithms that estimate features resolved in a global reference frame, especially in scenarios where feature initialization may be poor. Real-world experiments on the TUM-VI dataset showcase that the use of anchored feature representations alone can yield comparable performance to consistency-improved estimators employing a global feature representation, demonstrating the benefit of using anchored feature parameterizations for VINS.

Mohammed Ayman Shalaby, Charles Champagne Cossette, Jerome Le Ny et al.

Autonomously retracing a manually-taught path is desirable for many applications, and Teach and Repeat (T&R) algorithms present an approach that is suitable for long-range autonomy. In this paper, ultra-wideband (UWB) ranging-based T&R is proposed for vehicles with limited resources. By fixing single UWB transceivers at far-apart unknown locations in an indoor environment, a robot with 3 UWB transceivers builds a locally consistent map during the teach pass by fusing the range measurements under a custom ranging protocol with an on-board IMU and height measurements. The robot then uses information from the teach pass to retrace the same trajectory autonomously. The proposed ranging protocol and T&R algorithm are validated in simulation, where it is shown that the robot can successfully retrace the taught trajectory with sub-metre tracking error.

Steven Dahdah, James Richard Forbes

Approximating the Koopman operator from data is numerically challenging when many lifting functions are considered. Even low-dimensional systems can yield unstable or ill-conditioned results in a high-dimensional lifted space. In this paper, Extended Dynamic Mode Decomposition (DMD) and DMD with control, two methods for approximating the Koopman operator, are reformulated as convex optimization problems with linear matrix inequality constraints. Asymptotic stability constraints and system norm regularizers are then incorporated as methods to improve the numerical conditioning of the Koopman operator. Specifically, the H-infinity norm is used to penalize the input-output gain of the Koopman system. Weighting functions are then applied to penalize the system gain at specific frequencies. These constraints and regularizers introduce bilinear matrix inequality constraints to the regression problem, which are handled by solving a sequence of convex optimization problems. Experimental results using data from an aircraft fatigue structural test rig and a soft robot arm highlight the advantages of the proposed regression methods.

Charles Champagne Cossette, Mohammed Shalaby, David Saussié et al.

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 wrap-around. 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.

Zi Cong Guo, Vassili Korotkine, James R. Forbes et al.

We present the Koopman State Estimator (KoopSE), a framework for model-free batch state estimation of control-affine systems that makes no linearization assumptions, requires no problem-specific feature selections, and has an inference computational cost that is independent of the number of training points. We lift the original nonlinear system into a higher-dimensional Reproducing Kernel Hilbert Space (RKHS), where the system becomes bilinear. The time-invariant model matrices can be learned by solving a least-squares problem on training trajectories. At test time, the system is algebraically manipulated into a linear time-varying system, where standard batch linear state estimation techniques can be used to efficiently compute state means and covariances. Random Fourier Features (RFF) are used to combine the computational efficiency of Koopman-based methods and the generality of kernel-embedding methods. KoopSE is validated experimentally on a localization task involving a mobile robot equipped with ultra-wideband receivers and wheel odometry. KoopSE estimates are more accurate and consistent than the standard model-based extended Rauch-Tung-Striebel (RTS) smoother, despite KoopSE having no prior knowledge of the system's motion or measurement models.

Mitchell R. Cohen, Khairi Abdulrahim, James Richard Forbes

This paper considers optimal control of a quadrotor unmanned aerial vehicles (UAV) using the discrete-time, finite-horizon, linear quadratic regulator (LQR). The state of a quadrotor UAV is represented as an element of the matrix Lie group of double direct isometries, $SE_2(3)$. The nonlinear system is linearized using a left-invariant error about a reference trajectory, leading to an optimal gain sequence that can be calculated offline. The reference trajectory is calculated using the differentially flat properties of the quadrotor. Monte-Carlo simulations demonstrate robustness of the proposed control scheme to parametric uncertainty, state-estimation error, and initial error. Additionally, when compared to an LQR controller that uses a conventional error definition, the proposed controller demonstrates better performance when initial errors are large.

Charles Champagne Cossette, Alex Walsh, James Richard Forbes

The complex-step derivative approximation is a numerical differentiation technique that can achieve analytical accuracy, to machine precision, with a single function evaluation. In this letter, the complex-step derivative approximation is extended to be compatible with elements of matrix Lie groups. As with the standard complex-step derivative, the method is still able to achieve analytical accuracy, up to machine precision, with a single function evaluation. Compared to a central-difference scheme, the proposed complex-step approach is shown to have superior accuracy. The approach is applied to two different pose estimation problems, and is able to recover the same results as an analytical method when available.

Natalia Pavlasek, Alex Walsh, James Richard Forbes

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 single-receiver IEKF approach. An experiment further validates the proposed approach, confirming that the two-receiver IEKF has improved performance over the other filters considered.

Timothy D. Barfoot, James R. Forbes, Gabriele M. T. D'Eleuterio

Robotics and computer vision problems commonly require handling rigid-body motions comprising translation and rotation - together referred to as pose. In some situations, a vectorial parameterization of pose can be useful, where elements of a vector space are surjectively mapped to a matrix Lie group. For example, these vectorial representations can be employed for optimization as well as uncertainty representation on groups. The most common mapping is the matrix exponential, which maps elements of a Lie algebra onto the associated Lie group. However, this choice is not unique. It has been previously shown how to characterize all such vectorial parameterizations for SO(3), the group of rotations. Some results are also known for the group of poses, where it is possible to build a family of vectorial mappings that includes the matrix exponential as well as the Cayley transformation. We extend what is known for these pose mappings to the 4 x 4 representation common in robotics, and also demonstrate three different examples of the proposed pose mappings: (i) pose interpolation, (ii) pose servoing control, and (iii) pose estimation in a pointcloud alignment problem. In the pointcloud alignment problem our results lead to a new algorithm based on the Cayley transformation, which we call CayPer.

Mohammed Shalaby, Charles Champagne Cossette, Jerome Le Ny et al.

It is often convenient to separate a state estimation task into smaller "local" tasks, where each local estimator estimates a subset of the overall system state. However, neglecting cross-covariance terms between state estimates can result in overconfident estimates, which can ultimately degrade the accuracy of the estimator. Common cascaded filtering techniques focus on the problem of modelling cross-covariances when the local estimators share a common state vector. This letter introduces a novel cascaded and decentralized filtering approach that approximates the cross-covariances when the local estimators consider distinct state vectors. The proposed estimator is validated in simulations and in experiments on a three-dimensional attitude and position estimation problem. The proposed approach is compared to a naive cascaded filtering approach that neglects cross-covariance terms, a sigma point-based Covariance Intersection filter, and a full-state filter. In both simulations and experiments, the proposed filter outperforms the naive and the Covariance Intersection filters, while performing comparatively to the full-state filter.

Steven Dahdah, James Richard Forbes

The regression problem associated with finding a matrix approximation of the Koopman operator from data is considered. The regression problem is formulated as a convex optimization problem subject to linear matrix inequality (LMI) constraints. Doing so allows for additional LMI constraints to be incorporated into the regression problem. In particular, asymptotic stability constraints, regularization using matrix norms, and even regularization using system norms can be easily incorporated into the regression problem.

Timothy D. Barfoot, James R. Forbes, David Yoon

We present a Gaussian Variational Inference (GVI) technique that can be applied to large-scale nonlinear batch state estimation problems. The main contribution is to show how to fit both the mean and (inverse) covariance of a Gaussian to the posterior efficiently, by exploiting factorization of the joint likelihood of the state and data, as is common in practical problems. This is different than Maximum A Posteriori (MAP) estimation, which seeks the point estimate for the state that maximizes the posterior (i.e., the mode). The proposed Exactly Sparse Gaussian Variational Inference (ESGVI) technique stores the inverse covariance matrix, which is typically very sparse (e.g., block-tridiagonal for classic state estimation). We show that the only blocks of the (dense) covariance matrix that are required during the calculations correspond to the non-zero blocks of the inverse covariance matrix, and further show how to calculate these blocks efficiently in the general GVI problem. ESGVI operates iteratively, and while we can use analytical derivatives at each iteration, Gaussian cubature can be substituted, thereby producing an efficient derivative-free batch formulation. ESGVI simplifies to precisely the Rauch-Tung-Striebel (RTS) smoother in the batch linear estimation case, but goes beyond the 'extended' RTS smoother in the nonlinear case since it finds the best-fit Gaussian (mean and covariance), not the MAP point estimate. We demonstrate the technique on controlled simulation problems and a batch nonlinear Simultaneous Localization and Mapping (SLAM) problem with an experimental dataset.

David Evan Zlotnik, James Richard Forbes

The simultaneous localization and mapping (SLAM) problem is considered in three dimensions. The proposed algorithm, differential geometric SLAM (DG-SLAM), employs methods from differential geometry to propagate the state and map estimates. Unlike EKF SLAM, the proposed filter is provably asymptotically stable under the assumption of no measurement noise or biases. The robustness of the DG-SLAM algorithm is assessed in simulation with measurement noise. The simulation demonstrates successful localization and mapping.