Matrix Lie Group

An extended Kalman filter (EKF) for systems with configuration given by matrix Lie groups is presented. The error dynamics are given by the logarithm of the Lie group and are based on the kinematic differential equation of the logarithm, which is given in terms of the Jacobian of the Lie group. The probability distribution is also described in terms of the logarithm as a concentrated Gaussian distribution that is a tightly focused distribution around the identity of the Lie group. The filter is applied to estimation on SO(3) a case where a stereo camera setup tracks a crane wire with a payload. The wire, which is under tension and forms a line is monitored by two 2D-cameras, and a line detector is used to obtain a description of how the wire is projected onto each image plane. A model of a spherical pendulum is applied and the estimator is validated by applying it on simulated data, as well as experimental data.

The Invariant UKF, named IUKF, is a recently introduced algorithm dedicated to nonlinear systems possessing symmetries as illustrated by the quaternion-based kinematics modeling of a mini-UAV (Unmanned Aircraft Vehicle) considered in this paper. Within an invariant framework, this algorithm suggests a systematic approach to determine all the symmetry-preserving terms, without requiring any compatibility condition such as proposed in the π-IUKF, by introducing both notion of invariant output errors and UKF algorithm formulation. We propose in this paper to evaluate the applicability of our proposed IUKF observer to the case of attitude estimation for small UAVs using low-cost sensors. The IUKF algorithm is successfully validated in experiments and demonstrates that nonlinear state estimation converges on a much bigger set of trajectories than for more traditional approaches.

This paper proposes a docking maneuver for an underactuated autonomous underwater vehicle (AUV) to dock into a funnel shaped docking station. The novelty of the proposed approach is enabling an underactuated AUV that is unable to control its sway motion, to dock with no crab angle, in the presence of cross-currents. Docking without a crab angle can be beneficial in cases where the geometry of the docking station entrance does not allow entering with crab angles. In order to successfully dock under such restrictions, a path planner and two guidance laws are proposed. By properly switching between the two guidance laws, it is possible for the vehicle to slide cross-current into the docking station.

In this article we introduce a formula for the probability which an autocommutator element of a finite group G, equals to a fixed element 1 of G and derive some properties of this formula. Moreover, we obtain a lower bound and an upper bound for this probability in the special cases. This generalizes some results of Das et al. in 2010 and Moghaddam et al. in 2011.

Density estimation is an important technique for characterizing distributions given observations. Much existing research on density estimation has focused on cases wherein the data lies in a Euclidean space. However, some kinds of data are not well-modeled by supposing that their underlying geometry is Euclidean. Instead, it can be useful to model such data as lying on a manifold with some known structure. For instance, some kinds of data may be known to lie on the surface of a sphere. We study the problem of estimating densities on manifolds. We propose a method, inspired by the literature on “dequantization,” which we interpret through the lens of a coordinate transformation of an ambient Euclidean space and a smooth manifold of interest. Using methods from normalizing flows, we apply this method to the dequantization of smooth manifold structures in order to model densities on the sphere, tori, and the orthogonal group.

During sea missions, underwater vehicles are exposed to changes in the parameters of the system and subject to persistent external disturbances due to the ocean current influence. These issues make the design of a robust controller a quite challenging task. This paper focuses on the design of a adaptive high order sliding mode control for trajectory tracking on an underwater vehicle. The main feature of the developed control law is that it preserves the advantages of robust control, it does not need the knowledge of the upper bound of the disturbance and easy tuning in real applications. Using Lyapunov concept, asymptotic stability of the closed-loop tracking system is ensured. The effectiveness and robustness of the proposed controller for trajectory tracking in depth and yaw dynamics are demonstrated through real-time experiments.

