Certifiably optimal rotation and pose estimation based on the Cayley map

We present novel, convex relaxations for rotation and pose estimation problems that can a posteriori guarantee global optimality for practical measurement noise levels. Some such relaxations exist in the literature for specific problem setups that assume the matrix von Mises-Fisher distribution (a.k.a., matrix Langevin distribution or chordal distance) for isotropic rotational uncertainty. However, another common way to represent uncertainty for rotations and poses is to define anisotropic noise in the associated Lie algebra. Starting from a noise model based on the Cayley map, we define our estimation problems, convert them to Quadratically Constrained Quadratic Programs (QCQPs), then relax them to Semidefinite Programs (SDPs), which can be solved using standard interior-point optimization methods; global optimality follows from Lagrangian strong duality. We first show how to carry out basic rotation and pose averaging. We then turn to the more complex problem of trajectory estimation, which involves many pose variables with both individual and inter-pose measurements (or motion priors). Our contribution is to formulate SDP relaxations for all these problems based on the Cayley map (including the identification of redundant constraints) and to show them working in practical settings. We hope our results can add to the catalogue of useful estimation problems whose solutions can be a posteriori guaranteed to be globally optimal.

Adaptive Complementary Filter for Hybrid Inside-Out Outside-In HMD Tracking With Smooth Transitions

Head-mounted displays (HMDs) in room-scale virtual reality are usually tracked using inside-out visual SLAM algorithms. Alternatively, to track the motion of the HMD with respect to a fixed real-world reference frame, an outside-in instrumentation like a motion capture system can be adopted. However, outside-in tracking systems may temporarily lose tracking as they suffer by occlusion and blind spots. A possible solution is to adopt a hybrid approach where the inside-out tracker of the HMD is augmented with an outside-in sensing system. On the other hand, when the tracking signal of the outside-in system is recovered after a loss of tracking the transition from inside-out tracking to hybrid tracking may generate a discontinuity, i.e a sudden change of the virtual viewpoint, that can be uncomfortable for the user. Therefore, hybrid tracking solutions for HMDs require advanced sensor fusion algorithms to obtain a smooth transition. This work proposes a method for hybrid tracking of a HMD with smooth transitions based on an adaptive complementary filter. The proposed approach can be configured with several parameters that determine a trade-off between user experience and tracking error. A user study was carried out in a room-scale virtual reality environment, where users carried out two different tasks while multiple signal tracking losses of the outside-in sensor system occurred. The results show that the proposed approach improves user experience compared to a standard Extended Kalman Filter, and that tracking error is lower compared to a state-of-the-art complementary filter when configured for the same quality of user experience.

Visual-Inertial RGB-D SLAM with Encoder Integration of ORB Triangulation and Depth Measurement Uncertainties

In recent years, the accuracy of visual SLAM (Simultaneous Localization and Mapping) technology has seen significant improvements, making it a prominent area of research. However, within the current RGB-D SLAM systems, the estimation of 3D positions of feature points primarily relies on direct measurements from RGB-D depth cameras, which inherently contain measurement errors. Moreover, the potential of triangulation-based estimation for ORB (Oriented FAST and Rotated BRIEF) feature points remains underutilized. To address the singularity of measurement data, this paper proposes the integration of the ORB features, triangulation uncertainty estimation and depth measurements uncertainty estimation, for 3D positions of feature points. This integration is achieved using a CI (Covariance Intersection) filter, referred to as the CI-TEDM (Triangulation Estimates and Depth Measurements) method. Vision-based SLAM systems face significant challenges, particularly in environments, such as long straight corridors, weakly textured scenes, or during rapid motion, where tracking failures are common. To enhance the stability of visual SLAM, this paper introduces an improved CI-TEDM method by incorporating wheel encoder data. The mathematical model of the encoder is proposed, and detailed derivations of the encoder pre-integration model and error model are provided. Building on these improvements, we propose a novel tightly coupled visual-inertial RGB-D SLAM with encoder integration of ORB triangulation and depth measurement uncertainties. Validation on open-source datasets and real-world environments demonstrates that the proposed improvements significantly enhance the robustness of real-time state estimation and localization accuracy for intelligent vehicles in challenging environments.

