2016 American Control Conference
July 6-8, 2016, Boston, MA, USA

Robust Gaussian Filtering using a Pseudo Measurement

arXiv:1509.04072v3 [stat.ML] 30 May 2016

Manuel Wüthrich1 , Cristina Garcia Cifuentes1 , Sebastian Trimpe1
Franziska Meier1,2 , Jeannette Bohg1 , Jan Issac1 and Stefan Schaal1,2
Abstract— Many sensors, such as range, sonar, radar, GPS
and visual devices, produce measurements which are contaminated by outliers. This problem can be addressed by using
fat-tailed sensor models, which account for the possibility of
outliers. Unfortunately, all estimation algorithms belonging to
the family of Gaussian filters (such as the widely-used extended
Kalman filter and unscented Kalman filter) are inherently incompatible with such fat-tailed sensor models. The contribution
of this paper is to show that any Gaussian filter can be made
compatible with fat-tailed sensor models by applying one simple
change: Instead of filtering with the physical measurement,
we propose to filter with a pseudo measurement obtained by
applying a feature function to the physical measurement. We
derive such a feature function which is optimal under some
conditions. Simulation results show that the proposed method
can effectively handle measurement outliers and allows for
robust filtering in both linear and nonlinear systems.

I. I NTRODUCTION
Robust and accurate state estimation is essential to safely
control any dynamical system. However, many sensors, such
as range, sonar, radar, GPS and visual devices, provide measurements populated with outliers. Therefore, the estimation
algorithm must not be unduly affected by such outliers.
In this paper we argue that problems with outliers are a
direct consequence of unrealistic, thin-tailed sensor models.
Unfortunately, many widely-used estimation algorithms are
inherently incompatible with more realistic, fat-tailed sensor
models. This holds true for the extended Kalman filter (EKF)
[1], the unscented Kalman filter (UKF) [2], and any other
member of the family of Gaussian filters (GF) [3], as we
will show in Section IV-A.
The contribution of this paper is to show that any member
of the family of GFs can be made compatible with fattailed sensor models by applying one simple change: Instead of filtering with the physical measurement, we filter
with a pseudo measurement. This pseudo measurement is
obtained by applying a time-varying feature function to the
physical measurement. We derive a feature function which is
optimal under some conditions. In simulation experiments,
we demonstrate the robustness and accuracy of the proposed
method for linear as well as nonlinear systems.
Numerous robustification methods have been proposed for
individual members of the family of GFs, often involving
significant algorithmic changes. In contrast, the proposed
method can be applied to any GF with only minor changes
in the implementation. Any existing GF implementation can
be robustified by merely replacing the sensor model with a
1
Autonomous Motion Department at the Max
Institute for Intelligent Systems, Tübingen, Germany.

Planck
Email:

first.lastname@tuebingen.mpg.de
2

Computational Learning and Motor Control lab at the University of
Southern California, Los Angeles, CA, USA.

pseudo sensor model, and the physical measurement with a
pseudo measurement.
II. R ELATED W ORK
Ad-hoc procedures for reducing the influence of outliers
have been employed by engineers for a long time. One such
heuristic is to simply discard all measurements which are
too far away from the expected measurement. This approach
lacks a firm theoretical basis and there is no rigorous way of
choosing the thresholds. Furthermore, the information contained in measurements outside of the thresholds is discarded
completely, which can lead to decreased efficiency [4]. For
these reasons, significant research effort has been devoted to
robustifying GFs in a principled manner. In the following we
distinguish two main currents on robust filtering, the first is
based on robust statistics in the sense of [5] and the second
is based on fat-tailed sensor models.
A. Robust Statistics
In the framework of robust statistics in the spirit of [5],
the objective is to find an estimator with a small variance
when the Gaussian noise is contaminated with noise from
a broad class of distributions. The resulting estimators are
intermediary between the sample mean and the sample
median. For instance, Masreliez and Martin [6] propose such
an estimator for linear systems. This approach is extended
by Schick and Mitter [4].
B. Fat-tailed Sensor Models
Since fat-tailed sensor models are by definition nonGaussian, finding the posterior estimate is not trivial. In
particular, a lot of effort has been devoted to finding filtering
recursions for models with Student t-distributed noise.
Roth et al. [7] show that for linear systems where the noise
and the state are jointly t-distributed, an exact filter can be
found. The authors mention that these noise conditions are
rarely met in practice, and propose an approximation for
state-independent t-distributed noise. A different approximation scheme for linear systems with t-distributed noise is
proposed in Meinhold and Singpurwalla [8].
While those approximations are hand-crafted, Ting et al.
[9] and Särkkä and Nummenmaa [10] use variational inference techniques to find an optimal approximation to the
posterior. Agamennoni et al. [11, 12] unify and generalize
those methods.
C. Extensions to Nonlinear Systems
All methods mentioned above assume a linear sensor
model. It is possible to apply them to nonlinear systems by
linearizing the sensor model at each time step, as is done in

the EKF. However, the EKF has been shown to yield poor
performance for many nonlinear systems [13, 2, 14].
Application of these robustification methods to other members of the family of GFs, such as the UKF or the divided
difference filter (DDF) [15], is not straightforward.
One way of doing so is proposed by Karlgaard and Schaub
[16], who use a robust Huber estimator [5] in a DDF.
Similarly, Piche et al. [17] propose a method of extending
the mentioned linear Student t-based filtering methods to
nonlinear GFs. However, both of these methods rely on an
iterative optimization at each time step, which is computationally expensive. In contrast, the robustification proposed
in this paper allows to robustify any of the numerous GF
algorithms with just minor changes in the implementation.
III. F ILTERING
A discrete-time state-space model can be defined by
two probability distributions: a transition model p(xt |xt−1 ),
which describes the evolution of the state in time, and a sensor model p(yt |xt ), which describes how the measurement
yt is generated given the state xt . Alternatively, these two
models can also be written in functional form
xt = g(xt−1 , vt )
yt = h(xt , wt )

