Uwe D. Hanebeck

Markus Walker, Daniel Frisch, Uwe D. Hanebeck

Cross-entropy method model predictive control (CEM--MPC) is a powerful gradient-free technique for nonlinear optimal control, but its performance is often limited by the reliance on random sampling. This conventional approach can lead to inefficient exploration of the solution space and non-smooth control inputs, requiring a large number of samples to achieve satisfactory results. To address these limitations, we propose deterministic sampling CEM (dsCEM), a novel framework that replaces the random sampling step with deterministic samples derived from localized cumulative distributions (LCDs). Our approach introduces modular schemes to generate and adapt these sample sets, incorporating temporal correlations to ensure smooth control trajectories. This method can be used as a drop-in replacement for the sampling step in existing CEM-based controllers. Experimental evaluations on two nonlinear control tasks demonstrate that dsCEM consistently outperforms state-of-the-art iCEM in terms of cumulative cost and control input smoothness, particularly in the critical low-sample regime.

Markus Walker, Marcel Reith-Braun, Tai Hoang et al.

Sampling-based model predictive control (MPC) is effective for nonlinear systems but often produces non-smooth control inputs due to random sampling. To address this issue, we extend the model predictive path integral (MPPI) framework with deterministic sampling and improvements from cross-entropy method (CEM)--MPC, such as iterative optimization, proposing deterministic sampling MPPI (dsMPPI). This combination leverages the exponential weighting of MPPI alongside the efficiency of deterministic samples. Experiments demonstrate that dsMPPI achieves smoother trajectories compared to state-of-the-art methods.

Jörg Fischer, Achim Hekler, Maxim Dolgov et al.

This paper addresses the problem of sequence-based controller design for Networked Control Systems (NCS), where control inputs and measurements are transmitted over TCP-like network connections that are subject to stochastic packet losses and time-varying packet delays. At every time step, the controller sends a sequence of predicted control inputs to the actuator in addition to the current control input. In this sequence-based setup, we derive an optimal solution to the Linear Quadratic Gaussian (LQG) control problem and prove that the separation principle holds. Simulations demonstrate the improved performance of this optimal controller compared to other sequence-based approaches.

Daniel Lyons, Jan-P. Calliess, Uwe D. Hanebeck

We consider stochastic model predictive control of a multi-agent systems with constraints on the probabilities of inter-agent collisions. We first study a sample-based approximation of the collision probabilities and use this approximation to formulate constraints for the stochastic control problem. This approximation will converge as the number of samples goes to infinity, however, the complexity of the resulting control problem is so high that this approach proves unsuitable for control under real-time requirements. To alleviate the computational burden we propose a second approach that uses probabilistic bounds to determine regions with increased probability of presence for each agent and formulate constraints for the control problem that guarantee that these regions will not overlap. We prove that the resulting problem is conservative for the original problem with probabilistic constraints, ie. every control strategy that is feasible under our new constraints will automatically be feasible for the original problem. Furthermore we show in simulations in a UAV path planning scenario that our proposed approach grants significantly better run-time performance compared to a controller with the sample-based approximation with only a small degree of sub-optimality resulting from the conservativeness of our new approach.

Marcus Baum, Uwe D. Hanebeck

We present a novel method called Kernel-SME filter for tracking multiple targets when the association of the measurements to the targets is unknown. The method is a further development of the Symmetric Measurement Equation (SME) filter, which removes the data association uncertainty of the original measurement equation with the help of a symmetric transformation. The underlying idea of the Kernel-SME filter is to construct a symmetric transformation by means of mapping the measurements to a Gaussian mixture. This transformation is scalable to a large number of targets and allows for deriving a Gaussian state estimator that has a cubic time complexity in the number of targets.

Achim Hekler, Jörg Fischer, Uwe D. Hanebeck

In this paper, we address the problem of controlling a system over an unreliable connection that is affected by time-varying delays and randomly occurring packet losses. A novel sequence-based approach is proposed that extends a given controller designed without consideration of the network-induced disturbances. Its key idea is to model the unknown future control inputs by random variables, the so-called virtual control inputs, which are characterized by discrete probability density functions. Subject to this probabilistic description, the actual sequence of future control inputs is determined and transmitted to the actuator. The high performance of the proposed approach is demonstrated by means of Monte Carlo simulation runs with an inverted pendulum on a cart and by a detailed comparison to standard NCS approaches.