Decentralized state estimation: An approach using pseudomeasurements and preintegration

This paper addresses the problem of decentralized, collaborative state estimation in robotic teams. In particular, this paper considers problems where individual robots estimate similar physical quantities, such as each other's position relative to themselves. The use of pseudomeasurements is introduced as a means of modeling such relationships between robots' state estimates and is shown to be a tractable way to approach the decentralized state estimation problem. Moreover, this formulation easily leads to a general-purpose observability test that simultaneously accounts for measurements that robots collect from their own sensors, as well as the communication structure within the team. Finally, input preintegration is proposed as a communication-efficient way of sharing odometry information between robots, and the entire theory is appropriate for both vector-space and Lie-group state definitions. To overcome the need for communicating preintegrated covariance information, a deep autoencoder is proposed that reconstructs the covariance information from the inputs, hence further reducing the communication requirements. The proposed framework is evaluated on three different simulated problems, and one experiment involving three quadcopters.

Unified Shape and External Load State Estimation for Continuum Robots

Continuum robots navigate narrow, winding passageways while safely and compliantly interacting with their environments. Sensing the robot's shape under these conditions is often done indirectly, using a few coarsely distributed (e.g. strain or position) sensors combined with the robot's mechanics-based model. More recently, given high-fidelity shape data, external interaction loads along the robot have been estimated by solving an inverse problem on the mechanics model of the robot. In this paper, we argue that since shape and force are fundamentally coupled, they should be estimated simultaneously in a statistically principled approach. We accomplish this by applying continuous-time batch estimation directly to the arclength domain. A general continuum robot model serves as a statistical prior which is fused with discrete, noisy measurements taken along the robot's backbone. The result is a continuous posterior containing both shape and load functions of arclength, as well as their uncertainties. We first test the approach with a Cosserat rod, i.e. the underlying modeling framework that is the basis for a variety of continuum robots. We verify our approach numerically using distributed loads with various sensor combinations. Next, we experimentally validate shape and external load errors for highly concentrated force distributions (point loads). Finally, we apply the approach to a tendon-actuated continuum robot demonstrating applicability to more complex actuated robots.

Vehicle State Estimation Combining Physics-Informed Neural Network and Unscented Kalman Filtering on Manifolds

This paper proposes a novel vehicle state estimation (VSE) method that combines a physics-informed neural network (PINN) and an unscented Kalman filter on manifolds (UKF-M). This VSE aimed to achieve inertial measurement unit (IMU) calibration and provide comprehensive information on the vehicle's dynamic state. The proposed method leverages a PINN to eliminate IMU drift by constraining the loss function with ordinary differential equations (ODEs). Then, the UKF-M is used to estimate the 3D attitude, velocity, and position of the vehicle more accurately using a six-degrees-of-freedom vehicle model. Experimental results demonstrate that the proposed PINN method can learn from multiple sensors and reduce the impact of sensor biases by constraining the ODEs without affecting the sensor characteristics. Compared to the UKF-M algorithm alone, our VSE can better estimate vehicle states. The proposed method has the potential to automatically reduce the impact of sensor drift during vehicle operation, making it more suitable for real-world applications.

Event-based feature tracking in a visual inertial odometry framework