(1)
(2)

with vt and wt being normally distributed noise variables.
Note that any (even non-Gaussian) model can be specified
in this way, since vt and wt can be mapped onto any desired
distribution inside the nonlinear functions g(·) and h(·).
A. Exact Filtering
Filtering is concerned with estimating the current state
xt given all past measurements y1:t = {y1 , . . . , yt }. The
posterior distribution of the current state p(xt |y1:t ) can be
computed recursively from the distribution of the previous
state p(xt−1 |y1:t−1 ). This recursion can be written in two
steps: a prediction step1
Z
p(xt |y1:t−1 ) =
p(xt |xt−1 )p(xt−1 |y1:t−1 )
(3)
xt−1

and an update step
p(xt |y1:t ) = R

p(yt |xt )p(xt |y1:t−1 )
.
p(yt |xt )p(xt |y1:t−1 )
xt

(4)

These equations can generally not be solved in closed form
[18]. The most notable exception is the Kalman filter (KF)
[19], which provides the exact solution for linear Gaussian
systems. Significant research effort has been invested into
generalizing the KF to nonlinear dynamical systems.
B. Gaussian Filtering
The KF and its generalizations to nonlinear systems (e.g.
the EKF and the UKF) are members of the family of GFs
[3, 20, 14, 21]. GFs approximate both the predicted belief (3),
as well as the posterior belief (4) with Gaussian distributions.
1 We

use the notation

R

x (·)

as an abbreviation for

R∞

−∞ (·) dx.

In the prediction step (3), the exact distribution is approximated by a Gaussian2
p(xt |y1:t−1 ) = N (xt |µxt , Σxt xt ).

(5)

The prediction step is not affected by the type of sensor
model used and will therefore not be discussed here, see for
instance [3, 20, 14, 21] for more details.
We will only consider the update step (4) in the remainder
of the paper. For ease of notation, we will not write the
dependence on past measurements y1:t−1 explicitly anymore.
The remaining variables all have time index t, which can
therefore be dropped. The predicted belief p(xt |y1:t−1 ) can
now simply be written as p(x), and the posterior belief
p(xt |y1:t ) as p(x|y), etc.
As shown in [20], the GF can be understood as finding
an approximate Gaussian posterior q(x|y) by minimizing the
Kullback-Leibler divergence [22] to the exact joint distribution
arg min KL[p(x, y)|q(x|y)].
q

(6)

The form of q(x|y) is restricted to be Gaussian in x
q(x|y) = N (x|m(y), Σ)
with the mean being an affine function of y
 
1
m(y) = M
.
y

(7)

(8)

This minimization is performed at each update step and
yields the optimal parameters of the approximation (7)

Σxy Σ−1
(9)
M = µx − Σxy Σ−1
yy
yy µy
|
Σ = Σxx − Σxy Σ−1
yy Σxy .

(10)

See [20] for a detailed derivation of this result. The parameters µx and Σxx are given by the belief (5) computed in the
prediction step. The remaining parameters are defined as
Z
µy = yp(y)
(11)
Zy
Σyy = (y − µy )(y − µy )| p(y)
(12)
Zy
Σxy =
(x − µx )(y − µy )| p(x, y).
(13)
x,y

For a linear system, this solution corresponds to the KF
equations [20].
Numeric Integration Methods: For most nonlinear systems, the integrals (11), (12) and (13) cannot be computed in
closed form and have to be approximated. In the EKF, this
is done by linearization at the current mean estimate of the
state µx . This approximation does not take the uncertainty
in the estimate into account, which can lead to large errors
and sometimes even divergence of the filter [13, 14].
Therefore, approximations based on numeric integration
methods are preferable in most cases. Deterministic Gaussian
integration schemes have been investigated thoroughly, and
the resulting filters are collected under the term Sigma Point
2 N (z|µ, Σ)

denotes the Gaussian with mean µ and covariance Σ.

A. The Gaussian Filter using Fat Tails
The GF approximates all beliefs with Gaussians, but the
sensor model can have any form. In principle, nothing
prevents us from using the GF with a fat-tailed sensor model.
Unfortunately, the GF is not able to do proper inference
using such a model. The sensor model p(y|x) enters the
GF equations only through (11), (12) and (13).
R To make
this dependency explicit, we substitute p(y) = x p(y|x)p(x)
in (11) and (12), and p(x, y) = p(y|x)p(x) in (13), and
integrate in y
Z
µy =
µy|x (x)p(x)
(14)
Zx
Σyy = (Σyy|x (x) + µy|x (x)µy|x (x)| − µy µ|y )p(x) (15)
Zx
Σxy = (x − µx )(µy|x (x) − µy )| p(x).
(16)
x

What is important to note here is that these equations only
depend on the sensor model through the conditional mean
and the conditional covariance
Z
µy|x (x) = yp(y|x)
(17)
Zy
Σyy|x (x) = (y − µy|x (x))(y − µy|x (x))| p(y|x). (18)
y
3 According

to De Finetti’s definition of probability [24].

30
x

20
10
true state
observation

0
0

50

100
time