Jannik Steinbring, Christian Mandery, Nikolaus Vahrenkamp et al.

We present a new online approach to track human whole-body motion from motion capture data, i.e., positions of labeled markers attached to the human body. Tracking in noisy data can be effectively performed with the aid of well-established recursive state estimation techniques. This allows us to systematically take noise of the marker measurements into account. However, as joint limits imposed by the human body have to be satisfied during estimation, first we transform this constrained estimation problem into an unconstrained one by using periodic functions. Then, we apply the Smart Sampling Kalman Filter to solve this unconstrained estimation problem. The proposed recursive state estimation approach makes the human motion tracking very robust to partial occlusion of markers and avoids any special treatment or reconstruction of the missed markers. A concrete implementation built on the kinematic human reference model of the Master Motor Map framework and a Vicon motion capture system is evaluated. Different captured motions show that our implementation can accurately estimate whole-body human motion in real-time and outperforms existing gradient-based approaches. In addition, we demonstrate its ability to smoothly handle incomplete marker data.

Uwe D. Hanebeck

We propose a homotopy continuation method called FLUX for approximating complicated probability density functions. It is based on progressive processing for smoothly morphing a given density into the desired one. Distributed ordinary differential equations (DODEs) with an artificial time $γ\in [0,1]$ are derived for describing the evolution from the initial density to the desired final density. For a finite-dimensional parametrization, the DODEs are converted to a system of ordinary differential equations (SODEs), which are solved for $γ\in [0,1]$ and return the desired result for $γ=1$. This includes parametric representations such as Gaussians or Gaussian mixtures and nonparametric setups such as sample sets. In the latter case, we obtain a particle flow between the two densities along the artificial time. FLUX is applied to state estimation in stochastic nonlinear dynamic systems by gradual inclusion of measurement information. The proposed approximation method (1) is fast, (2) can be applied to arbitrary nonlinear systems and is not limited to additive noise, (3) allows for target densities that are only known at certain points, (4) does not require optimization, (5) does not require the solution of partial differential equations, and (6) works with standard procedures for solving SODEs. This manuscript is limited to the one-dimensional case and a fixed number of parameters during the progression. Future extensions will include consideration of higher dimensions and on the fly adaption of the number of parameters.

Uwe D. Hanebeck

We assume that a finite set of moments of a random vector is given. Its underlying density is unknown. An algorithm is proposed for efficiently calculating Dirac mixture densities maintaining these moments while providing a homogeneous coverage of the state space.

Jörg Fischer, Marc Reinhardt, Uwe D. Hanebeck

In this paper, a unified approach to sequence-based control and estimation of linear networked systems with multiple sensors is proposed. Time delays and data losses in the controller-actuator-channel are compensated by sending sequences of control inputs. The sequence-based design paradigm is further extended to the sensor-controller-channels without increasing the load of the network. In this context, we present a recursive solution based on the Hypothesizing Distributed Kalman Filter (HKF) that is included in the overall sequence-based controller design.

Kailai Li, Meng Li, Uwe D. Hanebeck

We present a novel tightly-coupled LiDAR-inertial odometry and mapping scheme for both solid-state and mechanical LiDARs. As frontend, a feature-based lightweight LiDAR odometry provides fast motion estimates for adaptive keyframe selection. As backend, a hierarchical keyframe-based sliding window optimization is performed through marginalization for directly fusing IMU and LiDAR measurements. For the Livox Horizon, a newly released solid-state LiDAR, a novel feature extraction method is proposed to handle its irregular scan pattern during preprocessing. LiLi-OM (Livox LiDAR-inertial odometry and mapping) is real-time capable and achieves superior accuracy over state-of-the-art systems for both LiDAR types on public data sets of mechanical LiDARs and in experiments using the Livox Horizon. Source code and recorded experimental data sets are available at https://github.com/KIT-ISAS/lili-om.

