GP-SUM. Gaussian Processes Filtering of non-Gaussian Beliefs

09/23/2017 ∙ by Maria Bauza, et al. ∙ 0

This work centers on the problem of stochastic filtering for systems that yield complex beliefs. The main contribution is GP-SUM, a filtering algorithm for dynamic systems expressed as Gaussian Processes (GP), that does not rely on linearizations or Gaussian approximations of the belief. The algorithm can be seen as a combination of a sampling-based filter and a probabilistic Bayes filter. GP-SUM operates by sampling the state distribution and propagating each sample through the dynamic system and observation models. Both, the sampling of the state and its propagation, are made possible by relying on the GP form of the system. In practice, the belief has the form of a weighted sum of Gaussians. We evaluate the performance of the algorithm with favorable comparisons against multiple versions of GP-Bayes filters on a standard synthetic problem. We also illustrate its practical use in a pushing task, and demonstrate that GP-SUM can predict heteroscedasticity, i.e., different amounts of uncertainty, and multi-modality when naturally occurring in pushing.



There are no comments yet.


page 1

page 2

page 3

page 4

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Robotics and uncertainty come hand in hand. One of the defining challenges of robotics research is to design uncertainty-resilient behavior to overcome noise in sensing, actuation and/or dynamics. This paper studies the problems of simulation and filtering in systems with stochastic dynamics and noisy observations, with a particular interest in cases where the state belief cannot be realistically approximated by a single Gaussian distribution.

Complex multimodal beliefs naturally arise in manipulation tasks where state or action noise can make the difference between contact/separation or between sticking/sliding. For example, the ordinary task of push-grasping a cup of coffee into your hand in Figure 1 illustrates the naturally occurring multimodality. Dogar and Srinivasa [1] used the observation that a push-grasped object tends to cluster into two clearly distinct outcomes—inside and outside the hand—to plan robust grasp strategies. Multimodality and complex belief distributions have been experimentally observed in a variety of manipulation actions such as planar pushing [2, 3], ground impacts [4], and bin-picking [5].

Fig. 1: Push-grasp of a coffee cup. A small change in the initial contact between the hand and the cup, or a small change in the hand’s motion can produce a very distinct—multimodal—outcome either exposing (top) or hiding (bottom) the cup’s handle from the hand’s palm.

The main contribution of this paper is a new algorithm GP-SUM to track complex state beliefs through a dynamic system expressed as a Gaussian process (GP) without the need to either linearize the transition or observation models or relying on unimodal Gaussian approximations of the belief.

GP-SUM operates by sampling the state distribution, so it can be viewed as a sampling-based filter. It also maintains the basic structure of a Bayes filter by exploiting the GP form of the dynamic and observation models to provide a probabilistic sound interpretation of each sample, so it can also be viewed as a GP-Bayes filter.

We compare GP-SUM’s performance to other existing GP-filtering algorithms like GP-UKF, GP-ADF and GP-Particle Filter in a standard benchmark [6, 7]. GP-SUM shows better filtering results both after a single and multiple filtering steps with a variety of metrics, and requires significantly less samples than standard particle filtering techniques.

We demonstrate the use of GP-SUM to predict the expected distribution of outcomes when pushing an object. Prior experimental work by Bauza and Rodriguez [3] shows that planar pushing produces heteroscedastic and multimodal behavior, i.e., some actions are more deterministic than others and distributions can break down into components. GP-SUM successfully recovers both when applied to a GP learned model of planar pushing. We compare the results against trajectories simulated with less efficient Monte Carlo-based simulations.

Both actions and sensed information determine the shape of the belief distribution. This paper provides an efficient algorithm for tracking distributions tailored to the case where the observation and transition models are expressed as GPs.

The paper is structured as follows:

  • Introduction (done),

  • related work,

  • background of Bayes filtering and Gaussian Processes,

  • main algorithm, assumptions and time complexity,

  • evaluation and benchmarking,

  • discussion of GP-SUM’s main limitations and possible improvements.

Ii Related Work

Gaussian processes (GPs) have proved to be a powerful tool to model the dynamics of complex systems [8, 9, 5], and have been applied to different contexts of robotics including planning and control [10, 11, 12], system identification [8, 13, 14], or filtering [15, 6, 7]. In this work, we study the problem of propagating and filtering the state of a system by providing accurate distributions of the state. When the models for the dynamics and measurements are learned through GP regression, the filtering algorithms are referred as GP-Bayes filters. Among these algorithms, the most frequently considered are GP-EKF [15], GP-UKF [15] and GP-ADF [6], with GP-ADF regarded as the state-of-the-art.