150

200

Fig. 1: Simulation of the system with fat-tailed measurement
described in Example 4.1.
4
2
x

0
4
−2
2

x

−4
0
−5

−2

0
y

5

(a) The exact density p(x1 |y1 ) (white means higher). Overlaid:
40
−4
contour
lines of the approximate density q(x1 |y1 ) given by a fat−5 a thin-tailed
5
tailed30GF (green) and
0 GF (orange).
y

x

IV. A C ASE FOR FAT TAILS
Measurement acquisition is typically modeled by a Gaussian or some other thin-tailed sensor model. This assumption
is usually made for analytical convenience, not because it
is an accurate representation of the belief of the engineer.
If an engineer were to believe that measurements are in
fact generated by a Gaussian distribution, then she would
have to accept a betting ratio of 7 × 1014 to 1 that no
measurement further than 8 standard deviations from the
state will occur.3 Few engineers would be interested in such
a bet, since one can usually not exclude the possibility of
acquiring a large measurement due to unexpected physical
effects in the measurement process.
The mismatch between the actual belief and the Gaussian
model can lead to counter-intuitive behavior of the inference
algorithm. More concretely, the posterior mean is an affine
function of the measurement. This implies that the shift in
the mean produced by a single measurement is not bounded.
This problematic behavior disappears when using a more
realistic, fat-tailed model instead of the Gaussian model [8].
There are several definitions of fat-tails which are commonly
used [25]. Here, we simply mean any distribution which
decays slower than the Gaussian. Which particular tail model
is used depends on the application.

40

20
40
10
30

x

Kalman Filters (SPKF) [13]. Well known members of this
family are the UKF [2], the DDF [15] and the cubature
Kalman filter (CKF) [23]. Alternatively, numeric integration
can also be performed using Monte Carlo methods. The
method presented in this paper applies to any GF, regardless
of which particular integration method is used.

0
20
0
10

true state
GF thin
GF fat

50

100
time

150

0
0

200
true state
GF thin
GF fat

50

100
time

150

200

(b) True state of the system over time, with the obtained estimates
and their standard deviation.

Fig. 2: Standard GF applied to the system with fat-tailed
measurement described in Example 4.1.

Since fat-tailed sensor models typically have very large
or even infinite covariances, the GF will behave as if the
measurements were extremely noisy. It achieves robustness
by simply discarding all measurements, which is obviously
not the behavior we were hoping for.
B. Simulation Example
To illustrate this problematic behavior, we apply the GF
to the following dynamical system:
Example 4.1: System specification4
p(xt |xt−1 ) = N (xt |xt−1 , 1.0)
(19)
p(yt |xt ) = 0.9 N (yt |xt , 1.0) + 0.1 C(yt |xt , 10.0) (20)
p(x0 ) = N (x0 |0.0, 1.0)
(21)
The measurements are contaminated with Cauchydistributed noise, which leads to occasional outliers, as
4 C(z|µ, γ)

denotes the Cauchy distribution with location µ and scale γ.

shown in Figure 1. We apply two GFs to this problem. The
first uses a sensor model which does not take into account the
fat-tailed Cauchy noise, it only models the Gaussian noise,
i.e. the left term in (20). The second GF uses a sensor model
which is identical to the true sensor (20). We will refer to
the first filter as the thin-tailed GF, and to the second filter
as the fat-tailed GF.
In Figure 2a, we show the exact density p(x1 |y1 ) after the
first filtering step. The approximations obtained by the thintailed GF (yellow) and the fat-tailed GF (green) are overlaid.
It can be seen that the approximation to the exact posterior is
very poor in both cases. The mean of the exact density p(x|y)
is approximately linear in y for small y. For measurements
y larger than about 5.0, the posterior mean reverts back to
the prior mean 0.0 and does not depend on y anymore.
This behavior cannot be captured by an approximation of
the form of (8), since it only allows for linear dependences
in y. The approximation by the thin-tailed GF fits the exact
posterior well for small y, but instead of flattening out it
keeps growing linearly for large y. Hence, it is not robust to
outliers. The approximation by the fat-tailed GF correctly
captures the behavior of the exact posterior for large y,
i.e. it is independent of y. However, this implies that all
measurements, not just outliers, are ignored, as expected
from the analysis in Section IV-A. For both filters, the
poor fit translates to poor filtering performance, as shown
in Figure 2b.

where we have collected the terms independent of q(x|y)
in C. Since there is no constraint on m(y), (24) can be
optimized for each y independently. This means that the
integral can be dropped, and we can simply minimize the
integrand KL[p(x|y)|q(x|y)] with respect to m(y). It is a
standard result from variational inference that the optimal
parameters of a Gaussian approximation are obtained by
moment matching [26]. That is, the optimal mean function
m∗ (y) of the approximation is simply equal to the exact
posterior mean
Z
∗
m (y) = µx|y (y) =
xp(x|y).
(25)
x

Therefore, the feature vector ϕ(y) would ideally be chosen
such that µx|y (y) can be expressed through a linear combination of features. Unfortunately, µx|y (y) cannot be found
in closed form in most cases.
The standard GF represents the mean of the posterior as an
affine function of y. This form is optimal for linear Gaussian
systems, and it serves as a good approximation for many
nonlinear thin-tailed systems. Similarly, the idea here is to
find the optimal feature for a linear Gaussian system with
an additive fat tail. This feature can be expected to provide
a good approximation for nonlinear fat-tailed systems.
B. The Optimal Feature for a Linear, Fat-Tailed Sensor
Suppose that we have a linear Gaussian sensor model