In this paper we generalize the Continuous-Discrete Extended Kalman Filter (CD-EKF) to the case where the state and the observations evolve on connected unimodular matrix Lie groups. We propose a new assumed density filter called Continuous-Discrete Extended Kalman Filter on Lie Groups (CD-LG-EKF). It is built upon a geometrically meaningful modeling of the concentrated Gaussian distribution on Lie Groups. Such a distribution is parametrized by a mean and a covariance matrix defined on the Lie group and in its associated Lie algebra respectively. Our formalism yields tractable equations for both non-linear continuous time propagation and discrete update of the distribution parameters under the assumption that the posterior distribution of the state is a concentrated Gaussian. As a side effect, we contribute to the derivation of the first and second order differential of the matrix Lie group logarithm using left connection. We also show that the CD-LG-EKF reduces to the usual CD-EKF if the state and the observations evolve on Euclidean spaces. Our approach leads to a systematic methodology for the design of filters, which is illustrated by the application to a camera pose filtering problem with observations on Lie group. In this application, the CD-LG-EKF signif

This paper presents a new robust adaptive Gaussian mixture sigma-point particle filter by adopting the concept of robust adaptive estimation to the Gaussian mixture sigma-point particle filter. This method approximates state mean and covariance via Sigma-point transformation combined with new available measurement information. It enables the estimations of state mean and covariance to be adjusted via the equivalent weight function and adaptive factor, thus restraining the disturbances of singular measurements and kinematic model noise. It can also obtain efficient predict prior and posterior density functions via Gaussian mixture approximation to improve the filtering accuracy for nonlinear and non-Gaussian systems. Simulation results and comparison analysis demonstrate the proposed method can effectively restrain the disturbances of abnormal measurements and kinematic model noise on state estimate, leading to improved estimation accuracy.

This paper addresses the drone tracking problem, using a model based on the Frenet-Serret frame. A kinematic model in 2D, representing intrinsic coordinates of the drone is used. The tracking problem is tackled using two recent filtering methods. On the one hand, the Invariant Extended Kalman Filter (IEKF), introduced in [1] is tested, and on the other hand, the second step of the filtering algorithm, i.e. the update step of the IEKF is replaced by the update step of the Unscented Kalman Filter (UKF), introduced in [2]. These two filters are compared to the well known Extended Kalman Filter. The estimation precision of all three algorithms are computed on a real drone tracking problem.

In this paper, we propose a new generic filter called Iterated Extended Kalman Filter on Lie Groups. It allows to perform parameter estimation when the state and the measurements evolve on matrix Lie groups. The contribution of this work is threefold: 1) the proposed filter generalizes the Euclidean Iterated Extended Kalman Filter to the case where both the state and the measurements evolve on Lie groups, 2) this novel filter bridges the gap between the minimization of intrinsic non linear least squares criteria and filtering on Lie groups, 3) in order to detect and remove outlier measurements, a statistical test on Lie groups is proposed. In order to demonstrate the efficiency of the proposed generic filter, it is applied to the specific problem of relative motion averaging, both on synthetic and real data, for Lie groups SE(3) (rigid body motions), SL(3) (homographies) and Sim(3) (3D similarities). Typical applications of these problems are camera network calibration, image mosaicing and partial 3D reconstruction The research leading to these results has received funding from the European Community's Seventh Framework Programme (FP7/2007-2013) under grant agreement 288199-Dem@Care

This paper investigates the stabilization of underactuated vehicles moving in a three-dimensional vector space. The vehicle’s model is established on the matrix Lie group SE(3), which describes the configuration of rigid bodies globally and uniquely. We focus on the kinematic model of the underactuated vehicle, which features an underactuation form that has no sway and heave velocity. To compensate for the lack of these two velocities, we construct additional rotation matrices to generate a motion of rotation coupled with translation. Then, the state feedback is designed with the help of the logarithmic map, and we prove that the proposed control law can exponentially stabilize the underactuated vehicle to the identity group element with an almost global domain of attraction. Later, the presented control strategy is extended to set-point stabilization in the sense that the underactuated vehicle can be stabilized to an arbitrary desired configuration specified in advance. Finally, simulation examples are provided to verify the effectiveness of the stabilization controller.

This paper presents a new robust adaptive Gaussian mixture sigma-point particle filter by adopting the concept of robust adaptive estimation to the Gaussian mixture sigma-point particle filter. This method approximates state mean and covariance via Sigma-point transformation combined with new available measurement information. It enables the estimations of state mean and covariance to be adjusted via the equivalent weight function and adaptive factor, thus restraining the disturbances of singular measurements and kinematic model noise. It can also obtain efficient predict prior and posterior density functions via Gaussian mixture approximation to improve the filtering accuracy for nonlinear and non-Gaussian systems. Simulation results and comparison analysis demonstrate the proposed method can effectively restrain the disturbances of abnormal measurements and kinematic model noise on state estimate, leading to improved estimation accuracy.

