On Learning Heteroscedastic Noise Models within Differentiable Bayes Filters

1 INTRODUCTION . For many real-world systems that we would like to control , we can not directly observe the current state directly . However , in order to stabilize a system at a goal state or make it track a trajectory , we need to have access to state feedback . An observer provides an estimate of the current system state from sensor measurements . Recursive Bayesian Filtering is a probabilistic approach towards estimating a belief about the current state . The method relies on a process model that predicts how the system behaves over time and an observation model that generates the expected observations given the predicted state . While the approach itself is general and makes few assumptions , the challenge is to formulate the process and observation models and to estimate the noise in these models . Process and observation noise quantify how certain the filter is about either the prediction or the observations . This information is used to determine how much the predicted state is updated based on the observation . Deep neural networks are well suited for tasks that require finding patterns or extracting information from raw , high-dimensional input signals and compressing them into a more compact representation . They have therefore become the method of choice especially in perception problems . For many robotics tasks like modeling dynamics , planning or tracking however , it has been shown that combining prior knowledge in the form of analytical models and/or algorithmic structure with trainable network components leads to better performance and generalizability than trying to learn the complete tasks from scratch ( Kloss et al. , 2017 ; Karkus et al. , 2017 ; Jonschkowski et al. , 2018 ; Tamar et al. , 2016 ; Okada et al. , 2017 ; Jonschkowski & Brock , 2016 ; Haarnoja et al. , 2016 ; Karkus et al. , 2018 ) . Specifically , ( Jonschkowski & Brock , 2016 ; Haarnoja et al. , 2016 ; Jonschkowski et al. , 2018 ; Karkus et al. , 2018 ) have presented differentiable Bayesian Filtering algorithms . The authors focus on learning the observation and dynamics models end-to-end through the filters and demonstrate that the recursive filtering structure improves prediction results over using recurrent neural networks that were trained for the same task . In many robotic applications , it is possible to formulate the process and observation model based on first-order principles . However , finding appropriate values for the process and observation noise is often difficult and despite of much research on identification methods ( e.g . ( Bavdekar et al. , 2011 ; Valappil & Georgakis , 2000 ) ) they are often tuned manually . To reduce the tedious tuning effort , the noise models are typically assumed to be a Gaussian with zero mean and constant covariance . Many real systems can however be better modeled with heteroscedastic noise models , where the level of uncertainty depends on the state of the system and/or possible control inputs . Taking heterostochasticity into account has been demonstrated to improve filtering performance in many robotic tasks ( Bauza & Rodriguez , 2017 ; Kersting et al. , 2007 ) . In this work , we propose a method to learn heteroscedastic noise models from data by optimizing the prediction likelihood end-to-end through differentiable Bayesian Filters . In addition to differentiable Extended Kalman Filters and Particle Filters , which have been proposed in related work , we also propose two different versions of the Unscented Kalman Filter . In our experiments we focus on learning the noise models and therefore assume that observation and process models are known or at least pretrained . We evaluate the performance of the different filters and noise models on two different real-world robotic problems : ( i ) Visual Odometry for an driving car ( Haarnoja et al. , 2016 ; Jonschkowski et al. , 2018 ; Geiger et al. , 2012 ) which has simple smooth dynamics and a low-dimensional state , and ( ii ) Visual tracking of an object that is pushed by a robot ( Yu et al. , 2016 ; Kloss et al. , 2017 ) . Planar pushing has challenging , discontinuous dynamics and was shown to have a heteroscedastic noise distribution ( Bauza & Rodriguez , 2017 ) . Furthermore , the dimensionality of the state is double of the Visual Odometry task . Our experiments show that using heteroscedastic process noise models drastically improves the tracking performance of the Particle Filter and Unscented Filter variants and facilitated learning as compared to learning a constant process noise model . While learning the noise models can be beneficial for all filters , the tracking performance of the EKF turned out to be least sensitive to the noise models . In comparison to the process noise , learning the observation noise did not improve the results much for the two tasks we evaluated . 2 BACKGROUND : BAYESIAN FILTERING . Filtering refers to the problem of estimating the state x of a stochastic system at time step t given an initial believe x0 , a sequence of observations zt and control inputs ut . The aim is to compute p ( xt|x0 ... t−1 , u0 ... t , z0 ... t ) . To do so , we describe the system with a state space representation , that consists of two equations : The process model f describes how the state changes over time and the observation model h generates observations given the current state : xt = f ( xt−1 , ut−1 , qt ) zt = h ( xt , rt ) The random variables q and r are the process and observation noise and represent the stochasticity of the system . This model makes the Markov assumption , i.e . the current state only depends on the previous state , and the observation only depends on the current state . This assumption makes it possible to compute p ( xt|x0 ... t−1 , u0 ... t , z0 ... t ) recursively from p ( xt−1|x0 ... t−2 , u0 ... t−1 , z0 ... t−1 ) . In the following , we review the most common filtering algorithms . For more details , we refer to Thrun et al . ( 2005 ) . 2.1 KALMAN FILTER . The Kalman Filter ( Kalman , 1960 ) is a closed-form solution to the filtering problem for systems with linear process and observation model and Gaussian additive noise . xt = f ( xt−1 , ut−1 , qt ) = Axt−1 + But + qt zt = h ( xt , rt ) = Hxt + rt Given these assumptions and a Gaussian initial belief , the belief can be represented by the mean µ and covariance matrix Σ over the estimate . At each timestep , the filter predicts µ̂ and Σ̂ given the process model . The innovation it is the difference between the predicted and actual observation and is used to correct the prediction . The Kalman Gain K trades-off the process noise Q and the observation noise R to determine the magnitude of the update . Prediction Step : Update Step : µ̂t = Aµt−1 + But ( 1 ) Σ̂t = AΣt−1A T + Qt ( 2 ) St = HΣ̂tH T + Rt ( 3 ) Kt = Σ̂tH TS−1t ( 4 ) it = zt −Hµ̂t ( 5 ) µt = µ̂t + Ktit ( 6 ) Σt = ( In −KtH ) Σ̂t ( 7 ) 2.2 EXTENDED KALMAN FILTER ( EKF ) . The EKF ( Sorenson , 1985 ) extends the Kalman Filter to systems with non-linear process and observation models . It uses the non-linear models for predicting µ̂ and the corresponding observations ẑ ( Equations 1 , 5 ) . For computing the prediction and update of Σ and K , these models are linearized around the current mean of the state and the Jacobians F|µt and H|µt replace A and H in Equations 2 - 4 and 7 . This first-order approximation can be problematic for systems with strong non-linearity , as it does not take the uncertainty about the mean into account ( Van Der Merwe , 2004 ) . 2.3 UNSCENTED KALMAN FILTER ( UKF ) . The UKF ( Simon J. Julier , 1997 ; Van Der Merwe , 2004 ) was proposed to address the aforementioned problem of the EKF . Its core idea is to represent a Gaussian random variable by a set of specifically chosen points in state space , the so called sigma points X . If this random variable undergoes a nonlinear transformation , we can calculate its new statistics from the transformed sigma points . This method is called the Unscented Transform ( Simon J. Julier , 1997 ) . For example , in the prediction step of the UKF , the non-linear transform is the process model ( Equation 10 ) and the new mean and covariance are computed in Equations 11 and 12 X 0 = µ X i = µ± ( √ ( n+ λ ) Σ ) i ∀i ∈ { 1 ... n } ( 8 ) w0 = λ λ+ n wi = 0.5 λ+ n ∀i ∈ { 1 ... 2n } ( 9 ) X̂t = f ( Xt−1 , ut ) ( 10 ) µ̂t = ∑ i wiX̂ it ( 11 ) Σ̂t = ∑ i wi ( X̂ it − µ̂t ) ( X̂ it − µ̂t ) T + Qt ( 12 ) By applying the non-linear prediction step separately to each sigma point and then fitting a new Gaussian to the transformed points ( Equations 11 , 12 ) , the UKF conveys the non- linear transformation of the covariance more faithfully than the EKF and is thus better suited for strongly non-linear problems ( Thrun et al. , 2005 ) . The parameter λ controls the spread of the sigma points and how strongly the original mean X 0 is weighted in comparison to the other sigma points . In practice , we found λ difficult to tune since placing the sigma points too far from the mean increases prediction uncertainty and can even destabilize the filter . Simon J. Julier ( 1997 ) suggested to chose λ such that λ + n = 3 . This however results in negative values of λ if n > 3 , for which the estimated covariance matrix is not guaranteed to be positive semidefinite anymore ( Simon J. Julier , 1997 ) . In addition , X 0 , which represents the original mean , is weighted negatively in this case , which seems counterintuitive and can cause divergence of the estimated mean . 2.4 MONTE CARLO UNSCENTED KALMAN FILTER ( MCUKF ) . The UKF represents the belief over the state with as few sigma points as possible . However , as described above , finding the correct scaling parameter λ can be difficult , especially if the state is high dimensional . Instead of relying on the unscented transform to calculate the mean and covariance of the state at the next timestep , we can also resort to Monte Carlo methods , as proposed by Wüthrich et al . ( 2016 ) . In practice , this means that we replace the carefully constructed sigma points and their weights in Equations 8 , 9 with samples from the current estimated state distribution , which all have uniform weights . The rest of the UKF algorithm stays the same , but more samples are necessary to represent the distribution accurately . 2.5 PARTICLE FILTER ( PF ) . In contrast to the different variants of the Kalman Filter explained before , the Particle Filter ( Gordon et al. , 1993 ) does not assume a parametric representation of the state distribution . Instead , it represents the state with a set of particles . The particle-based representation allows the filter to track multiple hypotheses about the state at the same time and makes it a popular choice for tasks like localization or visual object tracking ( Thrun et al. , 2005 ) . An initial set of particles X0 is drawn from some prior belief and initialized with uniform weights . At each recursion step , new particles are generated by sampling process noise and applying the process model to the previous particles : Xt = f ( Xt−1 , ut , qt ) ( 13 ) For each observation zt , we then evaluate the likelihood p ( zt|xit ) of a particle xit having generated this observation . Based on this , the weight wi of each particle is updated : xi : wit = w i t−1p ( zt|xit ) . A common problem of this filter is particle deprivation : Over time , many particles will receive a very low likelihood p ( zt|xit ) , and the state would be represented by too few particles with high weights . To prevent this , the particle filter algorithm uses resampling , where a new set of particles with uniform weights is drawn ( with replacement ) from the old set , according to the weights . This step focuses the particle set on regions of high likelihood and is usually applied after each timestep .

Summary: This paper presents a method to learn and use state and observation dependent noise in traditional Bayesian filtering algorithms.  For observation noise, the approach consists of constructing a neural network model which takes as input the raw observation data and produces a compact representation and an associated (diagonal) covariance.  Similarly for state process noise, a network predicts the (diagonal) covariance of the temporal process given the current state.