V. A M EASUREMENT F EATURE FOR ROBUSTIFICATION

b(y|x) = N (y|Ax + a, P )

To enable the GF to work with fat-tailed sensor models,
we hence have to change the form of the approximate belief
(7). In [20] it is shown that more flexible approximations
can be obtained by allowing for nonlinear features in y. The
mean function (8) then becomes


1
m(y) = M
.
(22)
ϕ(y)

which we refer to as the body. We would like to add a fat
tail t(·) to make the filter robust to outliers. The combined
sensor model with tail weight 0 ≤ ω ≤ 1 is then

The resulting filter is equivalent to the standard GF using
a virtual measurement which is obtained by applying a
nonlinear feature function ϕ(·) to the physical measurement.
In the following, we find a feature ϕ(·) which enables the
GF to work with fat-tailed sensor models. Instead of handdesigning such a feature, we attempt to find a feature which
is optimal in the sense that it minimizes the KL divergence
between the exact and the approximate distribution (6).
For this purpose, we first find the optimal, non-parametric
mean function m∗ (y) with respect to (6). Knowing that the
mean m(y) is an affine function (22) of the feature ϕ(y), we
can then deduce the optimal feature function ϕ∗ (y).
A. The Optimal Mean Function
In order to find the function m∗ (y) which minimizes (6),
we rewrite the objective (6)


Z
p(x, y)
p(x, y)
(23)
KL[p(x, y)|q(x|y)] =
log
q(x|y)
Zx,y
= KL[p(x|y)|q(x|y)]p(y) + C (24)
y

p(y|x) = (1 − ω)b(y|x) + ωt(y|x).

(26)

(27)

1) Assumptions on the Form of the Tail: The precise shape
of the tail is application specific and does not matter for the
ideas in this paper. However, the subsequent derivation relies
on the assumption that the tail t(y|x) is almost constant in
x on the length scale of the standard deviation of the belief
p(x). This allows us to treat p(x) like a Dirac function with
respect to t(y|x). More concretely, we will assume that the
approximation
Z
Z
f (x)t(y|x)p(x) ≈ t(y|µx ) f (x)p(x),
(28)
x

x

is accurate for any affine function f (x).
This is a reasonable assumption, since the tail accounts
for unexpected effects in the measurement process, which
by definition bear little or no relation to the state x. For
instance, Thrun et al. [27] suggest to use a tail which is
independent of the state x and uniform in y, to account for
outliers in range sensors. For such uniform tails, (28) is exact.
For state-dependent tails, we expect this approximation to be
accurate enough to provide insights into the required form
of the feature.
2) The Conditional Mean: We will now find the posterior
mean µx|y (y) for this measurement model, which will then
allow us to find the optimal feature. The posterior mean can


Μx y

3
2

1.0

1

0.5
-10

-5

5

y

10

-1

-0.5

-2

-1.0

-3

Fig. 3: The approximate optimal mean function (32) for the
dynamical system from Example 4.1 (at time t = 1).

be obtained from the predicted belief p(x) and the sensor
model p(y|x) using Bayes’ rule
R
Z
xp(y|x)p(x)
.
(29)
µx|y (y) =
xp(x|y) = Rx
p(y|x)p(x)
x
x
Inserting (27) we obtain
R
R
(1 − ω) x xb(y|x)p(x) + ω x xt(y|x)p(x)
R
R
µx|y (y) =
.
(1 − ω) x b(y|x)p(x) + ω x t(y|x)p(x)
(30)
Both the predicted belief p(x) = N (x|µx , Σxx ) and the
body of the sensor model b(y|x) are Gaussian. Therefore, the
integrals in the first term of the numerator and the first term
in the denominator can be solved analytically using standard
Gaussian marginalization and conditioning. The integrals in
the second terms of the numerator and the denominator can
be approximated according to (28), and we obtain
µx|y (y) ≈ µ̃x|y (y)
=

(1 −

(31)

ω)(d + Dy)N (y|µby , Σbyy ) + ωµx t(y|µx )
(1 − ω)N (y|µby , Σbyy ) + ωt(y|µx )

(32)

where we have defined
| −1
D = (Σ−1
A)−1 A| P −1
xx + A P

d=

(Σ−1
xx

|

+A P

−1

−1

A)

(Σ−1
xx µx

(33)
|

−A P

−1

a).

The expectations in (32)
Z Z
µby =
yb(y|x)p(x)
Zx Zy
Σbyy =
(y − µby )(y − µby )| b(y|x)p(x)
x

5

-5

y

j1
j2
j3

Fig. 4: The three components of the optimal feature (36) for
the dynamical system from Example 4.1 (at time t = 1).

3) The Optimal Feature: To identify the feature required
to express the optimal mean m∗ (y) = µx|y (y) ≈ µ̃x|y (y), we
compare (32) to (22). All the constant terms can be collected
in M = 0 d D µx and all the terms which depend on
y are part of the feature5


(1 − ω)N (y|µby , Σbyy )
y(1 − ω)N (y|µby , Σbyy )
ωt(y|µx )
.
ϕ(y) =
(36)
(1 − ω)N (y|µby , Σbyy ) + ωt(y|µx )
In Figure 4, we plot the three dimension of (36) for the
Example 4.1. All of the feature components are asymptotically constant in y, which means that the estimate
remains bounded for arbitrarily large measurements. The
three components have intuitive interpretations. The first
two components are approximately constant and linear in y
respectively, for measurements close to the expected value.
Hence, they allow the filter to express an affine dependence
on y which will vanish for very large measurements. The
third component is small for y close to the expected value,
and grows up to some constant for y which are large. It
hence allows the mean estimate to revert to a constant value
for large measurements.
For the special case of ω = 0, the feature becomes
ϕ(y) = (1, y, 0)| .