All these GP-filters rely on the assumption that the state distribution is well captured by a single Gaussian and depend on several approximations to maintain that Gaussianity. GP-EKF is based on the extended Kalman filter (EKF) and linearizes the GP models to guarantee that the final distributions are Gaussian. GP-UKF is based on the unscented Kalman filter (UKF) and provides a Gaussian distribution for the state using an appropriate set of sigma points that captures the moments of the state. Finally, GP-ADF computes the first two moments of the state distribution by exploiting the structure of GPs and thus returns a Gaussian distribution for the state.

GP-SUM instead is based on sampling from the state distributions and using Gaussian mixtures to represent these probabilities. This links our algorithm to the classical problem of particle filtering where each element of the mixture can be seen as a sample with an associated weight and a Gaussian. As a result, GP-SUM can be understood as a sampling algorithm that benefits from the parametric structure of Gaussians to simplify its computations and represent the state through weighted Gaussians. Another sampling algorithm for GP-filtering is provided in

[15] where they propose the GP-PF algorithm based on the classical particle filter (PF). However, when compared to GP-UKF or GP-EKF, GP-PF is less reliable and more prone to give inconsistent results.

In the broader context of Bayes filtering where the dynamics and observation models are known, multiple algorithms have been proposed to recover non-Gaussian state distributions. For instance, we can find some resemblances between GP-SUM and the algorithms Gaussian Mixture Filter (GMF)  [16], Gaussian Sum Filter (GSF) [17], and Gaussian Sum Particle Filtering (GSPM) [17]

; all using different techniques to propagate the state distributions as sum of Gaussians. GPM considers a Gaussian mixture model to represent the state distribution, but the covariance of each Gaussian is equal and comes from sampling the previous state distribution and computing the covariance of the resulting samples; GP-SUM instead recovers the covariance of the mixture from the dynamics of the system. GSF is as a set of weighted EKF running in parallel. As a consequence it requires to linearize the system models while GP-SUM does not. Finally GSPM, which has proven to be superior to GSF, is based on the sequential importance sampling filter (SIS)  

[17]. GSPM samples from the importance function which is defined as the likelihood of a state given an observation z, . GP-SUM instead does not need to learn this extra mapping, , to effectively propagate the state distributions.

Other algorithms relevant to GP-SUM are the multi-hypothesis tracking filter (MHT) [18] and the manifold particle filter (MPF) [19]

. MHT is designed to solve a data association problem for multiple target tracking by representing the joint distribution of the targets as a Gaussian mixture. Instead, MPF is a sample-based algorithm that has been recently applied to several manipulation tasks with complex dynamics. MPF exploits the contact manifolds of the system by collapsing the distribution defined by the samples into that manifold.

An advantage of GP-SUM is that it can be viewed as both a sampling technique and a parametric filter. Therefore most of the techniques employed for particle filtering are applicable. Similarly, GP-SUM can also be adapted to special types of GPs such as heteroscedastic or sparse GPs. For instance, GP-SUM can be easily extended to the case where sparse spectrum Gaussian processes (SSGPs) are considered by using the work from Pan et al. [7]. This implies that GP-SUM can be made significantly faster by using sparse GPs for the dynamics and observation models of the system.

Iii Background of Gaussian process filtering

This work focuses on the classical problem of Bayes filtering where the dynamics and observation models of the system are learned through Gaussian process regression. In this section, we introduce the reader to the concepts of Bayes filtering and Gaussian processes.

Iii-a Bayes filters

The goal of a Bayes filter is to track the state of the system, , in a probabilistic setting. At time , we consider that an action is applied to the system making its state evolve from to , and an observation of the new state, , is obtained. As a result, a Bayes filter computes the state distribution, , conditioned on the history of actions and observations obtained: . This probability is often referred as the belief of the state at time .

In general, a Bayes filter is composed of two steps: the prediction update and the measurement or filter update following the terminology from [20].

Prediction update. Given a model of the system dynamics, , the prediction update computes the prediction belief, , as:


