Asynchronous sensor fusion of GPS, IMU and CAN-based odometry for heavy-duty vehicles

In heavy-duty vehicles, multiple signals are available to estimate the vehicle's kinematics, such as Inertial Measurement Unit (IMU), Global Positioning System (GPS) and linear and angular speed readings from wheel tachometers on the internal Controller Area Network (CAN). These signals have different noise variance, bandwidth and sampling rate (being the latter, possibly, irregular). In this paper we present a non-linear sensor fusion algorithm allowing asynchronous sampling and non-causal smoothing. It is applied to achieve accuracy improvements when incorporating odometry measurements from CAN bus to standard GPS+IMU kinematic estimation, as well as the robustness against missing data. Our results show that this asynchronous multi-sensor (GPS+IMU+CAN-based odometry) fusion is advantageous in low-speed manoeuvres, improving accuracy and robustness to missing data, thanks to non-causal filtering. The proposed algorithm is based on Extended Kalman Filter and Smoother, with exponential discretization of continuous-time stochastic differential equations, in order to process measurements at arbitrary time instants; it can provide data to subsequent processing steps at arbitrary time instants, not necessarily coincident with the original measurement ones. Given the extra information available in the smoothing case, its estimation performance is less sensitive to the noise-variance parameter setting, compared to causal filtering. Working Matlab code is provided at the end of this work

Gaussiseen suodatukseen ja siloitukseen perustuva parametrien estimointi epälineaarisissa aikasarjamalleissa

State space modeling is a widely used statistical approach for sequential data. The resulting models can be considered to contain two interconnected estimation problems: that of the dynamic states and that of the static parameters. The difficulty of these problems depends critically on the linearity of the model, with respect to the states, the parameters or both. In this thesis we show how to obtain maximum likelihood and maximum a posteriori estimates for the static parameters. Two methods are considered: gradient based nonlinear optimization of the marginal log-likelihood and expectation maximization. The former requires the filtering distributions and the latter both the filtering and the smoothing distributions. When closed form solutions to these distributions are unavailable, we apply efficient Gaussian filtering based methods to obtain approximations. The resulting parameter estimation algorithms are demonstrated by a linear target-tracking model with simulated data and a nonlinear stochastic resonator model with photoplethysmograph data.Tila-avaruusmallinnus on eräs laajalti käytetty aikasarjojen mallinnusmenetelmä. Tila-avaruusmallin voidaan ajatella sisältävän kaksi keskenään vuorovaikkuteista estimointiongelmaa: dynaamisten tilojen estimointi sekä staattisten parametrien estimointi. Näiden estimointiongelmien vaikeuteen vaikuttaa erityisen paljon mallin lineaarisuus - sekä tilojen että parametrien suhteen. Tässä diplomityössä näytämme, kuinka staattisia parametrejä voidaan estimoida suurimman uskottavuuden estimaattorilla tai posteriorijakauman maksimoivalla estimaattorilla. Analysoimme kahta eri menetelmää: uskottavuusfunktion gradienttipohjaista epälineerista optimointia sekä expectation maximization algoritmiä. Näistä ensimmäinen vaatii suodinjakaumien ja jälkimmäinen sekä suodin- että siloitusjakaumien ratkaisemista. Mikäli näitä jakaumia ei voida ratkaista suljetussa muodossa, käytämme tehokkaita Gaussiseen suodatukseen perustuvia menetelmiä niiden likimääräiseen ratkaisemiseen. Lopputuloksina saatuja parametriestimointimenetelmiä sovelletaan ensin lineaarisessa kohteenseurantamallissa simuloidulla datalla ja sen jälkeen epälineaarisessa stokastisessa resonaattorimallissa fotopletysmografidatalla

A Novel Robust Rauch-Tung-Striebel Smoother Based on Slash and Generalized Hyperbolic Skew Student’s T-Distributions