(37)

(34)
Thus, if the sensor model does not have a fat tail, the standard
Gaussian Filter is retrieved. The linear mean function (8) is
a special case of the feature mean function (22).
(35)

y

only involve the body, and not the tail distribution. Hence,
we avoid the problems related to the potentially huge or even
infinite covariance of the tail discussed in Section IV-A.
In Figure 3, we plot the optimal mean function (32) for
dynamical system in Example 4.1 (at time t = 1). For y close
to the expected measurement µy = 0, the conditional mean
(32) is approximately linear in y. If a measurement y of large
magnitude is obtained, then the tail becomes predominant,
and the posterior mean reverts to the prior mean µx = 0.
The standard GF attempts to approximate this function by
an affine function (8). Not surprisingly, this yields very poor
results, as shown in Figure 2a.

VI. T HE ROBUST G AUSSIAN F ILTER
In the previous section, we found the approximately optimal measurement feature for a linear Gaussian sensor model
with additive fat tails. The GF can hence be enabled to work
with fat-tailed sensor models by filtering in feature space.
This robustification can be applied to any member of the
family of GFs, be it the EKF or an SPKF. We will refer
to the filter obtained by using the feature (36) as the robust
Gaussian filter (RGF).
For nonlinear, fat-tailed models, the RGF will not be
optimal, but it provides a good approximation in the same
way the standard GF provides a good approximation to
5 The factors (1−ω) and ω in the numerator could equally well have been
collected in M instead of the feature, since they are constant. However, we
prefer to maintain these terms in the feature since they provide appropriate
scaling.

4

Input: p(xt−1 |y1:t−1 ), yt , g(·), h(·)

2

Output: p(xt |y1:t )

4
0

p(xt |y1:t−1 ) = predict[p(xt−1 |y1:t−1 ), g(·)]

2:

p(xt |y1:t ) = update[p(xt |y1:t−1 ), h(·), yt ]

3:

Return p(xt |y1:t )

Algorithm 2 Robust Gaussian Filter

2
−2
0
−4

x

1:

x

Algorithm 1 Gaussian Filter

−10
−2

Output: p(xt |y1:t )

40
20

x

30

given µxt from Step 1 and µbyt , Σbyt yt from Step 2.
4: p(xt |y1:t ) = update[p(xt |y1:t−1 ), ϕt (h(·)), ϕt (yt )]
5: Return p(xt |y1:t )

x

N (yt |µbyt , Σbyt yt ) = predict[p(xt |y1:t−1 ), hb (·)]
3: ϕt (·) = feature[µxt , µbyt , Σbyt yt , t(·), ω] . as in (36),

5

10

y

30
10

p(xt |y1:t−1 ) = predict[p(xt−1 |y1:t−1 ), g(·)]

2:

0
y

−4 contour lines of the approximate density q(x |y ) overlaid
(a) The
1 1
40
on the−10
exact density
−5p(x1 |y1 ). 0
5
10

Input: p(xt−1 |y1:t−1 ), yt , g(·), h(·), hb (·), t(·), ω

1:

−5

20
0
10 0

true state
RGF

50

100
time

150

100
time

150

0
0

50

200
true state
RGF

200

(b) True state and filter estimates over time.

nonlinear, thin-tailed sensor models. If the RGF is applied
to a sensor model without a fat tail, it will coincide with the
standard GF, since the feature reduces to a linear function
(37). Hence, the RGF extends the GF. It broadens its domain
of applicability to fat-tailed sensor models.
Algorithm: For clarity, we describe the RGF algorithm
here step by step. Since this involves variables of several
time steps, we will reintroduce the time indices which we
dropped earlier.
The standard GF is described in Algorithm 1. The input to
the algorithm are the previous belief, the new measurement
yt , the transition model (1) and the sensor model (2). The
GF simply predicts, then updates, and finally returns the new
estimate. The concrete implementation of the predict and
the update functions depends on whether we are using an
EKF, a UKF, a DDF or some other GF.
The RGF is described in Algorithm 2. It requires the same
inputs as the GF, and additionally the separate components
of the sensor model: body, tail, and tail weight. In particular,
the functional form of the body hb (·) is used in Step 2, while
the feature computation in Step 3 requires the tail weight ω
and the evaluation of the tail’s distribution t(·).
The RGF delegates all the main computations to the basic
GF through the predict and the update functions. The
overhead in the implementation and in the computational cost
is minor. Hence, the proposed method makes it straightforward to robustify any existing GF algorithm.
VII. S IMULATION E XPERIMENTS
In this section, we evaluate the RGF through simulations.
First, we show that the optimal feature enables a good fit of
the approximate belief to the exact posterior in the linear
system used in previous sections. Secondly, we evaluate
the sensitivity of the RGF to the choice of tail parameters
(Section VII-B). Finally, we show that the proposed feature

Fig. 5: RGF applied to the system with fat-tailed measurement described in Example 4.1, to be compared to the
standard GF in Figure 2.