where is the belief of the system before applying the action . Thus the prediction belief can be understood as the pre-observation distribution of the state at time while the belief would be the post-observation distribution. In general, the integral in (1) can not be solved analytically and different approximations must be used to simplify its computation. Among these simplifications, it is common to linearize the dynamics of the system as it is classically done in the EKF or to directly assume that the prediction belief is Gaussian distributed [20].

Measurement update. Given a new measurement of the state, , the belief at time can be obtained by filtering the prediction belief. The belief is recovered using the observation model of the system and applying the Bayes’ rule:


This expression usually can not be solved in a closed form and several approximations are required to estimate the new belief. Linearizing the observation model or assuming Gaussianity are again common approaches


Note that the belief at time can be directly expressed in a recursive manner using the previous belief, and the transition and observation models:


We will show in Section IV that the same idea of recursion can be applied to the prediction belief, which is a key element for our algorithm GP-SUM.

In general, the dynamics and observation models are considered known and given by parametric descriptions. However, in real systems it is often the case that these models are unknown and it is convenient to learn them using non-parametric approaches such as Gaussian processes. This proves specially beneficial when the actual models are complex and parametric approaches do not provide a fair representation of the system behavior [11, 3].

Iii-B Gaussian processes

Gaussian processes (GPs) provide a flexible and non-parametric framework for function approximation and regression [21]

. In this paper, GPs are considered when modeling the dynamics of the system as well as its observation model. There are several advantages in using GPs over traditional parametric models. First, GPs can learn high fidelity models from noisy data as well as estimate the intrinsic noise in the system. Moreover, GPs can also quantify how certain are their predictions given the available data hence measuring the quality of the regression. For each point in the space, GPs provide the value of the expected output together with its variance. In practice, for each input considered, a GP returns a Gaussian distribution over the output space.

In classical GPs [21], the noise in the output is assumed to be Gaussian and constant over the input:


where is the latent or unobserved function that we want to regress, is a noisy observation of this function at the input , and represents zero-mean Gaussian noise with variance .

The assumption of constant Gaussian noise together with a GP prior on the latent function makes analytically inference possible for GPs (III-B). In practice, to learn a GP model over you only need a set of training points, , and a kernel function, . Given a new input , a trained GP assigns a Gaussian distribution to the output that can be expressed as:


where is a matrix that evaluates the kernel in the training points, ,

is a vector with

and is the value of the kernel at , . Finally, represents the vector of observations from the training set, and

is the set of hyperparameters including

and the kernel parameters that are optimized during the training process.

A notable property of GPs is that the expected variance of the output comes from the addition of two variances: and (III-B). The first one, , is constant and represents the overall noise of the data. The second one, , depends on the input and is only related to the regression error.

In this work we consider the ARD-SE kernel  [21] which provides smooth representations of during GP regression and is the most common kernel employed in the literature of GPs. However, it is possible to extend our algorithm to other kernel functions as it is done in [7].

Iii-C Heteroscedastic Gaussian processes

Assuming that the noise of the process is constant over the input space is sometimes too restricting. Allowing some regions of the input to be more noisy than others is specially beneficial for those systems with converging and diverging dynamics. Algorithms where GPs incorporate input-dependent noise have proven useful in different context such as mobile robot perception [22], volatility forecasting [23] and robotic manipulation [3]. In Section V, we explore the benefits of combining GP-SUM with input-dependent GPs to characterize the long term dynamics of planar pushing.

The extensions of GPs that incorporate input-dependent noise are often referred as Heteroscedastic Gaussian processes (HGPs). This implies that they can regress both the mean and the variance of the process for any element of the input space. Then, the main conceptual difference between GP and HGP regression is that for HGPs observations are assumed to be drawn from:


where explicitly depends on compared to (4) where

is a random variable independent of


GP-SUM can be extended to systems where the models for the transitions and measurements are given by heteroscedastic GPs. This is exemplified in Section V during the study of planar pushing where, depending on the type of push, the motion of the object is more or less predictable [3].

Iv GP-SUM Bayes filter

In this section we present GP-SUM, discuss its main assumptions, and describe its computational complexity. Given that GP-SUM is a GP-Bayes filter, our main assumption is that both the dynamics and the measurement models are represented by GPs. This implies that for any state and action on the system the probabilities and are available and are Gaussian.

To keep track of complex beliefs, GP-SUM does not approximate them by single Gaussians, but considers the weaker assumption that they are well approximated by Gaussians mixtures. Given this assumption, in Section IV-A we exploit that the transition and observation models are GPs to correctly propagate the prediction belief, i.e. the pre-observation state distribution. In Section IV-B we obtain a close-form solution for the belief expressed as a Gaussian mixture.