Introduction: Event cameras report pixel-wise brightness changes at high temporal resolutions, allowing for high speed tracking of features in visual inertial odometry (VIO) estimation, but require a paradigm shift, as common practices from the past decades using conventional cameras, such as feature detection and tracking, do not translate directly. One method for feature detection and tracking is the Eventbased Kanade-Lucas-Tomasi tracker (EKLT), an hybrid approach that combines frames with events to provide a high speed tracking of features. Despite the high temporal resolution of the events, the local nature of the registration of features imposes conservative limits to the camera motion speed. Methods: Our proposed approach expands on EKLT by relying on the concurrent use of the event-based feature tracker with a visual inertial odometry system performing pose estimation, leveraging frames, events and Inertial Measurement Unit (IMU) information to improve tracking. The problem of temporally combining high-rate IMU information with asynchronous event cameras is solved by means of an asynchronous probabilistic filter, in particular an Unscented Kalman Filter (UKF). The proposed method of feature tracking based on EKLT takes into account the state estimation of the pose estimator running in parallel and provides this information to the feature tracker, resulting in a synergy that can improve not only the feature tracking, but also the pose estimation. This approach can be seen as a feedback, where the state estimation of the filter is fed back into the tracker, which then produces visual information for the filter, creating a "closed loop". Results: The method is tested on rotational motions only, and comparisons between a conventional (not event-based) approach and the proposed approach are made, using synthetic and real datasets. Results support that the use of events for the task improve performance. Discussion: To the best of our knowledge, this is the first work proposing the fusion of visual with inertial information using events cameras by means of an UKF, as well as the use of EKLT in the context of pose estimation. Furthermore, our closed loop approach proved to be an improvement over the base EKLT, resulting in better feature tracking and pose estimation. The inertial information, despite prone to drifting over time, allows keeping track of the features that would otherwise be lost. Then, feature tracking synergically helps estimating and minimizing the drift.

Continuum robot state estimation using Gaussian process regression on SE(3)

Continuum robots have the potential to enable new applications in medicine, inspection, and countless other areas due to their unique shape, compliance, and size. Excellent progress has been made in the mechanical design and dynamic modeling of continuum robots, to the point that there are some canonical designs, although new concepts continue to be explored. In this paper, we turn to the problem of state estimation for continuum robots that can been modeled with the common Cosserat rod model. Sensing for continuum robots might comprise external camera observations, embedded tracking coils, or strain gauges. We repurpose a Gaussian process (GP) regression approach to state estimation, initially developed for continuous-time trajectory estimation in SE(3). In our case, the continuous variable is not time but arclength and we show how to estimate the continuous shape (and strain) of the robot (along with associated uncertainties) given discrete, noisy measurements of both pose and strain along the length. We demonstrate our approach quantitatively through simulations as well as through experiments. Our evaluations show that accurate and continuous estimates of a continuum robot’s shape can be achieved, resulting in average end-effector errors between the estimated and ground truth shape as low as 3.5 mm and 0.016° in simulation or 3.3 mm and 0.035° for unloaded configurations and 6.2 mm and 0.041° for loaded ones during experiments, when using discrete pose measurements.

On the eigenstructure of rotations and poses: commonalities and peculiarities

Locating vehicles, targets and objects in three-dimensional space is key to many fields of science and engineering such as robotics, aerospace, computer vision and graphics. Rotations and poses (position plus orientation) of bodies can be expressed in a variety of ways. Rotation matrices constitute one of the classic matrix Lie groups, the special orthogonal group— SO ( 3 ) . Poses can likewise be represented by matrices. One such representation is embodied in a 4   ×   4 matrix establishing another famous matrix Lie group, the special Euclidean group— SE ( 3 ) . An alternative representation of pose uses 6   ×   6 matrices and is referred to as the group of pose adjoints— Ad ( SE ( 3 ) ) . The eigenstructures of these representations reveal much about them from Euler’s theorem for rotations to the Mozzi–Chasles theorem for the general displacement of a rigid body. While the eigenstructure of SO ( 3 ) has been extensively studied, those of SE ( 3 ) and Ad ( SE ( 3 ) ) have hardly received the same scrutiny yet their structure is much richer. Motivated by their importance in kinematics and dynamics, we provide here a complete characterization of rotations and poses in terms of the eigenstructure of their matrix Lie group representations. An eigendecomposition of pose matrices reveals that they can be cast into a form similar to that of rotations although the structure of the former can vary depending on the nature of the pose involved. In particular, the pose matrices of SE ( 3 ) and Ad ( SE ( 3 ) ) cannot generally be diagonalized as can rotation matrices but they of course do yield to a Jordan normal form, from which we can identify a principal-axis pose in much the same manner that we can a principal-axis rotation. We also address the minimal polynomials for poses and derive a novel expression for the Jacobian in Ad ( SE ( 3 ) ) . Finally, we argue that the true counterpart to SO ( 3 ) for poses is not SE ( 3 ) but Ad ( SE ( 3 ) ) .