In this paper, a novel robust Rauch-Tung-Striebel smoother is proposed based on the Slash and generalized hyperbolic skew Student’s t-distributions. A novel hierarchical Gaussian state-space model is constructed by formulating the Slash distribution as a Gaussian scale mixture form and formulating the generalized hyperbolic skew Student’s t-distribution as a Gaussian variance-mean mixture form, based on which the state trajectory, mixing parameters and unknown noise parameters are jointly inferred using the variational Bayesian approach. The posterior probability density functions of mixing parameters of the Slash and generalized hyperbolic skew Student’s t-distributions are, respectively, approximated as truncated Gamma and generalized inverse Gaussian. Simulation results illustrate that the proposed robust Rauch-Tung-Striebel smoother has better estimation accuracy than existing state-of-the-art smoothers

Robust Rauch-Tung-Striebel smoothing framework for heavy-tailed and/or skew noises

A novel robust Rauch-Tung-Striebel smoothing framework is proposed based on a generalized Gaussian scale mixture (GGScM) distribution for a linear state-space model with heavy-tailed and/or skew noises. The state trajectory, mixing parameters and unknown distribution parameters are jointly inferred using the variational Bayesian approach. As such, a major contribution of this work is unifying results within the GGScM distribution framework. Simulation and experimental results demonstrate that the proposed smoother has better accuracy than existing smoothers

Estimação de parâmetros em tempo real através de filtro de Kalman com janela robusta suavizante e estimadores de estados não lineares