Antonio Zea, Uwe D. Hanebeck

In this work, we introduce iviz, a mobile application for visualizing ROS data. In the last few years, the popularity of ROS has grown enormously, making it the standard platform for open source robotic programming. A key reason for this success is the availability of polished, general-purpose modules for many tasks, such as localization, mapping, path planning, and quite importantly, data visualization. However, the availability of the latter is generally restricted to PCs with the Linux operating system. Thus, users that want to see what is happening in the system with a smartphone or a tablet are stuck with solutions such as screen mirroring or using web browser versions of rviz, which are difficult to interact with from a mobile interface. More importantly, this makes newer visualization modalities such as Augmented Reality impossible. Our application iviz, based on the Unity engine, addresses these issues by providing a visualization platform designed from scratch to be usable in mobile platforms, such as iOS, Android, and UWP, and including native support for Augmented Reality for all three platforms. If desired, it can also be used in a PC with Linux, Windows, or macOS without any changes.

Markus Walker, Marcel Reith-Braun, Daniel Frisch et al.

Sampling-based model predictive control (MPC) algorithms, such as model predictive path integral (MPPI), enable approximate, gradient-free solutions to optimal control problems by drawing samples from a proposal distribution, evaluating their trajectory costs, and updating the proposal parameters accordingly. However, these approaches typically rely on heuristics for adjusting hyperparameters, such as temperature or momentum, or manual tuning. We propose a trust region formulation for sampling-based MPC that constrains updates of the proposal distribution via a principled Kullback--Leibler (KL) divergence bound and, optionally, an entropy lower bound. This replaces heuristic hyperparameter adaptation with values that are optimal w.r.t. the underlying Lagrangian. We further improve sample efficiency and convergence by combining the trust region update with deterministic localized cumulative distribution (LCD)-based sampling. Experiments on two benchmark environments demonstrate that the proposed trust region update achieves faster convergence and better sample efficiency in low-sample and low-iteration regimes, especially when paired with deterministic LCD-based sampling.

Uwe D. Hanebeck

We propose a recursive particle filter for high-dimensional problems that inherently never degenerates. The state estimate is represented by deterministic low-discrepancy particle sets. We focus on the measurement update step, where a likelihood function is used for representing the measurement and its uncertainty. This likelihood is progressively introduced into the filtering procedure by homotopy continuation over an artificial time. A generalized Cramér distance between particle sets is derived in closed form that is differentiable and invariant to particle order. A Newton flow then continually minimizes this distance over artificial time and thus smoothly moves particles from prior to posterior density. The new filter is surprisingly simple to implement and very efficient. It just requires a prior particle set and a likelihood function, never estimates densities from samples, and can be used as a plugin replacement for classic approaches.

Daniel Frisch, Uwe D. Hanebeck

We consider estimating the parameters of a Gaussian mixture density with a given number of components best representing a given set of weighted samples. We adopt a density interpretation of the samples by viewing them as a discrete Dirac mixture density over a continuous domain with weighted components. Hence, Gaussian mixture fitting is viewed as density re-approximation. In order to speed up computation, an expectation-maximization method is proposed that properly considers not only the sample locations, but also the corresponding weights. It is shown that methods from literature do not treat the weights correctly, resulting in wrong estimates. This is demonstrated with simple counterexamples. The proposed method works in any number of dimensions with the same computational load as standard Gaussian mixture estimators for unweighted samples.

Gerhard Kurz, Florian Faion, Florian Pfaff et al.

We propose a new recursive method for simultaneous estimation of both the pose and the shape of a three-dimensional extended object. The key idea of the presented method is to represent the shape of the object using spherical harmonics, similar to the way Fourier series can be used in the two-dimensional case. This allows us to derive a measurement equation that can be used within the framework of nonlinear filters such as the UKF. We provide both simulative and experimental evaluations of the novel techniques.

Kailai Li, Johannes Cox, Benjamin Noack et al.