A novel approach based on Unscented Kalman Filter (UKF) is proposed for nonlinear state estimation. The Invariant UKF, named π-IUKF, is a recently introduced algorithm dedicated to nonlinear systems possessing symmetries as illustrated by the quaternion-based mini Remotely Piloted Aircraft System (RPAS) kinematics modeling considered in this paper. Within an invariant framework, this algorithm suggests a systematic approach to determine all the symmetrypreserving terms which correct accordingly the nonlinear statespace representation used for prediction, without requiring any linearization. Thus, based on both invariant filters, for which Lie groups have been identified and UKF theoretical principles, the developed π-IUKF has been previously and successfully applied to the mini-RPAS attitude estimation problem, highlighting remarkable invariant properties. We propose in this paper to extend the theoretical background and the applicability of our proposed π-IUKF observer to the case of a mini-RPAS equipped with an aided Inertial Navigation System (INS) which leads to augment the nonlinear state space representation with both velocity and position differential equations. All the measurements are provided on board by a set of lowcost and low-performance sensors (accelerometers, gyrometers, magnetometers, barometer and even Global Positioning System (GPS)). Our designed π-IUKF estimation algorithm is described in this paper and its performances are evaluated by exploiting successfully real flight test data. Indeed, the whole approach has been implemented onboard using a data logger based on the well-known Paparazzi system. The results show promising perspectives and demonstrate that nonlinear state estimation converges on a much bigger set of trajectories than for more traditional approaches.

The Invariant UKF, named IUKF, is a recently introduced algorithm dedicated to nonlinear systems possessing symmetries as illustrated by the quaternion-based kinematics modeling of a mini-UAV (Unmanned Aircraft Vehicle) considered in this paper. Within an invariant framework, this algorithm suggests a systematic approach to determine all the symmetry-preserving terms, without requiring any compatibility condition such as proposed in the π-IUKF, by introducing both notion of invariant output errors and UKF algorithm formulation. We propose in this paper to evaluate the applicability of our proposed IUKF observer to the case of attitude estimation for small UAVs using low-cost sensors. The IUKF algorithm is successfully validated in experiments and demonstrates that nonlinear state estimation converges on a much bigger set of trajectories than for more traditional approaches.

Orbit determination in application to the estimation of imp act probability has the goal of determining the evolution of the state probability d ensity function (pdf) and determining a measure of the probability of collision. N onlinear gravitational interaction and non-conservative forces can make the pdf fa r from Gaussian. This work implements three nonlinear sequential estimators: th e Extended Kalman Filter (EKF), the Unscented Kalman Filter (UKF) and the Particl e Filter (PF) to estimate the impact probability. Both the EKF and the UKF make t h Gaussian assumption and this work investigates the effect of this appro ximation on the impact probability calculation, while the PF can work for non-Gaus sian systems.

Orbit determination in application to the estimation of imp act probability has the goal of determining the evolution of the state probability d ensity function (pdf) and determining a measure of the probability of collision. N onlinear gravitational interaction and non-conservative forces can make the pdf fa r from Gaussian. This work implements three nonlinear sequential estimators: th e Extended Kalman Filter (EKF), the Unscented Kalman Filter (UKF) and the Particl e Filter (PF) to estimate the impact probability. Both the EKF and the UKF make t h Gaussian assumption and this work investigates the effect of this appro ximation on the impact probability calculation, while the PF can work for non-Gaus sian systems.

Hybrid systems are subject to multiple dynamic models, or so-called modes. To estimate the state, the sequence of modes has to be estimated, which results in an exponential growth of possible sequences. The most prominent solution to handle this is the interacting multiple model filter, which can be extended to smoothing. In this paper, we derive a novel generalization of the interacting multiple filter and smoother to manifold state spaces, e.g., quaternions, based on the boxplus-method. As part thereof, we propose a linear approximation to the mixing of Gaussians and a Rauch–Tung–Striebel smoother for single models on boxplus-manifolds. The derivation of the smoother equations is based on a generalized definition of Gaussians on boxplus-manifolds. The three, novel algorithms are evaluated in a simulation and perform comparable to specialized solutions for quaternions. So far, the benefit of the more principled approach is the generality towards manifold state spaces. The evaluatio...