Iv-a Updating the prediction belief

The main idea behind GP-SUM is described in Algorithm 1 and derived below. Consider (1) and (3), then the belief at time in terms of the prediction belief is:


If the prediction belief at time is well approximated by a finite Gaussian mixture, we can write:


where is the number of components of the Gaussian mixture and is the weight associated with the i-th Gaussian of the mixture .

Then we can compute the prediction belief at time combining (1) and (7) as:


Given the previous observation and the action , the prediction belief at time can be recursively computed using the prediction belief at time together with the transition and observation models. If has the form of a Gaussian mixture, then we can take samples, , and approximate (9) by:


Because the dynamics model is a GP, is the Gaussian , and is a constant value. As a result, we can take:


and express the updated prediction belief again as a Gaussian mixture:


In the ideal case where tends to infinity for all , the Gaussian mixture approximation for the prediction belief converges to the real distribution and thus the propagation over time of the prediction beliefs will remain correct. This property of GP-SUM contrasts with previous GP-Bayes filters where the prediction belief is approximated as a single Gaussian. In those cases, errors from previous approximations inevitably accumulate over time.

Note that the weights in (11) are directly related to the likelihood of the observations. As in most sample-based algorithms, if the weights are too small before normalization, it becomes a good strategy to re-sample or modify the number of samples considered. In Section V we address this issue by re-sampling again from the distributions while keeping the number of samples constant.

  SUM-GP(, , , ):
   = sample(, )
  for   do
  end for
   = normalizeweights()
Algorithm 1 Prediction belief recursion

Iv-B Recovering the belief from the prediction belief

After computing the prediction belief, we can use the observation to compute the belief as another Gaussian mixture using (7):


Note that if could be normalized and expressed as a Gaussian distribution, then the belief at time would directly be a Gaussian mixture. In most cases, however, is not proportional to a Gaussian. For those case, we find different approximations in the literature (see Algorithm 2). For instance, the algorithm GP-EKF [15] linearizes the observation model to express the previous distribution as a Gaussian.

  beliefcomputation(, , ):
  for   do
      Gaussianapprox( )
  end for
Algorithm 2 Belief recovery

In this work, we exploit the technique proposed by Deisenroth et al. [6] as it preserves the first two moments of and has proven to outperform GP-EKF [6]. This approximation assumes that and are both Gaussians. Note that this is an approximation, and that is only true when there is a linear relation between and . Using this assumption and that is a GP, can be approximated as a Gaussian by analytically computing its first two moments [6]. As a result, we recover the belief as a Gaussian mixture.

It is important to note that this approximation is only necessary to recover the belief, but does not incur in iterative filtering error. GP-SUM directly keeps track of the prediction belief, for which the approximation is not required.

Iv-C Computational complexity

The computational complexity of GP-SUM depends on the number of Gaussians considered at each step. For simplicity, we will now assume that at each time step the number of components is constant, . Note that the number of samples taken from the prediction belief corresponds to the number of components of the next distribution. Propagating the prediction belief one step then requires taking samples from the previous prediction belief and evaluate times the dynamics and measurement models. The cost of sampling once a Gaussian mixture can be considered constant, , while evaluating each model implies computing the output of a GP and takes computations where is the size of data used to train the GPs [21]. Therefore the overall cost of propagating the prediction belief is where is the largest size of the training sets considered. Approximating the belief does not represent an increase in complexity as it also implies operations [6].

Consequently GP-SUM’s time complexity increases linearly with the size of the Gaussian mixture while providing a more realistic approximation of the belief and better filtering results (see Section V). To further reduce the computational cost of GP-SUM, sparse GPs can be considered. For instance, combining GP-SUM with SSGPs [7] makes the evaluation of the GP models decrease from to with .

V Results

We evaluate the performance of our algorithm in two different systems. The first one considers a standard 1D benchmark for nonlinear state space models [6, 24] where our algorithm proves it can outperform previous GP-Bayes filters111The implementations of GP-ADF and GP-UKF are based on [6] and can be found at The second case studies how uncertainty propagates in a real robotic system. We first learn the dynamics of planar pushing from real data with GP regression and then use GP-SUM to propagate the system uncertainty overtime to capture the expected distribution of the object position.