Os estimadores de estado, ou observadores, são técnicas que reconstroem os estados de um modelo dinâmico a partir das medidas de entrada e saída do sistema. Eles podem ser baseados na teoria probabilística (proposto por Kalman), que considera ruídos no modelo ou na teoria determinística (introduzida por Luenberger) sem a presença de ruídos. Embora, na sua gênese, o controle “moderno” tenha motivado o surgimento dessas técnicas em 1960, os estimadores de estado são hoje em dia aplicados também em reconciliação de dados, analisadores virtuais, estimação de parâmetros, gêmeos digitais e detecção de falhas. Por isso, esta tese aborda um estudo sobre filtros de Kalman e suas aplicações focado, principalmente, no uso de janela robusta suavizante. As principais contribuições do trabalho são: (1) revisão bibliográfica histórica dos estimadores de estado, abordando suas principais interligações e características, incluindo uma motivação prática de suas utilizações; (2) avaliação de cinco metodologias de filtro de Kalman (estendido - EKF, estendido com restrições - CEKF, formulação curta do estendido com restrições – CEKF2, estendido com restrições e suavizado - CEKFS, sem rastro - UKF, e de cubatura – CKF implementadas a dados industriais, mostrando a sua capacidade de aplicação em casos reais, sendo eles, na produção de petróleo offshore e em uma rede de trocadores de calor; (3) proposta de técnica de estimação de bias em casos em que o estimador não linear retorna resultados insatisfatórios; (4) avaliação de três métodos de estimadores de estado com horizonte móvel para estimação simultânea de estados e parâmetros (estimação do horizonte móvel - MHE, com horizonte retrocedido - RNK, e robusto com horizonte retrocedido - RRNK); e (5) apresentação de formulação robusta e simples para problema de otimização do RNK e RRNK utilizando programação quadrática. De modo geral os filtros de Kalman não-lineares (UKF e CKF) retornam melhores resultados para os dados industriais quando o modelo está bem ajustado. No entanto, eles possuem elevado custo computacional e desempenho insatisfatório para modelos mal ajustados, enquanto os filtros estendidos não apresentam essas desvantagens. Por isso, utilizando técnica simples da estimação de bias como uma variável através de técnica de estado aumentado, o filtro de Kalman sem rastro e de cubatura se mostraram mais acurados, mesmo em um cenário de ajuste inadequado do modelo. Para a estimação simultânea de estados e parâmetros, o RRNK exibiu as suas vantagens na redução de erros de modelagem, retornando parâmetros mais suavizados. Nesse sentido, a reformulação dos problemas de otimização do RNK e RRNK em uma formulação de programação quadrática simples e robusta obteve um custo computacional nove vezes menor que o MHE.State estimators, or observers, are techniques that reconstruct the states of a dynamical model from the input and output measures of the system. They can be based on the probabilistic theory (proposed by Kalman), which considers noise in the model, or on the deterministic theory (introduced by Luenberger) without the presence of noise. Although in its genesis, “modern” control motivated the emergence of these techniques in 1960, state estimators are nowadays also applied in data reconciliation, virtual analyzers, parameter estimation, digital twins, and fault detection. For this reason, this thesis addresses a study on Kalman filters and their applications, focused mainly on the use of a robust softening window. The main contributions of the work are: (1) historical bibliographic review of state estimators, addressing their main interconnections and characteristics, including a practical motivation for their uses; (2) evaluation of five Kalman filter methodologies (extended – EKF, constrained extended – CEKF, short formulation of the constrained extended – CEKF2, constrained extended and smoother – CEKFS, unscented – UKF, cubature – CKF) implemented to industrial data, showing their ability to be applied in real cases, namely in offshore oil production and in a heat exchanger network; (3) proposal of bias estimation technique in cases where the nonlinear estimator returns unsatisfactory results; (4) evaluation of three methods of state estimators with moving window for simultaneous state and parameter estimation (moving horizon horizon – MHE, receding nonlinear Kalman filter – RNK, and robust receding nonlinear Kalman filter – RRNK), and (5) presentation of robust and simple formulation for RNK and RRNK optimization problem using quadratic programming. In general, non-linear Kalman filters (UKF and CKF) return better results for industrial data when the model is well adjusted. However, they have high computational costs and poor performance for poorly adjusted models, while extended filters do not present these disadvantages. Therefore, using a simple bias estimation technique as a variable using an increased state technique, the unscented and cubature Kalman filter proved to be more accurate, even in a scenario of inadequate model adjustment. For the simultaneous state and parameter estimation, the RRNK showed its advantages in reducing modeling errors, returning more smoothed parameters. In this sense, the RNK and RRNK optimization problems’ reformulation in a robust and straightforwards quadratic programming formulation obtained a computational cost nine times smaller than the MHE

Approximate Bayesian inference methods for stochastic state space models

This thesis collects together research results obtained during my doctoral studies related to approximate Bayesian inference in stochastic state-space models. The published research spans a variety of topics including 1) application of Gaussian filtering in satellite orbit prediction, 2) outlier robust linear regression using variational Bayes (VB) approximation, 3) filtering and smoothing in continuous-discrete Gaussian models using VB approximation and 4) parameter estimation using twisted particle filters. The main goal of the introductory part of the thesis is to connect the results to the general framework of estimation of state and model parameters and present them in a unified manner.Bayesian inference for non-linear state space models generally requires use of approximations, since the exact posterior distribution is readily available only for a few special cases. The approximation methods can be roughly classified into to groups: deterministic methods, where the intractable posterior distribution is approximated from a family of more tractable distributions (e.g. Gaussian and VB approximations), and stochastic sampling based methods (e.g. particle filters). Gaussian approximation refers to directly approximating the posterior with a Gaussian distribution, and can be readily applied for models with Gaussian process and measurement noise. Well known examples are the extended Kalman filter and sigma-point based unscented Kalman filter. The VB method is based on minimizing the Kullback-Leibler divergence of the true posterior with respect to the approximate distribution, chosen from a family of more tractable simpler distributions.The first main contribution of the thesis is the development of a VB approximation for linear regression problems with outlier robust measurement distributions. A broad family of outlier robust distributions can be presented as an infinite mixture of Gaussians, called Gaussian scale mixture models, and include e.g. the t-distribution, the Laplace distribution and the contaminated normal distribution. The VB approximation for the regression problem can be readily extended to the estimation of state space models and is presented in the introductory part.VB approximations can be also used for approximate inference in continuous-discrete Gaussian models, where the dynamics are modeled with stochastic differential equations and measurements are obtained at discrete time instants. The second main contribution is the presentation of a VB approximation for these models and the explanation of how the resulting algorithm connects to the Gaussian filtering and smoothing framework.The third contribution of the thesis is the development of parameter estimation using particle Markov Chain Monte Carlo (PMCMC) method and twisted particle filters. Twisted particle filters are obtained from standard particle filters by applying a special weighting to the sampling law of the filter. The weighting is chosen to minimize the variance of the marginal likelihood estimate, and the resulting particle filter is more efficient than conventional PMCMC algorithms. The exact optimal weighting is generally not available, but can be approximated using the Gaussian filtering and smoothing framework