Any left-invariant optimal control problem (with quadratic cost) can be lifted, via the celebrated Maximum Principle, to a Hamiltonian system on the dual of the Lie algebra of the underlying state space G. The (minus) Lie-Poisson structure on the dual space g * is used to describe the (normal) extremal curves. As an illustration, a typical left-invariant optimal control problem on the rotation group SO (3) is investigated. The reduced Hamilton equations associated with an extremal curve are derived and then explicitly integrated by Jacobi elliptic functions.

Unscented Kalman Filters (UKFs) have become popular in the research community. Most UKFs work only with Euclidean systems, but in many scenarios it is advantageous to consider systems with state-variables taking values on Riemannian manifolds. However, we can still find some gaps in the literature's theory of UKFs for Riemannian systems: for instance, the literature has not yet i) developed Riemannian extensions of some fundamental concepts of the UKF theory (e.g., extensions of σ-representation, Unscented Transformation, Additive UKF, Augmented UKF, additive-noise system), ii) proofs of some steps in their UKFs for Riemannian systems (e.g., proof of sigma points parameterization by vectors, state correction equations, noise statistics inclusion), and iii) relations between their UKFs for Riemannian systems. In this work, we attempt to develop a theory capable of filling these gaps. Among other results, we propose Riemannian extensions of the main concepts in the UKF theory (including closed forms), justify all steps of the proposed UKFs, and provide a framework able to relate UKFs for particular manifolds among themselves and with UKFs for Euclidean spaces. Compared with UKFs for Riemannian manifolds of the literature, the proposed filters are more consistent, formallyprincipled, and general. An example of satellite attitude tracking illustrates the proposed theory.

A typical left-invariant optimal control problem on the rotation group SO (3) is investigated. The reduced Hamilton equations associated with an extremal curve are derived in a simple and elegant manner. These equations are then explicitly integrated by Jacobi elliptic functions.

A classification of full-rank affine subspaces of (real) three-dimensional Lie algebras is presented. In the context of invariant control affine systems, this is exactly a classification of all full-rank systems evolving on three-dimensional Lie groups.

This paper considers control affine leftinvariant systems evolving on matrix Lie groups. Any left-invariant optimal control problem (with quadratic cost) can be lifted, via the celebrated Maximum Principle, to a Hamiltonian system on the dual of the Lie algebra of the underlying state space G. The (minus) Lie-Poisson structure on the dual space g * is used to describe the (normal) extremal curves. The fully actuated case on the Euclidean group SE (2) is considered and the reduced Hamilton equations associated with an extremal curve are derived in a simple and elegant manner. Finally, the nature of the equilibrium states is fully investigated by the energy-Casimir method.

This paper considers left-invariant control affine systems evolving on matrix Lie groups. Any left-invariant optimal control problem (with quadratic cost) can be lifted, via the celebrated Maximum Principle, to a Hamiltonian system on the dual of the Lie algebra of the underlying state space G. The (minus) Lie-Poisson structure on the dual space g * is used to describe the (normal) extremal curves. Complete integrability of (reduced) Hamiltonian dynamical systems is discussed briefly. Some observations concerning Casimir functions and the case of semisimple (matrix) Lie groups are made. As an application, a drift-free left-invariant optimal control problem on the rotation group SO (3) is investigated. The reduced Hamilton equations associated with an extremal curve are derived in a simple and elegant manner. Finally, these equations are explicitly integrated by Jacobi elliptic functions.