Progress in symmetry preserving robot perception and control through geometry and learning

This article reports on recent progress in robot perception and control methods developed by taking the symmetry of the problem into account. Inspired by existing mathematical tools for studying the symmetry structures of geometric spaces, geometric sensor registration, state estimator, and control methods provide indispensable insights into the problem formulations and generalization of robotics algorithms to challenging unknown environments. When combined with computational methods for learning hard-to-measure quantities, symmetry-preserving methods unleash tremendous performance. The article supports this claim by showcasing experimental results of robot perception, state estimation, and control in real-world scenarios.

Bayesian iterative closest point for mobile robot localization

Accurate localization of a robot in a known environment is a fundamental capability for successfully performing path planning, manipulation, and grasping tasks. Particle filters, also known as Monte Carlo localization (MCL), are a commonly used method to determine the robot’s pose within its environment. For ground robots, noisy wheel odometry readings are typically used as a motion model to predict the vehicle’s location. Such a motion model requires tuning of various parameters based on terrain and robot type. However, such an ego-motion estimation is not always available for all platforms. Scan matching using the iterative closest point (ICP) algorithm is a popular alternative approach, providing ego-motion estimates for localization. Iterative closest point computes a point estimate of the transformation between two poses given point clouds captured at these locations. Being a point estimate method, ICP does not deal with the uncertainties in the scan alignment process, which may arise due to sensor noise, partial overlap, or the existence of multiple solutions. Another challenge for ICP is the high computational cost required to align two large point clouds, limiting its applicability to less dynamic problems. In this paper, we address these challenges by leveraging recent advances in probabilistic inference. Specifically, we first address the run-time issue and propose SGD-ICP, which employs stochastic gradient descent (SGD) to solve the optimization problem of ICP. Next, we leverage SGD-ICP to obtain a distribution over transformations and propose a Markov Chain Monte Carlo method using stochastic gradient Langevin dynamics (SGLD) updates. Our ICP variant, termed Bayesian-ICP, is a full Bayesian solution to the problem. To demonstrate the benefits of Bayesian-ICP for mobile robotic applications, we propose an adaptive motion model employing Bayesian-ICP to produce proposal distributions for Monte Carlo Localization. Experiments using both Kinect and 3D LiDAR data show that our proposed SGD-ICP method achieves the same solution quality as standard ICP while being significantly more efficient. We then demonstrate empirically that Bayesian-ICP can produce accurate distributions over pose transformations and is fast enough for online applications. Finally, using Bayesian-ICP as a motion model alleviates the need to tune the motion model parameters from odometry, resulting in better-calibrated localization uncertainty.

On-manifold probabilistic Iterative Closest Point: Application to underwater karst exploration

This paper proposes MpIC, an on-manifold derivation of the probabilistic Iterative Correspondence (pIC) algorithm, which is a stochastic version of the original Iterative Closest Point. It is developed in the context of autonomous underwater karst exploration based on acoustic sonars. First, a derivation of pIC based on the Lie group structure of [Formula: see text] is developed. The closed-form expression of the covariance modeling the estimated rigid transformation is also provided. In a second part, its application to 3D scan matching between acoustic sonar measurements is proposed. It is a prolongation of previous work on elevation angle estimation from wide-beam acoustic sonar. While the pIC approach proposed is intended to be a key component in a Simultaneous Localization and Mapping framework, this paper focuses on assessing its viability on a unitary basis. As ground truth data in karst aquifer are difficult to obtain, quantitative experiments are carried out on a simulated karst environment and show improvement compared to previous state-of-the-art approach. The algorithm is also evaluated on a real underwater cave dataset demonstrating its practical applicability.

Lie Group Modelling for an EKF-Based Monocular SLAM Algorithm

This paper addresses the problem of monocular Simultaneous Localization And Mapping on Lie groups using fiducial patterns. For that purpose, we propose a reformulation of the classical camera model as a model on matrix Lie groups. Thus, we define an original-state vector containing the camera pose and the set of transformations from the world frame to each pattern, which constitutes the map’s state. Each element of the map’s state, as well as the camera pose, are intrinsically constrained to evolve on the matrix Lie group SE(3). Filtering is then performed by an extended Kalman filter dedicated to matrix Lie groups to solve the visual SLAM process (LG-EKF-VSLAM). This algorithm has been evaluated in different scenarios based on simulated data as well as real data. The results show that the LG-EKF-VSLAM can improve the absolute position and orientation accuracy, compared to a classical EKF visual SLAM (EKF-VSLAM).