Conjugate priors for Bayesian object tracking

Object tracking refers to the problem of using noisy sensor measurements to determine the location and characteristics of objects of interest in clutter. Nowadays, object tracking has found applications in numerous research venues as well as application areas, including air traffic control, maritime navigation, remote sensing, intelligent video surveillance, and more recently environmental perception, which is a key enabling technology in autonomous vehicles. This thesis studies conjugate priors for Bayesian object tracking with focus on multi-object tracking (MOT) based on sets of trajectories. Finite Set Statistics provides an elegant Bayesian formulation of MOT in terms of the theory of random finite sets (RFSs). Conjugate priors are also of great interest as they provide families of distributions that are suitable to work with when seeking accurate approximations to the true posterior distributions. Many RFS-based MOT approaches are only concerned with multi-object filtering without attempting to estimate object trajectories. An appealing approach to building tracks is by computing the multi-object densities on sets of trajectories. This leads to the development of trajectory filters, e.g., filters based on Poisson multi-Bernoulli mixture (PMBM) conjugate priors.In this thesis, [Paper A] and [Paper B] consider the problem of point object tracking where an object generates at most one measurement per scan. In [Paper A], it is shown that the trajectory MBM filter is the solution to the MOT problem for standard point object models with multi-Bernoulli birth. In addition, the multi-scan implementations of trajectory PMBM and MBM filters are presented. In [Paper B], a solution for recovering full trajectory information, via the calculation of the posterior of the set of trajectories from a sequence of multi-object filtering densities and the multi-object dynamic model, is presented. [Paper C] and [Paper D] consider the problem of ex- tended object tracking where an object may generate multiple measurements per scan. In [Paper C], the extended object PMBM filter for sets of objects is generalized to sets of trajectories. In [Paper D], a learning-based extended ob- ject tracking algorithm using a hierarchical truncated Gaussian measurement model tailored for automotive radar measurements is presented

Poisson Multi-Bernoulli Mixtures for Multiple Object Tracking

Multi-object tracking (MOT) refers to the process of estimating object trajectories of interest based on sequences of noisy sensor measurements obtained from multiple sources. Nowadays, MOT has found applications in numerous areas, including, e.g., air traffic control, maritime navigation, remote sensing, intelligent video surveillance, and more recently environmental perception, which is a key enabling technology in automated vehicles. This thesis studies Poisson multi-Bernoulli mixture (PMBM) conjugate priors for MOT. Finite Set Statistics provides an elegant Bayesian formulation of MOT based on random finite sets (RFSs), and a significant trend in RFSs-based MOT is the development of conjugate distributions in Bayesian probability theory, such as the PMBM distributions. Multi-object conjugate priors are of great interest as they provide families of distributions that are suitable to work with when seeking accurate approximations to the true posterior distributions. Many RFS-based MOT approaches are only concerned with multi-object filtering without attempting to estimate object trajectories. An appealing approach to building trajectories is by computing the multi-object densities on sets of trajectories. This leads to the development of many multi-object filters based on sets of trajectories, e.g., the trajectory PMBM filters. In this thesis, [Paper A] and [Paper B] consider the problem of point object tracking where an object generates at most one measurement per time scan. In [Paper A], a multi-scan implementation of trajectory PMBM filters via dual decomposition is presented. In [Paper B], a multi-trajectory particle smoother using backward simulation is presented for computing the multi-object posterior for sets of trajectories using a sequence of multi-object filtering densities and a multi-object dynamic model. [Paper C] and [Paper D] consider the problem of extended object tracking where an object may generate multiple measurements per time scan. In [Paper C], an extended object Poisson multi-Bernoulli (PMB) filter is presented, where the PMBM posterior density after the update step is approximated as a PMB. In [Paper D], a trajectory PMB filter for extended object tracking using belief propagation is presented, where the efficient PMB approximation is enabled by leveraging the PMBM conjugacy and the factor graph formulation