(36), which we designed for a linear system, also allows for
robustification in nonlinear systems (Section VII-C).
We implemented Algorithm 2 using Monte Carlo as
method for the numeric integration required by the
predict and the update functions6 .
A. Application to a Linear Filtering Problem
We revisit the simulation in Example 4.1 applying this
time the RGF, using the true transition and sensor models.
Comparing Figure 2a to Figure 5a, it is clear that the feature
(36) allows for a much better fit of the approximation to
the true density. As expected, this improved fit translates to
a better filtering performance (Figure 5b). As desired, the
proposed method is sensitive to measurements close to the
expected values, but does not react to extreme values.
B. Robustness to Tail Parameters
To show that the RGF is not sensitive to the specific choice
of the tail parameters, we simulate the same system as above,
and run several RGFs with different tail parameters. First,
we apply a RGF using a sensor model matching the true
sensor, i.e. with tail parameters ω = 0.1, γ = 10. Then, we
apply two RGFs which use incorrect tail parameters. In one
case we make both the weight and scale of the tail much
lower than in the true distribution: ω = 0.001, γ = 1.0
(underestimation of the true tail). In the other case we make
them much higher: ω = 0.5, γ = 100.0 (overestimation).
Figure 6 shows almost no degradation in the performance.
The key aspect enabling good filtering performance is that
6 Code is available at https://git-amd.tuebingen.mpg.de/
amd-clmc/python_gaussian_filtering

where v [1] and v [2] follow a standard normal distribution,
and ∆ is the discretization time
 The drag and gravity
 step.
0
t
Vt and Gt = − Gm
coefficients, Dt = −βt exp R0H−R
,
Rt3
0
dependqon the distance of the object to theq
centre of the Earth

40

x

30
20
true
RGF
RGF
RGF

10
0
0

50

100
time

state
overestimation
underestimation
exact model

150

Rt =

value
0.05
5 · 10−3
0.59
13.4
3.986 · 105
6374

units
s
km/s2
1/km
km
km3 /s2
km

σnom,r
σcon,r
σnom,θ
σcon,θ
α

200

value
0.5
15.8
0.63
200
0.15

units
km
km
mrad
mrad

TABLE I: Simulation parameters.

the sensor model has a tail which decays slower than the
Gaussian distribution, even when the shape of the true tail
is not precisely known.
C. Application to a Nonlinear Filtering Problem
As an example of nonlinear filtering, we consider the
problem of using measurements from a radar ground station
to track the position of a vehicle that enters the atmosphere
at high altitude and speed. The measurements provided by
the radar are range and bearing angle to the target vehicle.
This type of problem has been used before to compare the
capability of filters to deal with strong nonlinearities, e.g.
[28, 16].
The noise in radar systems is typically referred to as glint
noise in the literature, and is known to be contaminated
with outliers [29, 30, 16, 31, 32]. It has been modeled in
different ways, e.g. using a Student t distribution, or as a
mixture of two zero-mean Gaussian distributions (one with
high weight and low variance and another with low weight
and high variance), see [31] and references therein. In this
section, we simulate the same system as in [28], but replace
their Gaussian measurement noise with a mixture of two
Gaussians as in [16, 31].
State Transition Process: The state consists of the position
of the vehicle (x[1] , x[2] ), its velocity (x[3] , x[4] ), and an
unknown aerodynamics parameter x[5] , which has to be
estimated. The state dynamics are
[1]

[1]

[3]

xt+1 = xt + ∆xt
[2]
xt+1
[3]
xt+1
[4]
xt+1
[5]
xt+1

=
=
=
=

[2]
xt +
[3]
xt +
[4]
xt +
[5]
xt ,

(38)

[4]
∆xt
[3]
∆(Dt xt
[4]
∆(Dt xt

+
+

[1]
Gt xt )
[2]
Gt xt )

(39)

√
+
+

√

∆
∆

[1]
σ v vt
[2]
σ v vt

[2]

[3]

[4]

(xt )2 + (xt )2 ,
[5]

Fig. 6: Robustness of the RGF to the choice of tail parameters. The RGF behaves very similarly even when the tail
parameters are severely under- or overestimated.

∆
σv
β0
H0
Gm0
R0

[1]

(xt )2 + (xt )2 , its speed Vt =

(40)
(41)
(42)