Fig. 2: Non-linear dynamics and measurement model of the 1D system presented in Section V-A. The dynamics are specially non-linear around zero which will potentially lead to unstable behavior and multi-modal state distributions.
Fig. 3: Graphical example of how GP-SUM, GP-ADF and Gauss GP-SUM propagate complex state distributions. In this case, the prior has making the dynamics extremely nonlinear around it. As a result, the prediction belief and the belief become multimodal in the first time steps. Compared to GP-ADF, GP-SUM can express properly these complex distributions and its predictions are more accurate. The belief at time for GP-SUM clearly points out that there are three narrow regions for the location of the true state while GP-ADF only considers a Gaussian wide enough to enclose them all.

V-a Synthetic task: algorithm evaluation and comparison

We compare GP-SUM with other existing GP-Bayes filters using the following 1D dynamical system:


and observation model:


illustrated in Figure 2

. The GP models for prediction and measurement are trained using 1000 samples uniformly distributed around the interval

. GP-SUM uses the same number of Gaussian components over all time steps, . The prior distribution of is Gaussian with variance and mean . is modified 200 times to assess the filters in multiple scenarios and becomes specially interesting around where the dynamics are highly nonlinear. For each value of , the filters take 10 time steps. This procedure is repeated 300 times to average the performance of GP-SUM, GP-ADF, GP-UKF, and GP-PF, described in Section II. For GP-PF, the number of particles is the same as GP-SUM components, .

The error in the final state of the system is evaluated using 3 metrics. The most relevant is the negative log-likelihood, NLL, which measures the likelihood of the true state according to the predicted belief. We also report the root-mean-square error, RMSE, even though it only uses the mean of the belief instead of its whole distribution. Similarly, the Mahalanobis distance, Maha, only considers the first two moments of the belief so we have to approximate the belief from GP-SUM by a Gaussian. For the GP-PF we only compute the RMSE given that particle filters do not provide close-form distributions. In all the metrics proposed, low values imply better performance.

NLL 0.49 0.17 95.03 97.02 -0.55 0.34 -
Maha 0.69 0.06 2.80 0.72 0.67 0.04 -
RMSE 2.18 0.39 34.48 23.14 2.18 0.38 2.27 0.35
TABLE II: Comparison between GP-filters after 10 time steps
NLL 9.58 15.68 1517.17 7600.82 -0.24 0.11 -
Maha 0.99 0.31 8.25 3.82 0.77 0.06 -
RMSE 2.27 0.16 12.96 16.68 0.19 0.02 N/A
TABLE I: Comparison between GP-filters after 1 time step.
Fig. 4: Evolution over time of the negative log-likelihood, NLL. In the first time steps the shape of the real belief is more likely to be non-Gaussian and this explains why GP-SUM performs better than both GP-ADF and the Gaussian approximation of GP-SUM, Gauss GP-SUM. As time evolves, GP-SUM and Gauss GP-SUM converge meaning that the shape of the real belief tends to become Gaussian. On the other hand, GP-ADF’s performance worsens over time because for the cases where the dynamics are highly nonlinear its predicted variance tends to increase making the likelihood of the true state lower with time.

From Table II and Table II, it is clear that GP-SUM outperforms the other algorithms in all the metrics proposed and can be considered more stable as it obtains the lowest variance in most of the metrics. In the first time step, GP-PF is already outperformed by GP-SUM and GP-ADF, and after a few more steps in time, particle starvation becomes a major issue for GP-PF as the likelihood of the observations becomes extremely low. For this reason, we did not report a RMSE value for the GP-PF after 10 time steps. GP-UKF performance is clearly surpassed by GP-SUM and GP-ADF after 1 and 10 time steps.

In Figure 3 we compare the true state distributions (computed numerically) to the distributions obtained by GP-ADF, GP-SUM, and a simplified version of GP-SUM, Gauss GP-SUM, that takes a Gaussian approximation of GP-SUM at each time step. It becomes clear that by allowing non-Gaussian beliefs GP-SUM can assign higher likelihood to the actual state while better approximating the true belief. Instead, GP-ADF can only assign a single Gaussian wide enough to cover all the high density regions.