We present a novel Riemannian approach for planar pose graph optimization problems. By formulating the cost function based on the Riemannian metric on the manifold of dual quaternions representing planar motions, the nonlinear structure of the SE(2) group is inherently considered. To solve the on-manifold least squares problem, a Riemannian Gauss-Newton method using the exponential retraction is applied. The proposed Riemannian pose graph optimizer (RPG-Opt) is further evaluated based on public planar pose graph data sets. Compared with state-of-the-art frameworks, the proposed method gives equivalent accuracy and better convergence robustness under large uncertainties of odometry measurements.

Maxim Dolgov, Uwe D. Hanebeck

Gaussian processes constitute a very powerful and well-understood method for non-parametric regression and classification. In the classical framework, the training data consists of deterministic vector-valued inputs and the corresponding (noisy) measurements whose joint distribution is assumed to be Gaussian. In many practical applications, however, the inputs are either noisy, i.e., each input is a vector-valued sample from an unknown probability distribution, or the probability distributions are the inputs. In this paper, we address Gaussian process regression with inputs given in form of probability distributions and propose a framework that is based on distances between such inputs. To this end, we review different admissible distance measures and provide a numerical example that demonstrates our framework.

Gerhard Kurz, Igor Gilitschenski, Florian Pfaff et al.

In this paper, we present libDirectional, a MATLAB library for directional statistics and directional estimation. It supports a variety of commonly used distributions on the unit circle, such as the von Mises, wrapped normal, and wrapped Cauchy distributions. Furthermore, various distributions on higher-dimensional manifolds such as the unit hypersphere and the hypertorus are available. Based on these distributions, several recursive filtering algorithms in libDirectional allow estimation on these manifolds. The functionality is implemented in a clear, well-documented, and object-oriented structure that is both easy to use and easy to extend.

Maxim Dolgov, Uwe D. Hanebeck

In this paper, we consider stochastic optimal control of Markov Jump Linear Systems with state feedback but without observation of the jumping parameter. The proposed control law is assumed to be linear with constant gains that can be obtained from the necessary optimality conditions using an iterative algorithm. The proposed approach is demonstrated in a numerical example.

Jannik Steinbring, Martin Pander, Uwe D. Hanebeck

Nonlinear Kalman Filters are powerful and widely-used techniques when trying to estimate the hidden state of a stochastic nonlinear dynamic system. In this paper, we extend the Smart Sampling Kalman Filter (S2KF) with a new point symmetric Gaussian sampling scheme. This not only improves the S2KF's estimation quality, but also reduces the time needed to compute the required optimal Gaussian samples drastically. Moreover, we improve the numerical stability of the sample computation, which allows us to accurately approximate a thousand-dimensional Gaussian distribution using tens of thousands of optimally placed samples. We evaluate the new symmetric S2KF by computing higher-order moments of standard normal distributions and investigate the estimation quality of the S2KF when dealing with symmetric measurement equations. Finally, extended object tracking based on many measurements per time step is considered. This high-dimensional estimation problem shows the advantage of the S2KF being able to use an arbitrary number of samples independent of the state dimension, in contrast to other state-of-the-art sample-based Kalman Filters.

Gerhard Kurz, Igor Gilitschenski, Uwe D. Hanebeck

For recursive circular filtering based on circular statistics, we introduce a general framework for estimation of a circular state based on different circular distributions, specifically the wrapped normal distribution and the von Mises distribution. We propose an estimation method for circular systems with nonlinear system and measurement functions. This is achieved by relying on efficient deterministic sampling techniques. Furthermore, we show how the calculations can be simplified in a variety of important special cases, such as systems with additive noise as well as identity system or measurement functions. We introduce several novel key components, particularly a distribution-free prediction algorithm, a new and superior formula for the multiplication of wrapped normal densities, and the ability to deal with non-additive system noise. All proposed methods are thoroughly evaluated and compared to several state-of-the-art solutions.

Nicole Bäuerle, Igor Gilitschenski, Uwe D. Hanebeck