and its unknown ballistic coefficient βt = β0 exp(xt ). Other
quantities such as the nominal ballistic coefficient β0 and the
mass m0 and and radius R0 of the Earth are constant, see
Table I.
Sensor Model: The radar is located at (xr , yr ) and measures range rt and bearing angle θt to the target object
q
[1]
[2]
[1]
rt = (xt − xr )2 + (xt − yr )2 + wt
(43)
!
[2]
xt − yr
[2]
+ wt
(44)
θt = 103 arctan
[1]
xt − xr
w ∼ (1 − α)N (w|0, Σnom ) + αN (w|0, Σcon ).
(45)
The nominal noise covariance is represented by Σnom =
2
2
2
2
, σcon,θ
]) is the
, σnom,θ
]), and Σcon = diag([σcon,r
diag([σnom,r
covariance of the contaminating noise component. We use α
and covariances similar to [31], see Table I.
Filter Specification: We compare an RGF with two GFs.
The three filters use transition models that coincide with the
real process (38)–(42).
In problems of this type, the contaminating noise is often
not precisely known. Therefore, we make our RGF assume
a measurement model as in Example 4.1
pRGF (w) = 0.9 N (w|0, Σnom )
+ 0.1 C(w|0, diag([(10σnom,r )2 , (10σnom,θ )2 ])),

(46)

which makes use of some knowledge of the nominal noise
Σnom , while the shape of the tail and the mixing weight take
default values. Similarly, the first GF only knows about Σnom
pGFthin (w) = N (w|0, Σnom ).

(47)

As discussed in Section IV-A, the GF is not able to
produce accurate estimates in systems with large variance
even if the true measurement process (45) is known. To show
this empirically, we apply a second GF which uses the true
covariance of the sensor (45)
pGFfat (w) = N (w|0, (1 − α)Σnom + αΣcon ).

(48)

We simulate the system during 100 s, using the integration time step ∆ for the predictions and taking radar
measurements at 1 Hz. As in [28], the initial state of the
system is x0 = [6500.4, 349.14, −1.8093, −6.7967, 0.6932],
and the initial belief for all filters is centered at µ0 =
[6500.4, 349.14, −1.8093, −6.7967, 0]. Note the mismatch
between the true ballistic coefficient and the initial belief,
i.e. the nominal β0 .
Results: Figures 7a and 7b respectively show the error in
the estimate of x[1] and the corresponding velocity x[3] . We
do not include the error in the position and velocity along
the other dimension, since they are qualitatively similar. We
can see that the GF using the nominal variance (yellow)
reacts strongly to outliers. The GF using the true variance

−2

−0.1

error in x[

error i

−0.2
−0.3

−3
20

40

60

80

100

20

40

time (s)

thin
GFGF
thin
GFGF
fatfat
RGF
RGF

0.20.2
0.4

0.2
0.10.1
0.0
0.0
0.0
−0.2

11
0
00
−2

−1
−1
−4
−2
−2
−6

100
100

RGF
RGF

8
−0.5
−0.5

60

80

100

40 40
60 60
time
time
(s)(s)

80 80

100
100

thin
GFGF
thin
GFGF
fatfat
RGF
RGF

−1.5
−1.5
4

−2.0
−2.0
2
−2.5
−2.5

4040
6060
time
time
(s)(s)

8080

RGF
RGF

0

100
100

(b)
ErrorGFGFbetween
estimated and true state
thin
thin
0.40.4
[3]
GFGF
fatfat component).
x0.2
(velocity
0.2
0.00.0

00

40

6
−1.0
−1.0

2020

error in x[4] (km/s)
error in x[4] (km/s)

8080

(a)4 4ErrorGFGFbetween
the estimated and the
thin
thin
fat (position component).
true2 2stateGFGF
xfat[1]

error in x[2] (km)
error in x[2] (km)

10
0.00.0

thin
GFGF
thin
GFGF
fatfat
RGF
RGF

−1.0
−0.3
−0.3

−3
−3
4040
6060
time
time
(s)(s)

20

time (s)

−0.4
−0.1
−0.1
−0.6
−0.2
−0.2
−0.8

2020

−2.5

100

error in x[5] (adimensional)
error in x[5] (adimensional)
position error (km)

22
2

80

time (s)

error in x[3] (km/s)
x[3] (km/s)
error in x[4]

error in x[1] (km)
x[1] (km)
error in x[2]

33
4

60

−1.5
−2.0

2020

(c)
1010 Euclidean
GFGF
thin
thin distance between estimated
GFGF
fat
fat position.
and true
2D

position error (km)
position error (km)

error

−1

8 8

RGF
RGF

Fig. 7: Results on the nonlinear filtering problem.
The RGF deals well with the nonlinearities
and the fat-tailed measurements.
6 6
−0.2
−0.2
−2
−2

−0.4
−0.4

4 4

−0.6
−0.6

−4
−4

−0.8
−0.8

2 2
[9] J.-A. Ting, E. Theodorou, and S. Schaal. A Kalman filter for robust outlier
(green)
of the sensor does not react as strongly.
However,
−6
−6
−1.0
−1.0
detection. In IEEE/RSJ0 International
Conference on Intelligent Robots and
0
due to the 20large
variance,
the
true state poorly.
In
4040
6060it tracks
8080
100
2020
4040
6060Systems
8080
2020
40 40
60 60
80 80
100
20
100
100
100
(IROS),100
2007.
time
(s)(s)
(s)(s)
time
(s)(s)
time
time
time
[10] S. Särkkä and A. Nummenmaa. Recursive noise adaptive Kalman filtering by
contrast, the RGF (red)
is robust to outliers and at the sametime
variational Bayesian approximations. IEEE Transactions on Automatic Control,
time tracks the true state well. This translates to a low 2D
2009.
location error as shown in Figure 7c. These results indicate [11] G Agamennoni, J. I. Nieto, and E. M. Nebot. An outlier-robust kalman filter.
In Robotics and Automation (ICRA), 2011 IEEE International Conference on,
that the optimal feature for linear systems allows to robustify
2011.
[12] G Agamennoni, J. I. Nieto, and E. M. Nebot. Approximate inference in statenonlinear systems too.

VIII. C ONCLUSION
In the standard GF algorithm, the mean estimate is an
affine function of the measurement. We showed that for fattailed sensor models this provides a very poor approximation
to the exact posterior mean.
A recent result [20] showed that filtering in measurement
feature space can allow for more accurate approximations of
the exact posterior. Here, we have found the feature that is
optimal for fat-tailed sensor models under certain conditions.
We have shown both theoretically and in simulation that
applying the standard GF in this feature space enables it to
work well with fat-tailed sensor models. The proposed RGF
is hence robust to outliers while maintaining the computational efficiency of the standard GF. Any member of the
family of GFs, such as the EKF or the UKF, can thus be
robustified by the proposed method without changing any of
the main computations.
We have applied this algorithm to the problem of 3D object
tracking using an Xtion range sensor [33]. The main source
of outliers in this application are occlusions of the tracked
object. While the standard GF immediately loses track of
the object when occlusions occur, the RGF works well even
under heavy occlusion.
R EFERENCES
[1] H. W. Sorenson. Kalman Filtering: Theory and Application. IEEE Press selected
reprint series. IEEE Press, 1960.
[2] S. J. Julier and J. K. Uhlmann. A new extension of the Kalman filter to nonlinear
systems. In Proceedings of AeroSense: The 11th Int. Symp. on Aerospace/Defense
Sensing, Simulations and Controls, pages 182–193, 1997.
[3] S. Särkkä. Bayesian filtering and smoothing. Cambridge University Press, New
York, NY, USA, 2013.
[4] I. C. Schick and S. K. Mitter. Robust recursive estimation in the presence of
heavy-tailed observation noise. The Annals of Statistics, 1994.
[5] P. J. Huber. Robust estimation of a location parameter. Annals of Mathematical
Statistics, 1964.
[6] C. Masreliez and R. Martin. Robust Bayesian estimation for the linear model
and robustifying the Kalman filter. IEEE Transactions on Automatic Control,
1977.
[7] M. Roth, E. Ozkan, and F. Gustafsson. A Student’s t filter for heavy tailed process
and measurement noise. In IEEE International Conference on Acoustics, Speech
and Signal Processing (ICASSP), 2013.
[8] R. J. Meinhold and N. D. Singpurwalla. Robustification of Kalman filter models.
Journal of the American Statistical Association, 1989.

[13]
[14]
[15]
[16]
[17]

[18]
[19]
[20]
[21]
[22]
[23]
[24]
[25]
[26]
[27]
[28]
[29]
[30]
[31]
[32]
[33]

space models with heavy-tailed noise. IEEE Transactions on Signal Processing,
2012.
R. van der Merwe and E. Wan. Sigma-Point Kalman Filters for probabilistic
inference in dynamic state-space models. In In Proceedings of the Workshop on
Advances in Machine Learning, 2003.
K. Ito and Kaiqi Xiong. Gaussian filters for nonlinear filtering problems. IEEE
Transactions on Automatic Control, 45(5):910–927, May 2000.
Magnus NøRgaard, Niels K. Poulsen, and Ole Ravn. New developments in state
estimation for nonlinear systems. Automatica, 36(11):1627–1638, November
2000. ISSN 0005-1098.
C. D. Karlgaard and H. Schaub. Comparison of several nonlinear filters for
a benchmark tracking problem. In AIAA Guidance, Navigation, and Control
Conference and Exhibit, Keystone, CO, USA, august 2006.
R. Piche, S. Särkkä, and J. Hartikainen. Recursive outlier-robust filtering and
smoothing for nonlinear systems using the multivariate student-t distribution.
In IEEE International Workshop on Machine Learning for Signal Processing
(MLSP), 2012.
H. J. Kushner. Approximations to optimal nonlinear filters. IEEE Transactions
on Automatic Control, 12(5):546–556, 1967.
R. E. Kalman. A New Approach to Linear Filtering and Prediction Problems.
Transactions of the ASME - Journal of Basic Engineering, (82 (Series D)):35–45,
1960.
M. Wüthrich, S. Trimpe, D. Kappler, and S. Schaal. A New Perspective and
Extension of the Gaussian Filter. In Robotics: Science and Systems (R:SS),
2015.
Y. Wu, D. Hu, M. Wu, and X. Hu. A numerical-integration perspective on
Gaussian filters. IEEE Transactions on Signal Processing, 54(8):2910–2921,
2006.
David J. MacKay. Information Theory, Inference and Learning Algorithms.
Cambridge University Press, 2003.
I. Arasaratnam and S. Haykin. Cubature Kalman filters. Automatic Control,
IEEE Transactions on, 2009.
B. de Finetti. La prévision : ses lois logiques, ses sources subjectives. Annales
de l’institut Henri Poincaré, 1937.
R. Cooke, D. Nieboer, and J. Misiewicz. Fat-tailed distributions: Data, diagnostics, and dependence. Technical report, 2011.
D. Barber. Bayesian Reasoning and Machine Learning. Cambridge University
Press, New York, NY, USA, 2012.
S. Thrun, D. Fox, W. Burgard, and F. Dellaert. Robust Monte Carlo localization
for mobile robots. Artificial Intelligence, 2001.
S. J. Julier and J. K. Uhlmann. Unscented filtering and nonlinear estimation.
Proceedings of the IEEE, 2004.
G. A. Hewer, R. D. Martin, and J. Zeh. Robust preprocessing for Kalman filtering
of glint noise. IEEE Transactions on Aerospace and Electronic Systems, 1987.
W.-R. Wu and P.-P. Cheng. A nonlinear IMM algorithm for maneuvering target
tracking. IEEE Transactions on Aerospace and Electronic Systems, 1994.
I. Bilik and J. Tabrikian. Target tracking in glint noise environment using
nonlinear non-Gaussian Kalman filter. In IEEE Conference on Radar, 2006.
H. Du, W. Wang, and L. Bai. Observation noise modeling based particle
filter: An efficient algorithm for target tracking in glint noise environment.
Neurocomputing.
J. Issac, M. Wüthrich, C. Garcia Cifuentes, J. Bohg, S. Trimpe, and S. Schaal.
Depth-Based Object Tracking Using a Robust Gaussian Filter. In Robotics and
Automation (ICRA), IEEE International Conference on, 2016. URL http:
//arxiv.org/abs/1602.06157.