This paper considers left-invariant control affine systems evolving on matrix Lie groups. Any left-invariant optimal control problem (with quadratic cost) can be lifted, via the celebrated Maximum Principle, to a Hamiltonian system on the dual of the Lie algebra of the underlying state space G. The (minus) Lie-Poisson structure on the dual space g * is used to describe the (normal) extremal curves. Complete integrability of (reduced) Hamiltonian dynamical systems is discussed briefly. Some observations concerning Casimir functions and the case of semisimple (matrix) Lie groups are made. As an application, a drift-free left-invariant optimal control problem on the rotation group SO (3) is investigated. The reduced Hamilton equations associated with an extremal curve are derived in a simple and elegant manner. Finally, these equations are explicitly integrated by Jacobi elliptic functions.

This paper considers control affine leftinvariant systems evolving on matrix Lie groups. Any left-invariant optimal control problem (with quadratic cost) can be lifted, via the celebrated Maximum Principle, to a Hamiltonian system on the dual of the Lie algebra of the underlying state space G. The (minus) Lie-Poisson structure on the dual space g * is used to describe the (normal) extremal curves. The fully actuated case on the Euclidean group SE (2) is considered and the reduced Hamilton equations associated with an extremal curve are derived in a simple and elegant manner. Finally, the nature of the equilibrium states is fully investigated by the energy-Casimir method.

Q uaternions of rotation have become popular for the purpose of attitude parametrization because they yield a minimal parameter global attitude representation and describe the kinematics of a rigid body through a linear ordinary differential equation. Over the last forty years optimal quaternion estimators using vector observations

Real-life processes are mostly characterized by several operating regimes or modes. Each operating mode corresponds to particular operating conditions and may require the use of a specific control strategy. Therefore, the task of identifying or determining continuously the current operating mode of the process is of a great value as it allows to implement the control strategy that is in accordance with the current operating mode of the process. The use of an unscented Kalman filter is considered here for the purpose of ...

Large-scale distributed systems such as sensor networks, often need to achieve filtering and consensus on an estimated parameter from high-dimensional measurements. Running a Kalman filter on every node in such a network is computationally intensive; in particular the matrix inversion in the Kalman gain update step is expensive. In this paper, we extend previous results in distributed Kalman filtering and large-scale machine learning to propose a gradient descent step for updating an estimate of the error covariance matrix; this is then embedded and analyzed in the context of distributed Kalman filtering. We provide properties of the resulting filters, in addition to a number of applications throughout the paper.

This paper presents a novel Kalman filter (KF) for estimating the attitude-quaternion as well as gyro random drifts from vector measurements. Employing a special manipulation on the measurement equation results in a linear pseudo-measurement equation whose error is statedependent. Because the quaternion kinematics equation is linear, the combination of the two yields a linear KF that eliminates the usual linearization procedure and is less sensitive to initial estimation errors. General accurate expressions for the covariance matrices of the system statedependent noises are developed. In addition, an analysis shows how to compute these covariance matrices efficiently. An adaptive version of the filter is also developed to handle modeling errors of the dynamic system noise statistics. Monte-Carlo simulations are carried out that demonstrate the efficiency of both versions of the filter. In the particular case of high initial estimation errors, a typical extended Kalman filter (EKF) fails to converge whereas the proposed filter succeeds. This paper presents a novel Kalman filter (KF) for estimating the attitude-quaternion as well as gyro random drifts from vector measurements. Employing a special manipulation on the measurement equation results in a linear pseudo-measurement equation whose error is state-dependent. Because the quaternion kinematics equation is linear, the combination of the two yields a linear KF that eliminates the usual linearization procedure and is less sensitive to initial estimation errors. General accurate expressions for the covariance matrices of the system state-dependent noises are developed. In addition, an analysis shows how to compute these covariance matrices efficiently. An adaptive version of the filter is also developed to handle modeling errors of the dynamic system noise statistics. Monte-Carlo simulations are carried out that demonstrate the efficiency of both versions of the filter. In the particular case of high initial estimation errors, a typical extended Kalman filter (EKF) fails to converge whereas the proposed filter succeeds.

We explore the potential of variance matrices to represent not just statistical error on object pose estimates but also partially constrained degrees of freedom. Using an iterated extended Kalman filter as an estimation tool, we generate, combine and predict partially constrained pose estimates from 3D range data. We find that partial constraints on the translation component of pose which occur frequently in practice are handled well by the method. One key advantage of the method is that it allows simultaneous representation of both lack of knowledge, weak constraints, a priori position constraints and statistical error in a framework that allows incremental reasoning.