In Figure 4 we study the temporal evolution of the Negative Log-Likelihood (NLL) metric for GP-ADF, GP-SUM and Gauss GP-SUM. As the number of steps increases, GP-SUM and Gauss GP-SUM tend to coincide because GP-SUM tends to become more confident on the location of the true state and its belief becomes more Gaussian. Figure 4 also shows that GP-ADF worsens its performance over time. This is due to those cases where the dynamics are highly non-linear, i.e. around zero, and the variance of GP-ADF increases over time. As a result, at the cost of larger computational expense, the expressiveness of its distributions and the lack of heavy assumptions makes GP-SUM a good fit for those systems where multimodality and complex behaviors can not be neglected.

Fig. 5: Figure taken from [3]. The top image shows the real system used to collect data from pushing a square object. The image below shows three different planar pushes whose outcome (repeated 100 times) yields very different distributions: (left) convergent, (center) divergent, and (right) multi-modal. The trajectories in green represent the center of mass of the object while the red ellipse approximates the final distribution of the object pose.
Fig. 6: Figure taken from [3] that represents the input and output space for the dynamics of the planar push system. (a) The input considers the point of contact between the object and the pusher , the angle of push , and the pusher’s velocity . (b) The output is parameterized as the displacement of the object in a reference frame aligned with the push direction, . The dynamics of the system are learned using a HGP that maps the type of push, , to the motion of the object after a short time step.
Fig. 7: Uncertainty propagation during pushing. Figure (a) represents two different pushes and several trajectories obtained from those pushes by propagating the HGP dynamics in time. Figure (b) shows the evolution of the state distribution provided by GP-SUM over time. For simplicity, until the last time step we only show the mean of each Gaussian component where different colors represent different time steps. Both pushes present ring-shaped distributions and multimodality: the one on top presents more symmetric distributions while the one on the bottom is asymmetric because the object is more likely to remain to the left of the pusher’s direction. The modes disconnected from the main distribution are due to the lose of contact with the pusher. This can be due to the object orientation which has been omitted in the plots to enable a clear visualization of the results.
Fig. 8: Comparison of two pushes with different levels of noise. One push is clearly noisier than the other as it generates a much wider distribution of the object position from the beginning. This is in part possible because the dynamics, given by an HGP, consider input-dependent noise.

V-B Real task: propagating uncertainty in pushing

Planar pushing is an underdetermined and sometimes undecidable physical interaction [25], except when making sufficient assumptions and simplifications [26, 27]. It has also been shown experimentally that uncertainty in frictional interaction yields stochastic pushing behavior [3, 2]. Moreover, the type of push has a strong influence in the amount of expected uncertainty, i.e., the level of ”noise” in the pushing dynamics is action dependent, a.k.a., heteroscedastic. This can be observed in Figure 5 where three different pushes repeated multiple times lead to very different distributions for the object position including multimodality.

A pushing controller could benefit from a model of the heteroscedasticity in pushing dynamics by preferring those pushes that lead to lower uncertainty. To this end, it is necessary an heteroscedastic pushing model for the dynamics together with an algorithm that reliably and efficiently propagates uncertainty over time. In this work, we use HGPs to model the dynamics of pushing as done by Bauza and Rodriguez [3], and GP-SUM to propagate the uncertainty of the system.

As illustrated in Figure 6, we model planar pushing as a HGP that takes as inputs the contact point between the pusher and the object, the pusher’s velocity, and its direction. The output of the dynamics is the displacement of the object relative to the pusher’s motion. We train the HGP model with real data from the MIT pushing dataset [2]. In this setting, a cylindrical rod attached to an industrial arm acts as the pusher of a square object made of stainless-steel, where the interaction between the robot and the object is approximated by a single contact point.

Because we are only concerned about simulating the propagation of uncertainty over time, we do not consider a measurement model. As a result, when using GP-SUM the prediction belief and the belief coincide and all the components in the Gaussian mixtures have the same weights.

Given that the distribution of the object position tends to become non-Gaussian overtime, GP-SUM can obtain more accurate results than other algorithms. Figure 7 shows the resemblance between sampled trajectories from propagating the dynamics in time in a Monte-Carlo fashion and the distributions obtained by GP-SUM. It also becomes clear that the shape of the distributions recovered by GP-SUM are ring-shaped and even multimodal which can not be captured by standard GP-Bayes filters that predict single Gaussian distributions.

Being able to propagate the uncertainty of the object position over time exposes interesting properties of the planar push system. For instance in Figure 8 we observe two different pushes and how the belief for the object position evolves. It becomes clear that one of the pushes leads to more noisy distributions. Being able to recover these behaviors is specially useful when deciding what are the best pushes to exert. If our goal is to push an object to a specific region of the space, then it would be advantageous to consider those pushes that lead to narrower (low-variance) distributions and avoid those that involve multimodal outcomes because they are often harder to control.