Vectorial parameterizations of pose

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 \times 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.

Global inverse optimal exponential path-tracking control of mobile robots driven by Lévy processes

This paper formulates and solves a new problem of global practical inverse optimal exponential path-tracking control of mobile robots driven by Lévy processes with unknown characteristics. The control design is based on a new inverse optimal control design for nonlinear systems driven by Lévy processes and ensures global practical exponential stability almost surely and in the pth moment for the path-tracking errors. Moreover, it minimizes cost function that penalizes tracking errors and control torques without having to solve a Hamilton–Jacobi–Bellman or Hamilton–Jaccobi–Isaacs equation.

Variational Bayesian Iteration-Based Invariant Kalman Filter for Attitude Estimation on Matrix Lie Groups

Motivated by the rapid progress of aerospace and robotics engineering, the navigation and control systems on matrix Lie groups have been actively studied in recent years. For rigid targets, the attitude estimation problem is a benchmark one with its states defined as rotation matrices on Lie groups. Based on the invariance properties of symmetry groups, the invariant Kalman filter (IKF) has been developed by researchers for matrix Lie group systems; however, the limitation of the IKF is that its estimation performance is prone to be degraded if the given knowledge of the noise statistics is not accurate. For the symmetry Lie group attitude estimation problem, this paper proposes a new variational Bayesian iteration-based adaptive invariant Kalman filter (VBIKF). In the proposed VBIKF, the a priori error covariance is not propagated by the conventional steps but directly calibrated in an iterative manner based on the posterior sequences. The main advantage of the VBIKF is that the statistics parameter of the system process noise is no longer required and so the IKF’s hard dependency on accurate process noise statistics can be reduced significantly. The mathematical foundation for the new VBIKF is presented and its superior performance in adaptability and simplicity is further demonstrated by numerical simulations.

An Improved Invariant Kalman Filter for Lie Groups Attitude Dynamics with Heavy-Tailed Process Noise

Attitude estimation is a basic task for most spacecraft missions in aerospace engineering and many Kalman type attitude estimators have been applied to the guidance and navigation of spacecraft systems. By building the attitude dynamics on matrix Lie groups, the invariant Kalman filter (IKF) was developed according to the invariance properties of symmetry groups. However, the Gaussian noise assumption of Kalman theory may be violated when a spacecraft maneuvers severely and the process noise might be heavy-tailed, which is prone to degrade IKF’s performance for attitude estimation. To address the attitude estimation problem with heavy-tailed process noise, this paper proposes a hierarchical Gaussian state-space model for invariant Kalman filtering: The probability density function of state prediction is defined based on student’s t distribution, while the conjugate prior distributions of the scale matrix and degrees of freedom (dofs) parameter are respectively formulated as the inverse Wishart and Gamma distribution. For the constructed hierarchical Gaussian attitude estimation state-space model, the Lie groups rotation matrix of spacecraft attitude is inferred together with the scale matrix and dof parameter using the variational Bayesian iteration. Numerical simulation results illustrate that the proposed approach can significantly improve the filtering robustness of invariant Kalman filter for Lie groups spacecraft attitude estimation problems with heavy-tailed process uncertainty.

Estimating Lower Limb Kinematics Using a Lie Group Constrained Extended Kalman Filter with a Reduced Wearable IMU Count and Distance Measurements