In this paper, we propose a new generic filter called Iterated Extended Kalman Filter on Lie Groups. It allows to perform parameter estimation when the state and the measurements evolve on matrix Lie groups. The contribution of this work is threefold: 1) the proposed filter generalizes the Euclidean Iterated Extended Kalman Filter to the case where both the state and the measurements evolve on Lie groups, 2) this novel filter bridges the gap between the minimization of intrinsic non linear least squares criteria and filtering on Lie groups, 3) in order to detect and remove outlier measurements, a statistical test on Lie groups is proposed.

A classification of full-rank affine subspaces of (real) three-dimensional Lie algebras is presented. In the context of invariant control affine systems, this is exactly a classification of all full-rank systems evolving on three-dimensional Lie groups.

We classify the full-rank left-invariant control affine systems evolving on (real) semisimple three-dimensional Lie groups. This is accomplished by reducing the problem to that of classifying the affine subspaces of the Lie algebras so (2, 1) and so (3).

We consider left-invariant control affine systems evolving on Lie groups. In this context, feedback equivalence specializes to detached feedback equivalence. We characterize (local) detached feedback equivalence in a simple algebraic manner. We then classify all (full-rank) systems evolving on three-dimensional Lie groups. A representative is identified for each equivalence class. Systems on the Heisenberg group, the Euclidean group, and the orthogonal group are treated in full, as typical examples. In these three cases, simple algebraic characterizations of the equivalence classes are also exhibited. A few remarks conclude the paper.

We consider left-invariant control affine systems evolving on three-dimensional matrix Lie groups. Equivalence and controllability are addressed. The full-rank systems are classified under detached feedback equivalence; a representative is identified for each equivalence class. A characterization of controllability on each group is then determined.

We seek to classify the full-rank left-invariant control affine systems evolving on solvable three-dimensional Lie groups. In this paper we consider only the cases corresponding to the solvable Lie algebras of types II, IV , and V in the Bianchi-Behr classification.

We compare the performance of the extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter (PF) for the angle-only filtering problem in 3D using bearing and elevation measurements from a single maneuvering sensor. These nonlinear filtering algorithms use discrete-time dynamic and measurement models. Two types of coordinate systems are considered, Cartesian coordinates and modified spherical coordinates (MSC) for the relative state vector. The paper presents new algorithms using the UKF and PF with the MSC. We also present an improved filter initialization algorithm. Numerical results from Monte Carlo simulations show that the EKF-MSC and UKF-MSC have the best state estimation accuracy among all nonlinear filters considered and have comparable accuracy with modest computational cost.

Continuous-discrete filtering (CDF) arises in many real-world problems such as ballistic projectile tracking, ballistic missile tracking, bearing-only tracking in 2D, angle-only tracking in 3D, and satellite orbit determination. We develop CDF algorithms using the extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter (PF) with applications to the angle-only tracking in 3D. The modified spherical coordinates are used to represent the target state. Monte Carlo simulations are performed to compare the performance and computational complexity of the proposed filtering algorithms. Our results show that the CDF algorithms based on the EKF and UKF have the best state estimation accuracy and nearly the same computational cost.

We compare the performance of the extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter (PF) for the angle-only filtering problem in 3D using bearing and elevation measurements from a single maneuvering sensor. These nonlinear filtering algorithms use discrete-time dynamic and measurement models. Two types of coordinate systems are considered, Cartesian coordinates and modified spherical coordinates (MSC) for the relative state vector. The paper presents new algorithms using the UKF and PF with the MSC. We also present an improved filter initialization algorithm. Numerical results from Monte Carlo simulations show that the EKF-MSC and UKF-MSC have the best state estimation accuracy among all nonlinear filters considered and have comparable accuracy with modest computational cost.

The paper investigates the problem of nonlinear filtering applied to spacecraft navigation. Differential algebraic (DA) techniques are proposed as a valuable tool to implement the higher-order numerical and analytic extended Kalman filters. Working in the DA framework allows us to consistently reduce the required computational effort without losing accuracy. The performance of the proposed filters is assessed on different orbit determination problems with realistic orbit uncertainties. The case of nonlinear measurements is also considered. Numerical simulations show the good performance of the filter in case of both complex dynamics and highly nonlinear measurement problems.