Vi Discussion and Future work

GP-Bayes filters are a powerful tool to model and track systems with complex and noisy dynamics. Most approaches rely on the assumption that the belief is Gaussian or can be iteratively approximated by a Gaussian. The Gaussian assumption is an effective simplification. It enables filtering with high frequency updates or in high dimensional systems. It is most reasonable in systems where the local dynamics are simple, i.e., linearizable, and when accurate observation models are readily available to continuously correct for complex or un-modelled dynamics.

In this paper we look at situations where the Gaussian belief is less reasonable. That is the case of contact behavior with non-smooth local dynamics due to sudden changes in stick/slip or contact/separation, and is the case in stochastic simulation where, without the benefit of sensor feedback, uncertainty distributions naturally grow over time. We propose the GP-SUM algorithm which considers the use of Gaussian mixtures to represent complex state distributions.

Our approach is sample-based in nature, but has the advantage of using a minimal number of assumptions compared to other GP-filters based on single Gaussian distributions or the linearization of the GP-models. Since GP-SUM preserves the probabilistic nature of a Bayes filter, it also makes a more effective use of sampling than particle filters.

When considering GP-SUM, several aspects must be taken into account:

Number of samples. Choosing the appropriate number of samples determines the number of Gaussians in the prediction belief and hence its expressiveness. Adjusting the number of Gaussian over time might be beneficial in order to properly cover the state space. Similarly, high-dimensional states might require higher values of to ensure a proper sampling of the prediction belief. Because of the sample-based nature of GP-SUM, many techniques from sample-based algorithms can be effectively applied such as resampling or adding randomly generated components to avoid particle deprivation.

Likelihood of the observations. There is a direct relation between the weights of the beliefs and the likelihood of the observations. We can exploit this relationship to detect when the weight of the samples degenerates and correct it by re-sampling or modifying the number of samples.

Computational cost. Unlike non-sampling GP-filters, the cost of GP-SUM scales linearly with the number of samples. Nevertheless, for non-linear systems we showed that our algorithm can recover the true state distributions more accurately and thus obtain better results when compared to faster algorithms such as GP-ADF, GP-UKF or GP-PF.

GP extensions. The structure of GP-SUM is not restricted to classical GPs for the dynamics and observation models. Other types of GPs such as HGPs or sparse GPs can be considered. For instance, combining GP-SUM with SSGPs [7] would make the computations more efficient.

