Robust Filtering and Smoothing with Gaussian Processes

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.Comment: 7 pages, 1 figure, draft version of paper accepted at IEEE Transactions on Automatic Contro

Particle tracking using the unscented Kalman filter in high energy physics experiments

This thesis was submitted for the award of Doctor of Philosophy and was awarded by Brunel University London.The extended Kalman lter (EKF) has a long history in the field of non-linear tracking. More recently, statistically-based estimators have emerged that avoid the need for a deterministic linearisation process. The Unscented Kalman filter (UKF) is one such technique that has been shown to perform favourably for some non-linear systems when compared to an EKF implementation, both in terms of accuracy and robustness. In this Thesis, the UKF is applied to a high energy physics particle tracking problem where currently the EKF is being implemented. The effects of measurement redundancy are investigated to determine improvements in accuracy of particle track reconstruction. The relationship between measurement redundancy and relative observability is also investigated through an experimental and theoretical analysis. Smoothing (backward filtering), in the high energy physics experiments, is implementedusing the Rauch Tung Striebel (RTS) smoother with the EKF , however, in Unscented Kalman filter algorithms, the Jacobian matrices required by the RTS method, are not available. The Unscented Rauch Tung Striebel (URTS) smoother addresses this problem by avoiding the use of Jacobian matrices but is not effi cient for large dimensional systems such as high energy physics experiments. A technique is implemented in the RTS smoother to make it suitable for the UKF. The method is given the name the Jacobian Equivalent Rauch Tung Striebel (JE-RTS) smoother. The implementation of this method is quite straight forward when the UKF is used as an estimator

On-Manifold Preintegration for Real-Time Visual-Inertial Odometry

Current approaches for visual-inertial odometry (VIO) are able to attain highly accurate state estimation via nonlinear optimization. However, real-time optimization quickly becomes infeasible as the trajectory grows over time, this problem is further emphasized by the fact that inertial measurements come at high rate, hence leading to fast growth of the number of variables in the optimization. In this paper, we address this issue by preintegrating inertial measurements between selected keyframes into single relative motion constraints. Our first contribution is a \emph{preintegration theory} that properly addresses the manifold structure of the rotation group. We formally discuss the generative measurement model as well as the nature of the rotation noise and derive the expression for the \emph{maximum a posteriori} state estimator. Our theoretical development enables the computation of all necessary Jacobians for the optimization and a-posteriori bias correction in analytic form. The second contribution is to show that the preintegrated IMU model can be seamlessly integrated into a visual-inertial pipeline under the unifying framework of factor graphs. This enables the application of incremental-smoothing algorithms and the use of a \emph{structureless} model for visual measurements, which avoids optimizing over the 3D points, further accelerating the computation. We perform an extensive evaluation of our monocular \VIO pipeline on real and simulated datasets. The results confirm that our modelling effort leads to accurate state estimation in real-time, outperforming state-of-the-art approaches.Comment: 20 pages, 24 figures, accepted for publication in IEEE Transactions on Robotics (TRO) 201

Optimization viewpoint on Kalman smoothing, with applications to robust and sparse estimation

In this paper, we present the optimization formulation of the Kalman filtering and smoothing problems, and use this perspective to develop a variety of extensions and applications. We first formulate classic Kalman smoothing as a least squares problem, highlight special structure, and show that the classic filtering and smoothing algorithms are equivalent to a particular algorithm for solving this problem. Once this equivalence is established, we present extensions of Kalman smoothing to systems with nonlinear process and measurement models, systems with linear and nonlinear inequality constraints, systems with outliers in the measurements or sudden changes in the state, and systems where the sparsity of the state sequence must be accounted for. All extensions preserve the computational efficiency of the classic algorithms, and most of the extensions are illustrated with numerical examples, which are part of an open source Kalman smoothing Matlab/Octave package.Comment: 46 pages, 11 figure

Orbit Estimation of Non-Cooperative Maneuvering Spacecraft

Due to the ever increasing congestion of the space environment, there is an increased demand for real-time situation awareness of all objects in space. An unknown spacecraft maneuver changes the predicted orbit, complicates tracking, and degrades estimate accuracies. Traditional orbit estimation routines are implemented, tested, and compared to a multiple model format that adaptively handles unknown maneuvers. Multiple Model Adaptive Estimation is implemented in an original way to track a non-cooperative satellite by covariance inflation and filtering-through a maneuver. Parameters for successful instantaneous maneuver reconstruction are analyzed. Variable State Dimension estimation of a continuously maneuvering spacecraft is investigated. A requirements based analysis is performed on short arc orbital solutions. Large covariance propagation of potential maneuvers is explored. Using ground-based radars, several thousand simulations are run to develop new techniques to estimate orbits during and after both instantaneous and continuous maneuvers. The new methods discovered are more accurate by a factor of 700 after only a single pass when compared to non-adaptive methods. The algorithms, tactics, and analysis complement on-going efforts to improve Space Situational Awareness and dynamic modeling

Iterative State Estimation in Non-linear Dynamical Systems Using Approximate Expectation Propagation

Bayesian inference in non-linear dynamical systems seeks to find good posterior approximations of a latent state given a sequence of observations. Gaussian filters and smoothers, including the (extended/unscented) Kalman filter/smoother, which are commonly used in engineering applications, yield Gaussian posteriors on the latent state. While they are computationally efficient, they are often criticised for their crude approximation of the posterior state distribution. In this paper, we address this criticism by proposing a message passing scheme for iterative state estimation in non-linear dynamical systems, which yields more informative (Gaussian) posteriors on the latent states. Our message passing scheme is based on expectation propagation (EP). We prove that classical Rauch--Tung--Striebel (RTS) smoothers, such as the extended Kalman smoother (EKS) or the unscented Kalman smoother (UKS), are special cases of our message passing scheme. Running the message passing scheme more than once can lead to significant improvements of the classical RTS smoothers, so that more informative state estimates can be obtained. We address potential convergence issues of EP by generalising our state estimation framework to damped updates and the consideration of general alpha-divergences

Unified Forms for Kalman and Finite Impulse Response Filtering and Smoothing

The Kalman filter and smoother are optimal state estimators under certain conditions. The Kalman filter is typically presented in a predictor/corrector format, but the Kalman smoother has never been derived in that format. We derive the Kalman smoother in a predictor/corrector format, thus providing a unified form for the Kalman filter and smoother. We also discuss unbiased finite impulse response (UFIR) filters and smoothers, which can provide a suboptimal but robust alternative to Kalman estimators. We derive two unified forms for UFIR filters and smoothers, and we derive lower and upper bounds for their estimation error covariances