Tracking the kinematics of human movement usually requires the use of equipment that constrains the user within a room (e.g., optical motion capture systems), or requires the use of a conspicuous body-worn measurement system (e.g., inertial measurement units (IMUs) attached to each body segment). This paper presents a novel Lie group constrained extended Kalman filter to estimate lower limb kinematics using IMU and inter-IMU distance measurements in a reduced sensor count configuration. The algorithm iterates through the prediction (kinematic equations), measurement (pelvis height assumption/inter-IMU distance measurements, zero velocity update for feet/ankles, flat-floor assumption for feet/ankles, and covariance limiter), and constraint update (formulation of hinged knee joints and ball-and-socket hip joints). The knee and hip joint angle root-mean-square errors in the sagittal plane for straight walking were 7.6±2.6∘ and 6.6±2.7∘, respectively, while the correlation coefficients were 0.95±0.03 and 0.87±0.16, respectively. Furthermore, experiments using simulated inter-IMU distance measurements show that performance improved substantially for dynamic movements, even at large noise levels (σ=0.2 m). However, further validation is recommended with actual distance measurement sensors, such as ultra-wideband ranging sensors.

A Deep Reinforcement Learning Approach for Active SLAM

In this paper, we formulate the active SLAM paradigm in terms of model-free Deep Reinforcement Learning, embedding the traditional utility functions based on the Theory of Optimal Experimental Design in rewards, and therefore relaxing the intensive computations of classical approaches. We validate such formulation in a complex simulation environment, using a state-of-the-art deep Q-learning architecture with laser measurements as network inputs. Trained agents become capable not only to learn a policy to navigate and explore in the absence of an environment model but also to transfer their knowledge to previously unseen maps, which is a key requirement in robotic exploration.

Relative multiplicative extended Kalman filter for observable GPS-denied navigation

This work presents a multiplicative extended Kalman filter (MEKF) for estimating the relative state of a multirotor vehicle operating in a GPS-denied environment. The filter fuses data from an inertial measurement unit and altimeter with relative-pose updates from a keyframe-based visual odometry or laser scan-matching algorithm. Because the global position and heading states of the vehicle are unobservable in the absence of global measurements such as GPS, the filter in this article estimates the state with respect to a local frame that is colocated with the odometry keyframe. As a result, the odometry update provides nearly direct measurements of the relative vehicle pose, making those states observable. Recent publications have rigorously documented the theoretical advantages of such an observable parameterization, including improved consistency, accuracy, and system robustness, and have demonstrated the effectiveness of such an approach during prolonged multirotor flight tests. This article complements this prior work by providing a complete, self-contained, tutorial derivation of the relative MEKF, which has been thoroughly motivated but only briefly described to date. This article presents several improvements and extensions to the filter while clearly defining all quaternion conventions and properties used, including several new useful properties relating to error quaternions and their Euler-angle decomposition. Finally, this article derives the filter both for traditional dynamics defined with respect to an inertial frame, and for robocentric dynamics defined with respect to the vehicle’s body frame, and provides insights into the subtle differences that arise between the two formulations.

Lie Group Statistics and Lie Group Machine Learning Based on Souriau Lie Groups Thermodynamics & Koszul-Souriau-Fisher Metric: New Entropy Definition as Generalized Casimir Invariant Function in Coadjoint Representation

In 1969, Jean-Marie Souriau introduced a "Lie Groups Thermodynamics" in Statistical Mechanics in the framework of Geometric Mechanics. This Souriau's model considers the statistical mechanics of dynamic systems in their "space of evolution" associated to a homogeneous symplectic manifold by a Lagrange 2-form, and defines in case of non null cohomology (non equivariance of the coadjoint action on the moment map with appearance of an additional cocyle) a Gibbs density (of maximum entropy) that is covariant under the action of dynamic groups of physics (e.g., Galileo's group in classical physics). Souriau Lie Group Thermodynamics was also addressed 30 years after Souriau by R.F. Streater in the framework of Quantum Physics by Information Geometry for some Lie algebras, but only in the case of null cohomology. Souriau method could then be applied on Lie groups to define a covariant maximum entropy density by Kirillov representation theory. We will illustrate this method for homogeneous Siegel domains and more especially for Poincaré unit disk by considering SU(1,1) group coadjoint orbit and by using its Souriau's moment map. For this case, the coadjoint action on moment map is equivariant. For non-null cohomology, we give the case of Lie group SE(2). Finally, we will propose a new geometric definition of Entropy that could be built as a generalized Casimir invariant function in coadjoint representation, and Massieu characteristic function, dual of Entropy by Legendre transform, as a generalized Casimir invariant function in adjoint representation, where Souriau cocycle is a measure of the lack of equivariance of the moment mapping.