In this paper, we are interested in estimating global motions (homo-
graphies, 3D rotations, 3D Euclidean motions, etc.) as well as the
covariance of the estimation errors from relative measurements by
exploiting the Lie group structure of the motions. We propose a gen-
erative model based on the formulation of a concentrated Gaussian
distribution on matrix Lie groups. In this context, the global mo-
tion estimation problem reduces to the minimization of the sum of
squared intrinsic invariant (w.r.t the right action of the Lie group on
itself) errors. We derive an iterated extended Kalman filter on matrix
Lie groups from the Gauss-Newton formalism on matrix Lie groups,
which exhibits a low computational complexity. Experimental re-
sults on simulated data, in the context of a consistent pose registra-
tion problem, show that the proposed algorithm significantly outper-
forms the state of the art approaches.

In this paper we generalize the Continuous-Discrete Extended Kalman Filter (CD-EKF) to the case where the state and the observations evolve on connected unimodular matrix Lie groups. We propose a new assumed density filter called Continuous-Discrete Extended Kalman Filter on Lie Groups (CD-LG-EKF). It is built upon a geometrically meaningful modeling of the concentrated Gaussian distribution on Lie Groups. Such a distribution is parametrized by a mean and a covariance matrix defined on the Lie group and in its associated Lie algebra respectively. Our formalism yields tractable equations for both non-linear continuous time propagation and discrete update of the distribution parameters under the assumption that the posterior distribution of the state is a concentrated Gaussian. As a side effect, we contribute to the derivation of the first and second order differential of the matrix Lie group logarithm using left connection. We also show that the CD-LG-EKF reduces to the usual CD-EKF if the state and the observations evolve on Euclidean spaces. Our approach leads to a systematic methodology for the design of filters, which is illustrated by the application to a camera pose filtering problem with observations on Lie group. In this application, the CD-LG-EKF signif-icantly outperforms two constrained non-linear filters (one based on a linearization technique and the other on the unscented transform) applied on the embedding space of the Lie group.

This paper presents a new finite-dimensional Bayesian filter. The filter calculates the exact analytical expression for the posterior probability density function (pdf) of static systems with kind of nonlinear measurement equation subject to Gaussian measurement uncertainty. The paper also extends this filter to a limited class of dynamic systems. The filter is applied to the estimation of the inaccurately known position and orientation of two mating parts during autonomous robotic assembly. The sufficient statistics of the posterior pdf are obtained by Kalman Filter formulas, making online estimation possible.

In this paper, we generalize the Discrete Extended Kalman Filter (D-EKF) to the case where the state and the observations evolve on Lie group manifolds. We propose a new filter called Discrete Extended Kalman Filter on Lie Groups (D-LG-EKF). It assumes that the posterior distribution of the state is a concentrated Gaussian distribution on Lie group. Our formalism yields closed-form equations for both nonlinear discrete propagation and update of the distribution parameters based on the likelihood. We also show that the D-LG-EKF reduces to the traditional D-EKF if the state evolves on an Euclidean space. Our approach leads to a systematic methodology for the design of filters, which is illustrated by the application to a camera pose estimation problem. Results show that the D-LG-EKF outperforms both a constrained D-EKF and a D-EKF applied on the Lie algebra of the Lie group.

Common estimation algorithms, such as least squares estimation or the Kalman filter, operate on a state in a state space S that is represented as a real-valued vector. However, for many quantities, most notably orientations in 3D, S is not a vector space, but a so-called manifold, i.e. it behaves like a vector space locally but has a more complex global topological structure. For integrating these quantities, several ad-hoc approaches have been proposed.

Abstract Real-life processes are mostly characterized by several operating regimes or modes. Each operating mode corresponds to particular operating conditions and may require the use of a specific control strategy. Therefore, the task of identifying or determining continuously the current operating mode of the process is of a great value as it allows to implement the control strategy that is in accordance with the current operating mode of the process.