Future research will focus on combining GP-SUM with planning and control techniques. Being able to detect multimodality or noisy actions can help to better navigate the complex and noisy dynamics of the system and reduce the final uncertainty in the state distribution.


  • Dogar and Srinivasa [2010] M. R. Dogar and S. S. Srinivasa, “Push-Grasping with Dexterous Hands: Mechanics and a Method,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2010, pp. 2123–2130.
  • Yu et al. [2016] K.-T. Yu, M. Bauza, N. Fazeli, and A. Rodriguez, “More than a Million Ways to be Pushed. A High-Fidelity Experimental Data Set of Planar Pushing,” in IROS, 2016.
  • Bauza and Rodriguez [2017] M. Bauza and A. Rodriguez, “A probabilistic data-driven model for planar pushing,” 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 3008–3015, 2017.
  • Fazeli et al. [2017] N. Fazeli, E. Donlon, E. Drumwright, and A. Rodriguez, “Empirical Evaluation of Common Impact Models on a Planar Impact Task,” in IEEE International Conference on Robotics and Automation (ICRA), 2017.
  • Paolini et al. [2014] R. Paolini, A. Rodriguez, S. S. Srinivasa, and M. T. Mason, “A Data-Driven Statistical Framework for Post-Grasp Manipulation,” IJRR, vol. 33, no. 4, 2014.
  • Deisenroth et al. [2009] M. Deisenroth, M. Huber, and U. Hanebeck, “Analytic moment-based gaussian process filtering,” in ICML, 2009, pp. 225–232.
  • Pan et al. [2017] Y. Pan, X. Yan, E. A. Theodorou, and B. Boots, “Prediction under uncertainty in sparse spectrum gaussian processes with applications to filtering and control,” in ICML, 2017, pp. 2760–2768.
  • Kocijan et al. [2005] J. Kocijan, A. Girard, B. Banko, and R. Murray-Smith, “Dynamic systems identification with gaussian processes,” Mathematical and Computer Modelling of Dynamical Systems, 2005.
  • Nguyen-Tuong et al. [2009] D. Nguyen-Tuong, M. Seeger, and J. Peters, “Model learning with local gaussian process regression,” Advanced Robotics, vol. 23, no. 15, pp. 2015–2034, 2009.
  • Murray-Smith and Sbarbaro [2002] R. Murray-Smith and D. Sbarbaro, “Nonlinear adaptive control using nonparametric gaussian process prior models,” IFAC Proceedings Volumes, vol. 35, no. 1, pp. 325–330, 2002.
  • Deisenroth et al. [2012]

    M. Deisenroth, C. Rasmussen, and D. Fox, “Learning to control a low-cost manipulator using data-efficient reinforcement learning,” in

    Robotics: Science and Systems, vol. 7, 2012, pp. 57–64.
  • Mukadam et al. [2016] M. Mukadam, X. Yan, and B. Boots, “Gaussian process motion planning,” in 2016 IEEE International Conference on Robotics and Automation (ICRA).    IEEE, 2016, pp. 9–15.
  • Gregorčič and Lightbody [2008] G. Gregorčič and G. Lightbody, “Nonlinear system identification: From multiple-model networks to gaussian processes,”

    Engineering Applications of Artificial Intelligence

    , 2008.
  • Ažman and Kocijan [2011] K. Ažman and J. Kocijan, “Dynamical systems identification using gaussian process models with incorporated local models,” Engineering Applications of Artificial Intelligence, 2011.
  • Ko and Fox [2009] J. Ko and D. Fox, “Gp-bayesfilters: Bayesian filtering using gaussian process prediction and observation models,” Autonomous Robots, vol. 27, no. 1, pp. 75–90, 2009.
  • Stordal et al. [2011] A. S. Stordal, H. A. Karlsen, G. Nævdal, H. J. Skaug, and B. Vallès, “Bridging the ensemble kalman filter and particle filters: the adaptive gaussian mixture filter,” Computational Geosciences, vol. 15, no. 2, pp. 293–305, 2011.
  • Kotecha and Djuric [2003] J. Kotecha and P. Djuric, “Gaussian sum particle filtering,” IEEE Transactions on signal processing, 2003.
  • Blackman [2004] S. S. Blackman, “Multiple hypothesis tracking for multiple target tracking,” IEEE Aerospace and Electronic Systems Magazine, vol. 19, no. 1, pp. 5–18, Jan 2004.
  • Koval et al. [2017] M. C. Koval, M. Klingensmith, S. S. Srinivasa, N. S. Pollard, and M. Kaess, “The manifold particle filter for state estimation on high-dimensional implicit manifolds,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), May 2017, pp. 4673–4680.
  • Thrun et al. [2005] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics (Intelligent Robotics and Autonomous Agents).    The MIT Press, 2005.
  • Rasmussen and Williams [2006] C. Rasmussen and C. Williams,

    Gaussian Processes for Machine Learning

    .    MIT Press, 2006.
  • Kersting et al. [2007] K. Kersting, C. Plagemann, P. Pfaff, and W. Burgard, “Most likely heteroscedastic gaussian process regression,” in ICML, 2007.
  • Lazaro-Gredilla and Titsias [2011] M. Lazaro-Gredilla and M. Titsias, “Variational Heteroscedastic Gaussian Process Regression,” in ICML, 2011.
  • Kitagawa [1996] G. Kitagawa, “Monte carlo filter and smoother for non-gaussian nonlinear state space models,” Journal of Computational and Graphical Statistics, vol. 5, no. 1, pp. 1–25, 1996.
  • Mason [1986] M. T. Mason, “Mechanics and Planning of Manipulator Pushing Operations,” IJRR, vol. 5, no. 3, 1986.
  • Goyal et al. [1991] S. Goyal, A. Ruina, and J. Papadopoulos, “Planar Sliding with Dry Friction Part 1 . Limit Surface and Moment Function,” Wear, vol. 143, 1991.
  • Lynch and Mason [1996] K. M. Lynch and M. T. Mason, “Stable Pushing: Mechanics, Controllability, and Planning,” IJRR, vol. 15, no. 6, 1996.