A Bi-Invariant Statistical Model Parametrized by Mean and Covariance on Rigid Motions

This paper aims to describe a statistical model of wrapped densities for bi-invariant statistics on the group of rigid motions of a Euclidean space. Probability distributions on the group are constructed from distributions on tangent spaces and pushed to the group by the exponential map. We provide an expression of the Jacobian determinant of the exponential map of SE(n) which enables the obtaining of explicit expressions of the densities on the group. Besides having explicit expressions, the strengths of this statistical model are that densities are parametrized by their moments and are easy to sample from. Unfortunately, we are not able to provide convergence rates for density estimation. We provide instead a numerical comparison between the moment-matching estimators on SE(2) and R3, which shows similar behaviors.

Estimation and Observability Analysis of Human Motion on Lie Groups

This article proposes a framework for human-pose estimation from the wearable sensors that rely on a Lie group representation to model the geometry of the human movement. Human body joints are modeled by matrix Lie groups, using special orthogonal groups SO(2) and SO(3) for joint pose and special Euclidean group SE(3) for base-link pose representation. To estimate the human joint pose, velocity, and acceleration, we develop the equations for employing the extended Kalman filter on Lie groups (LG-EKF) to explicitly account for the non-Euclidean geometry of the state space. We present the observability analysis of an arbitrarily long kinematic chain of SO(3) elements based on a differential geometric approach, representing a generalization of kinematic chains of a human body. The observability is investigated for the system using marker position measurements. The proposed algorithm is compared with two competing approaches: 1) the extended Kalman filter (EKF) and 2) unscented KF (UKF) based on the Euler angle parametrization, in both simulations and extensive real-world experiments. The results show that the proposed approach achieves significant improvements over the Euler angle-based filters. It provides more accurate pose estimates, is not sensitive to gimbal lock, and more consistently estimates the covariances.

Contact-aided invariant extended Kalman filtering for robot state estimation

Legged robots require knowledge of pose and velocity in order to maintain stability and execute walking paths. Current solutions either rely on vision data, which is susceptible to environmental and lighting conditions, or fusion of kinematic and contact data with measurements from an inertial measurement unit (IMU). In this work, we develop a contact-aided invariant extended Kalman filter (InEKF) using the theory of Lie groups and invariant observer design. This filter combines contact-inertial dynamics with forward kinematic corrections to estimate pose and velocity along with all current contact points. We show that the error dynamics follows a log-linear autonomous differential equation with several important consequences: (a) the observable state variables can be rendered convergent with a domain of attraction that is independent of the system’s trajectory; (b) unlike the standard EKF, neither the linearized error dynamics nor the linearized observation model depend on the current state estimate, which (c) leads to improved convergence properties and (d) a local observability matrix that is consistent with the underlying nonlinear system. Furthermore, we demonstrate how to include IMU biases, add/remove contacts, and formulate both world-centric and robo-centric versions. We compare the convergence of the proposed InEKF with the commonly used quaternion-based extended Kalman filter (EKF) through both simulations and experiments on a Cassie-series bipedal robot. Filter accuracy is analyzed using motion capture, while a LiDAR mapping experiment provides a practical use case. Overall, the developed contact-aided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in system.

Coupled recursive estimation for online interactive perception of articulated objects

We present online multi-modal perception systems for extracting kinematic and dynamic models of articulated objects from physical interactions with the environment. The systems rely on a RGB-D stream, contact wrenches, and proprioception. The proposed systems share an algorithmic foundation: they are based on an architecture of coupled recursive estimation processes. We present and advocate this architecture as a general, versatile, and robust solution for online interactive perception problems. We validate the architecture in extensive experiments to extract kinematic models interactively, varying the appearance, size, structure, and dynamic properties of objects for different tasks and under different environmental conditions. In addition, we experimentally show that the information acquired by the online perception systems enables robot manipulation of articulated objects. Furthermore, we discuss the relationship between the proposed architecture for robot perception and insights about biological perception systems.