We consider a Hidden Markov Model (HMM) where the integrated continuous-time Markov chain can be observed at discrete time points perturbed by a Brownian motion. The aim is to derive a filter for the underlying continuous-time Markov chain. The recursion formula for the discrete-time filter is easy to derive, however involves densities which are very hard to obtain. In this paper we derive exact formulas for the necessary densities in the case the state space of the HMM consists of two elements only. This is done by relating the underlying integrated continuous-time Markov chain to the so-called asymmetric telegraph process and by using recent results on this process. In case the state space consists of more than two elements we present three different ways to approximate the densities for the filter. The first approach is based on the continuous filter problem. The second approach is to derive a PDE for the densities and solve it numerically and the third approach is a crude discrete time approximation of the Markov chain. All three approaches are compared in a numerical study.

Uwe D. Hanebeck

This paper is concerned with the optimal approximation of a given multivariate Dirac mixture, i.e., a density comprising weighted Dirac distributions on a continuous domain, by an equally weighted Dirac mixture with a reduced number of components. The parameters of the approximating density are calculated by minimizing a smooth global distance measure, a generalization of the well-known Cramér-von Mises Distance to the multivariate case. This generalization is achieved by defining an alternative to the classical cumulative distribution, the Localized Cumulative Distribution (LCD), as a characterization of discrete random quantities (on continuous domains), which is unique and symmetric also in the multivariate case. The resulting approximation method provides the basis for various efficient nonlinear state and parameter estimation methods.

Igor Gilitschenski, Gerhard Kurz, Simon J. Julier et al.

Orientation estimation for 3D objects is a common problem that is usually tackled with traditional nonlinear filtering techniques such as the extended Kalman filter (EKF) or the unscented Kalman filter (UKF). Most of these techniques assume Gaussian distributions to account for system noise and uncertain measurements. This distributional assumption does not consider the periodic nature of pose and orientation uncertainty. We propose a filter that considers the periodicity of the orientation estimation problem in its distributional assumption. This is achieved by making use of the Bingham distribution, which is defined on the hypersphere and thus inherently more suitable to periodic problems. Furthermore, handling of non-trivial system functions is done using deterministic sampling in an efficient way. A deterministic sampling scheme reminiscent of the UKF is proposed for the nonlinear manifold of orientations. It is the first deterministic sampling scheme that truly reflects the nonlinear manifold of the orientation.

Gerhard Kurz, Igor Gilitschenski, Simon Julier et al.

Directional estimation is a common problem in many tracking applications. Traditional filters such as the Kalman filter perform poorly because they fail to take the periodic nature of the problem into account. We present a recursive filter for directional data based on the Bingham distribution in two dimensions. The proposed filter can be applied to circular filtering problems with 180 degree symmetry, i.e., rotations by 180 degrees cannot be distinguished. It is easily implemented using standard numerical techniques and suitable for real-time applications. The presented approach is extensible to quaternions, which allow tracking arbitrary three-dimensional orientations. We evaluate our filter in a challenging scenario and compare it to a traditional Kalman filtering approach.

Uwe D. Hanebeck, Jannik Steinbring

In this paper, we propose a progressive Bayesian procedure, where the measurement information is continuously included into the given prior estimate (although we perform observations at discrete time steps). The key idea is to derive a system of ordinary first-order differential equations (ODE) by employing a new coupled density representation comprising a Gaussian density and its Dirac Mixture approximation. The ODE is used for continuously tracking the true non-Gaussian posterior by its best matching Gaussian approximation. The performance of the new filter is evaluated in comparison with state-of-the-art filters by means of a canonical benchmark example, the discrete-time cubic sensor problem.

Marc Peter Deisenroth, Ryan Turner, Marco F. Huber et al.

We propose a principled algorithm for robust Bayesian filtering and smoothing in nonlinear stochastic dynamic systems when both the transition function and the measurement function are described by non-parametric Gaussian process (GP) models. GPs are gaining increasing importance in signal processing, machine learning, robotics, and control for representing unknown system functions by posterior probability distributions. This modern way of "system identification" is more robust than finding point estimates of a parametric function representation. In this article, we present a principled algorithm for robust analytic smoothing in GP dynamic systems, which are increasingly used in robotics and control. Our numerical evaluations demonstrate the robustness of the proposed approach in situations where other state-of-the-art Gaussian filters and smoothers can fail.