Lecture notes

Multi-Sensor Fusion and Estimation

for Robot Navigation

Philippe Bonnifait

December 20, 2023

Université de technologie de Compiègne

Cover note This document provides material that is constantly evolving from year
to year.
Should you find any errors or inconsistencies, the author would be grateful if you
could please bring them to his attention.

2
Contents
1 Generic problems 8
1.1 Basic concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.1.1 Robotic navigation . . . . . . . . . . . . . . . . . . . . . . . . 8
1.1.2 Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.1.3 Multi-sensor fusion . . . . . . . . . . . . . . . . . . . . . . . . 9
1.1.4 Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.1.5 Performance metrics . . . . . . . . . . . . . . . . . . . . . . . 10
1.1.6 Soft sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.2 Main categories of estimation and fusion problems . . . . . . . . . . . 14
1.2.1 Static problems . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.2.2 Dynamic problems . . . . . . . . . . . . . . . . . . . . . . . . 15
1.3 Modeling moving objects . . . . . . . . . . . . . . . . . . . . . . . . . 17
1.3.1 Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
1.3.2 Notations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
1.3.3 Evolution models . . . . . . . . . . . . . . . . . . . . . . . . . 20
1.3.4 Observation models . . . . . . . . . . . . . . . . . . . . . . . . 21
1.3.5 Mobile robot modeling . . . . . . . . . . . . . . . . . . . . . . 23
1.4 Classical navigation problems in robotics . . . . . . . . . . . . . . . . 23

2 Static Estimation 25
2.1 Linear case . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.1.1 Problem statement . . . . . . . . . . . . . . . . . . . . . . . . 25
2.1.2 Least Squares . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.1.3 Weighted Least Squares . . . . . . . . . . . . . . . . . . . . . 27
2.1.4 Example: combination of observations . . . . . . . . . . . . . 29
2.1.5 Robustification . . . . . . . . . . . . . . . . . . . . . . . . . . 30
2.2 Nonlinear Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
2.2.1 Searching for the zeros of a function: the iterative Newton-
Raphson method . . . . . . . . . . . . . . . . . . . . . . . . . 30
2.2.2 Adaptation of the method to state estimation . . . . . . . . . 32
2.2.3 Non-linear least squares: Gauss-Newton method . . . . . . . . 32
2.3 Examples of localization problems . . . . . . . . . . . . . . . . . . . . 34
2.3.1 Localization of a robot with goniometric measurements . . . . 34
2.3.2 Computation of a GPS fix . . . . . . . . . . . . . . . . . . . . 38

3 Dynamic State Estimation 44

3.1 Reminders on deterministic state observation . . . . . . . . . . . . . . 44
3.1.1 Observability of the state . . . . . . . . . . . . . . . . . . . . . 44
3.1.2 Dynamic observation . . . . . . . . . . . . . . . . . . . . . . . 44
3.1.3 Limitations of deterministic observation . . . . . . . . . . . . 46
3.2 Some elements on random signals theory . . . . . . . . . . . . . . . . 47
3.2.1 Variables and random vectors . . . . . . . . . . . . . . . . . . 47
3.2.2 Random signals . . . . . . . . . . . . . . . . . . . . . . . . . . 51

3
 Contents

3.3 Uncertainty ellipsoid . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

3.3.1 Equiprobability contour . . . . . . . . . . . . . . . . . . . . . 54
3.3.2 Ellipse of a Gaussian of dimension two . . . . . . . . . . . . . 54
3.3.3 Circle encompassing an ellipse . . . . . . . . . . . . . . . . . . 57
3.4 Second order state estimation . . . . . . . . . . . . . . . . . . . . . . 57
3.4.1 Problems considered . . . . . . . . . . . . . . . . . . . . . . . 57
3.4.2 Estimator/Predictor observer type . . . . . . . . . . . . . . . 58
3.5 Optimal linear estimation . . . . . . . . . . . . . . . . . . . . . . . . 60
3.6 Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
3.6.1 Update stage . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
3.6.2 Expressions of the Kalman gain . . . . . . . . . . . . . . . . . 63
3.6.3 Expressions of the covariance matrix update . . . . . . . . . . 64
3.6.4 Prediction stage . . . . . . . . . . . . . . . . . . . . . . . . . . 65
3.6.5 Sequential Program . . . . . . . . . . . . . . . . . . . . . . . . 65
3.6.6 Comments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
3.6.7 Case of observations with uncorrelated noises . . . . . . . . . 66

4 Non linear systems 71

4.1 Problem statement of probabilistic transformations of random vectors 71
4.2 Extended transformation of random vectors . . . . . . . . . . . . . . 72
4.2.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
4.2.2 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.2.3 Expression of the variance of the estimation error with Weighted
Least-Squares . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.3 Unscented transformation . . . . . . . . . . . . . . . . . . . . . . . . 76
4.3.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
4.3.2 Symmetric elementary sampling . . . . . . . . . . . . . . . . . 77
4.3.3 Sampling with 2n+1 points . . . . . . . . . . . . . . . . . . . 78
4.3.4 Sampling along the principal components . . . . . . . . . . . . 79
4.4 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . 80
4.4.1 Linearization around the current estimates . . . . . . . . . . . 80
4.4.2 Filter Steps . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
4.4.3 Calculation of Jacobian matrices . . . . . . . . . . . . . . . . 82
4.4.4 Sequential Program of an EKF . . . . . . . . . . . . . . . . . 82
4.5 Iterated Extended Kalman filter . . . . . . . . . . . . . . . . . . . . . 83
4.6 Unscented Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . 85
4.6.1 Update . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
4.6.2 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
4.6.3 UKF Sequential Program . . . . . . . . . . . . . . . . . . . . 88

5 Bayesian filtering 89
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
5.2 Bayesian Reminders . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
5.2.1 Continuous Case . . . . . . . . . . . . . . . . . . . . . . . . . 89
5.2.2 Discrete case . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
5.3 Bayesian Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
5.3.1 Observation Update . . . . . . . . . . . . . . . . . . . . . . . 97
5.3.2 State and Covariance Estimates . . . . . . . . . . . . . . . . . 98
5.3.3 Prediction (time-update) . . . . . . . . . . . . . . . . . . . . . 98

4
 Contents

5.4 Grid filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99

5.4.1 Static example . . . . . . . . . . . . . . . . . . . . . . . . . . 99
5.4.2 Dynamic grid filter . . . . . . . . . . . . . . . . . . . . . . . . 102
5.4.3 Comments on the grid filter . . . . . . . . . . . . . . . . . . . 105
5.5 Particle filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
5.5.1 Principle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
5.5.2 Monte Carlo Integration and random generator issue . . . . . 106
5.5.3 Computation of the weights of the particles . . . . . . . . . . 108
5.5.4 Bootstrap Particle Filter . . . . . . . . . . . . . . . . . . . . . 110
5.5.5 State and covariance estimation . . . . . . . . . . . . . . . . . 110
5.5.6 SIS Algorithm (Sequential Importance Sampling) . . . . . . . 111
5.5.7 SIR Algorithm: resampling the particles set . . . . . . . . . . 111
5.5.8 Resampling the SIR when necessary . . . . . . . . . . . . . . . 113
5.5.9 Strengths and weaknesses . . . . . . . . . . . . . . . . . . . . 114

6 Frames transformation with associated uncertainties 115

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
6.2 Notations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
6.3 Transformation operators . . . . . . . . . . . . . . . . . . . . . . . . . 116
6.3.1 Compounding operator ⊕ . . . . . . . . . . . . . . . . . . . . 116
6.3.2 Inversion operator ⊖ . . . . . . . . . . . . . . . . . . . . . . . 118
6.3.3 Composition of the operators . . . . . . . . . . . . . . . . . . 120
6.4 Operator ⊕ applied to a position . . . . . . . . . . . . . . . . . . . . 122
6.5 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
6.6 Non linearity issues from uncertainty propagation . . . . . . . . . . . 122

7 Multi-Sensor Fusion 126

7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
7.2 Static Data Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
7.2.1 Merging two uncorrelated estimates of the same unknown vector126
7.2.2 General case of the fusion of n vectors with uncorrelated errors 128
7.2.3 Merging two uncorrelated random vectors linked by a linear
relationship . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
7.3 Dynamic Data Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . 129
7.3.1 Merging synchronous measurements . . . . . . . . . . . . . . . 129
7.3.2 Fusion of asynchronous measures . . . . . . . . . . . . . . . . 132
7.3.3 Fusion of Out Of Sequence measures . . . . . . . . . . . . . . 133
7.4 Consistency of an estimator . . . . . . . . . . . . . . . . . . . . . . . 133
7.4.1 Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133
7.4.2 Chi2 law . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133
7.4.3 NEES test with a reference . . . . . . . . . . . . . . . . . . . . 134
7.4.4 NIS test on the innovation . . . . . . . . . . . . . . . . . . . . 136
7.5 Outliers rejection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
7.5.1 Mahalanobis distance . . . . . . . . . . . . . . . . . . . . . . . 137
7.5.2 Rejection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138
7.6 Data association . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138

8 KF with Covariance Intersection 140

8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
8.2 Covariance Intersection . . . . . . . . . . . . . . . . . . . . . . . . . . 140
8.2.1 Optimal implementation . . . . . . . . . . . . . . . . . . . . . 140

5
 Contents

8.2.2 Fast Covariance Intersection . . . . . . . . . . . . . . . . . . . 142

8.3 Kalman update with covariance intersection . . . . . . . . . . . . . . 143
8.3.1 First expression . . . . . . . . . . . . . . . . . . . . . . . . . . 143
8.3.2 Second expression . . . . . . . . . . . . . . . . . . . . . . . . . 144
8.4 Split Covariance Intersection Filter . . . . . . . . . . . . . . . . . . . 147
8.4.1 Update stage . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
8.4.2 Prediction stage . . . . . . . . . . . . . . . . . . . . . . . . . . 148
8.4.3 The SCIF: a general filtering method . . . . . . . . . . . . . . 148
8.4.4 Practical implementation . . . . . . . . . . . . . . . . . . . . . 148

6
Nomenclature
(x, y) 2D Cartesian coordinates

α Model error

β Measurement error

x̂ Estimated state vector

ω Yaw Rate

ϕ Roll angle

ψ Yaw angle

θ Pitch angle in 3D or Heading angle in 2D

P Covariance or covariance matrix

Pxy Cross-covariance matrix between the prediction error and the predicted ob-
servation

Py Covariance matrix of the predicted observation

x State or parameter vector

y Measurments or observation vector

GDOP Global Dilution Of Precision

MOT Moving objects tracking

NEES Normalized Estimation Error Squared

NIS Normalized Innovation Error Squared

SLAM Simultaneous localization and mapping

TDOA Time Difference Of Arrival

TOA Time Of Arrival

7
1 Generic estimation and
multi-sensor fusion problems for
robot navigation
1.1 Basic concepts
1.1.1 Robotic navigation
Navigation is a field of study that focuses on the process of monitoring and control-
ling the movement of a mobile platform from one place to another. For a mobile
robot (e.g. an autonomous vehicle, a UAV), the main objective is to reach a given
goal while avoiding obstacles.
For some authors (like [2]), navigation is the estimation of the state of the platform
on which the sensors are located.
In this course which deals with mobile robots, navigation is the simultaneous
estimation of the state of the robot and of the dynamic environment.
The content focuses on the following elementary functions:

• The vehicle must determine its own location (in absolute and with respect to
a map),

• Localize and track the others objects (vehicles, pedestrians),

• Characterize its environment and improve its map as appropriate.

Based on this knowledge, the robot can plan a path or a trajectory and control its
motion until the end of the mission.

1.1.2 Estimation
Estimation is the process of inferring the value of a quantity of interest from indirect,
inaccurate and uncertain observations.
In a continuous space, estimation can be viewed as the process of selecting a
particular point - the “best estimate” [2].
In a discrete-valued case, estimation (also called decision in this particular case)
can be viewed as the selection of one out of a set of discrete alternatives.
In estimation theory, two approaches are generally considered:

• The probabilistic approach assumes that the measured data is random with
probability distribution dependent on the parameters or variables of interest,

• The set-membership approach assumes that the measured data vector belongs
to a set which depends on the parameter vector.

8
 1 Generic problems

Both theories have advantages and drawbacks. On the one hand, representing ran-
dom vectors by sets makes it possible to have less assumptions on the random
variables (such as independence) and dealing with non-linearities is easier. On the
other hand, a probability distribution function provides a richer information than a
set enclosing its support.

1.1.3 Multi-sensor fusion

Multi-sensor fusion (or multi-sensor data fusion) is the process of combing informa-
tion from a number of different sources to provide a robust and complete description
of an environment or process of interest.
Multi-sensor fusion is of special significance in any application where a large
amount of data must be combined, fused and distilled to obtain information of
appropriate quality and integrity on which decisions can be made [6].
Multi-sensor fusion can also refer to estimation problems with more information
than unknowns in which one seeks to take advantage of all available information.
Information means here
• Measurements,
• Relationships (like observation or evolution models),
• Knowledge of the various random factors (noises or disturbances)
• Prior knowledge.
Information can be
• Inaccurate (see below),
• Uncertain (see below),
• Complementary (two information sources provide different characteristics or
knowledge of the same observed phenomenon),
• Redundant (two sources observe the same phenomenon and provide the same
information),
• Contradictory (in conflict),
• Abnormal (ie an outlier that not matches the model that one has),
• Incomplete (partial knowledge or some information about the observed phe-
nomenon is missing).
The goal of multi-sensor fusion is to:
1. Detect consistent and inconsistent data,
2. Merge the data that are consistent and solve ambiguities,
3. Reject outliers (and sometimes identify them),
4. Provide confidence indicators on which decisions can be made.
Multi-sensor fusion is of special significance in any application where a large amounts
of data must be combined, fused and distilled to obtain information of appropriate
quality and integrity.

9
 1 Generic problems

1.1.4 Tracking
Tracking is the estimation of the state of a moving object based on remote measure-
ments. It can be done using one or more sensors at fixed locations or on moving
platforms.
In this course, the sensors are attached to the mobile robot that has to track by
itself the moving objects in its surrounding.

1.1.5 Performance metrics

[Link] Accuracy
There exists different meanings of the word “accuracy”.
According to the metrology standard ISO 5725-1, accuracy consists of Trueness
(proximity of measurement results to the true value) and Precision (repeatability or
reproducibility of the measurement).

Figure 1.1: Accuracy = Trueness + Precision (Justesse et fidélité in French)

[Link] Uncertainty
There exists different meanings of the word “uncertainty”.
Usually, in a multi-sensor fusion context, uncertainty is linked to the confidence
one has in a proposal (or an hypothesis) to be true.
Higher the uncertainty, lower the trueness of the proposal.
Uncertainty is a different concept from inaccuracy.
Examples:

• “Paul is approximately 18 years old.”

– The age of Paul is inaccurate,
– The fact that Paul is of the age of legal majority is uncertain.

• Consider an observer who tries to detect and track targets.

– The actual existence of each target is uncertain,
– Each track is followed with inaccuracy.

10
 1 Generic problems

[Link] Inaccuracy and uncertainty in information fusion

In the field of information fusion (and not data fusion) inaccuracy and uncertainty
have quite different meaning since often the frame of discernment is discrete (in
opposition to continuous).
Let consider an example. We have two camera systems able to characterize the
color of some balls which can be in the following frame of discernment

Θ = {red, green, blue, black, yellow} (1.1)

We present a ball which is “blue”.
The first system says: I have detected a “blue or black ball” with a confidence of
60%
The second one: I have detected a “red or blue or black ball” with a confidence of
90%
The first one is more accurate but less certain than the second one.

[Link] Integrity of an information

Integrity is a measure of the trust that can be placed by a client application in the
correctness of the information supplied by a system. Integrity includes the ability
of the system to provide timely warnings to users when the system should not be
used. Integrity is frequently related to safety critical applications. Integrity is also
sometimes defined as the ability to get reliable confidence information associated
with a result.

When applied to positioning, the integrity concept can be described in a numerical

manner according to the notions of Alert Limit, Time To Alert, Integrity Risk and
Protection Level that define the level of risk associated with the position estimates.
These variables are defined below to express numerically the integrity concept [26]:
• Alert Limit (AL), is the largest error allowable for safe operation without
issuing an alert. It can be defined in terms of Horizontal Alert Limit (HAL).

• Time To Alert (T T A), is the maximum allowable time from the appearance
of position failure until the alert.

• Integrity Risk (IR), is the probability (per operation or per hour) that the
position error exceeds a protection level (see definition below) at any moment
without providing a timely warning. In other words, it is the maximum prob-
ability with which the receiver is allowed to provide position failure without
warning the user.

• Protection Level (PL), is a statistical bound on the Position Error (PE) such
that the probability of the absolute error exceeding the PL is smaller or equal
to the Target Integrity Risk (T IR), which is defined as a requirement for the
application:
P r(PE > PL) ≤ T IR. (1.2)

The T IR is used to tune the parameters of the models and filters to compute the
PL in real-time. A system is considered as satisfying the integrity requirement at a
given T IR, if the following relation is empirically verified:

IR < T IR. (1.3)

11
 1 Generic problems

PL is partitioned in two parts: Horizontal PL (HPL) and Vertical PL (VPL). The

HPL represents the radius of a circle in the horizontal plane (e.g. a plane tangent to
the Earth surface), with its center being at the estimated position. It describes the
region certain to include the true (and unknown) horizontal position for the given
T IR. When the PL exceeds the alert limit, it implies that the system is unavailable.
Figure 1.2 shows the Stanford-ESA integrity diagram used to evaluate the integrity
level. It traces the PE versus the PL for a set of measurements with a given T IR,
allowing for the evaluation of the performance of the location estimates and their
usability. This evaluation is defined as follows:

• System Unavailability (SA), the system is unavailable when the protection

level exceeds the alert limit.

• Misleading Information (MI), arises when the position error exceeds the pro-
tection level (underestimation of PE by PL) but stays below the alert limit.

• Hazardous Misleading Information (HMI), occurs when the system is declared

available, i.e., no alert is issued, but the position error exceeds the alert limit.

Alert Limit
Alert Limit

Figure 1.2: Stanford-ESA integrity diagram from [25].

1.1.6 Soft sensors

There are many systems for which there is a redundancy of sensors.
This is the case, for example, in a modern car equipped with

• An Anti-lock Braking System (ABS) on all 4 wheels,

• A trajectory stabilizer - Electronic Stability Program (ESP).

In this case and to achieve these functions, the computers use the following mea-
surements:

1. Elementary displacements of the wheels,

2. Instantaneous speeds of the wheels (speed tachometer using the pulses),

3. Yaw rate gyro,

4. Angular position of the steering wheel,

12
 1 Generic problems

5. Longitudinal accelerometer.

On a car equipped with a CAN bus, this information may be made available to any
computer connected to the same bus.
Now, if it is desired to have a measure of the speed of the vehicle, there appears
a redundancy of measures. Which to choose? Instead of answering this question,
multi-sensor fusion is an approach that provides a generic methodology.

One can also note that some sensors can expect input parameters and not only
upon initial configuration.
This is for example the case for

1. A raster image from a camera. Processing may be limited to a window of

interest.

2. A steerable camera,

3. A digital map in which we will look for information around a given point.

Figure 1.3 illustrates the concept of soft sensor.

Soft
Sensors sensor

Multisensor Soft Estimates

Fusion and sensor with confidence
Estimation indicators

Sensors with
input parameters
Soft
sensor

Information exchange

Figure 1.3: Soft sensor

The engine that fuses and filters the data has the following characteristics.
It can

1. Acquire the raw data

2. Filter it if necessary

3. Estimate quantities not directly observed

4. Merge this information with other data

5. Match data

6. Detect inconsistencies

13
 1 Generic problems

7. Diagnose faults and failures

The outputs are estimated with associated quantified inaccuracies and uncertainties.
The module presented here is centralized, but this is not the only possibility. In
practice, there are often multiple levels of fusion for reasons of implementation and
achievement (eg, video pre-processing to extract primitives).

1.2 Main categories of estimation and fusion

problems
Estimation and multi-sensor fusion can be viewed as schemes for information ex-
traction and combination based on measurements.
They are usually classify into the following categories depending of the nature of
the considered variable:

1. A parameter i.e. a time-invariant quantity (a scalar or a vector)

2. The state of a dynamic system (usually a vector), which evolves in time ac-
cording to a stochastic equation

1.2.1 Static problems

A static problem is a problem in which time is not involved explicitly because:

1. The system is static (stationary). The data were obtained at the same place
possibly by different sensors,

2. All data were sampled at the same time.

Let denote

• x ∈ Rn the set of unknowns,

• y ∈ Rm the set of observations (or measurements)1

If m > n, then there is data redundancy: it is a static fusion problem.

The relation (or set of relations) between y and x is called observation equation.
Observation models can be linear:

y = C.x + β (1.4)

Where β represents an observation error vector of the same size as y.

or non-linear:
y = g (x, β) (1.5)
The problem to solve is to find the “best” value of the state x given this observation
model and the measurements.

1
An "observation" is data that can be directly used in a filter or state observer. A "measurement"
is a data measured by a sensor. Often, measurements and observations are the same but, in
some problems, measurements must be associated (to a map for example) to be used in a filter.
In this case, when the measurement is unambiguously associated, it becomes an observation.

14
 1 Generic problems

1.2.2 Dynamic problems

Dynamic systems whose state x(t) changes over time are now considered. We say
that we treat a dynamic fusion/estimation problem if we are able to model the
evolution over time of all or only some of the components of x(t).
The notion of state space provides an interesting methodological framework.

[Link] Continuous state space

Consider a system operating under the effect of an input u(t) generating observations
y(t).

u y
system

Figure 1.4: Schematic bloc representation of a system

At any given time, any system (mechanical, electrical, chemical, etc.) is in a

certain state. The state is the set of variables that define the system completely to
the problem that we consider.
For example, the dynamic state of a solid is its position, speed and acceleration.
In an electrical circuit, the state is the current in each inductor and the voltage
across each capacitor.
A state space is not unique.
If we propose to use different quantities to represent the state, and we bring
together these quantities in a single vector denoted x(t), then x(t) is a relevant
proposal if it corresponds to it a system of equations of the following kind [11] :

ẋ(t) = f (x(t), u(t))

(
(1.6)
y(t) = g (x(t), u(t))

The state x is a vector that contains the relevant variables to observe and control the
system with the input u. The first equation is called evolution model and the second
one observation model where y represents the measurements from the sensors. In
general, x, u and y are vectors. When the input appears in the observation model,
we say that there is transmission of the input to the output.
In the linear case, the state space is:

ẋ(t) = A.x(t) + B.u(t)

(
(1.7)
y(t) = C.x(t) + D.u(t)
Let x0 be the state corresponding to the initial time t0 . Under regularity condi-
tions, the current state can be written as:

x(t) = φ (x0 , u(τ ) | τ ∈ [t0 , t]) (1.8)

Thus, the current state depends only on the initial and subsequent entries, which
is why the ability to find x0 is related to the notion of observability. It can be noted
that the input u(t) may not exist; the system is then in a free running state.

15
 1 Generic problems

[Link] Discrete state space

When using a computer, one thinks at discrete times. If periodic sampling is as-
sumed, we write

xk = x(t0 + [Link] )
In this case, the continuous state space (1.6) which is a view of the system, becomes
a discrete state space:

xk+1 = fd (xk , uk )
(
(1.9)
yk = g(xk , uk )
The model function fd is different from f .
Within the framework of automatic control, the input u(t) is the output of a
control program which periodically updates the output of its D/A converter. In this
case, u(t) is constant between sampling times and it is possible to find exact discrete
models in the linear case. In the nonlinear one, the exact discretization is usually
impossible and an approximate discretization has to be used.

u(t)

t
k k+1

Figure 1.5: Piece-wise constant control

However, when using a state space in a multi-sensor problem, some problems can
be modeled with a measured input um,k . In this case, the state space is usually
written as:

xk = f (xk−1 , um,k )
(
(1.10)
yk = g(xk , uk )
This indicates that the measures um,k and yk are sampled at the same time.
In robotics, sampling can be spatial, temporal or both.

[Link] Stochastic state space

In general, evolution and observation models are approximate because:

1. Models are approximations of reality,

2. They are often the result of an approximate discretization,

3. They involve sensors, always affected by errors.

16
 1 Generic problems

We explicit these errors in a so-called stochastic representation.

ẋ(t) = A.x(t) + B.u(t) + G.α(t)

(
(1.11)
y(t) = C.x(t) + D.u(t) + H.α(t) + β(t)
α is the noise model which often represents disturbances and β the observation
noise mainly related to sensor measurements.
If there is no direct transmission of the input to the output, this form simplifies
to:

ẋ(t) = A.x(t) + B.u(t) + α(t)

(
(1.12)
y(t) = C.x(t) + β(t)
This is the one we will consider later in this course.
In practice, we will often consider discrete state spaces as2 :

xk+1 = Ad .xk + Bd .uk + αk

(
(1.13)
yk = [Link] + βk

1.3 Modeling moving objects

1.3.1 Frames
[Link] Global frame
This is a reference linked to the earth and which rotates with it. It’s called ECEF
(Earth Centered Earth Fixed). Its center is near the center of the earth. The most
famous is the WGS84 used by the GPS system.

Figure 1.6: ECEF frame

2
People often do an abuse of notation by keeping the same names for the matricesA and B in
continuous and discrete cases (although they are different)

17
 1 Generic problems

[Link] Navigation frame

This is a frame tangent to the evolution area. For example, one can choose its origin
at the center of research of UTC, the vertical axis z being along the gravity vector,
its x-axis to the east and y to the north. This is the East-North-Up (ENU) frame.

Z
y

x
O

Y

Figure 1.7: Navigation frame

[Link] Mobile or body frame

This is the reference frame associated to the vehicle. It is usually located at the
middle of the rear wheels for road vehicles. By convention, the x-axis is carried by
the vector →
−
s which is collinear with the speed vector →
−
v.

Rolling surface zM Body frame

RM

z yM
a

(s, n, a) M n
orthonormal and
direct v
s
Navigation xM
O z
frame y

y
x

Figure 1.8: Body frame

18
 1 Generic problems

Horizontal plan
a
z0
n
(s, n, a)
orthonormal and s v
pitch
direct roll

z y0
O

y
x g
yaw
x0 s0
Earth gravity

Figure 1.9: Attitude angles

1.3.2 Notations
When one manipulates a vector (denoted X for instance), he has to defined the
reference frame. A classical notation is to use a superscript like :

0
X (1.14)
which means that X is expressed in the frame R0 .
When the superscript is omitted, the frame has to be clearly defined in the text.

Position It will be noted in Cartesian coordinates by:

 
x
 y  (1.15)
 

z
Note: Be careful not to confuse the x-coordinate with the state vector and the
y-ordinate with the vector of measurements. In practice, the confusion is easy to
avoid.

Attitude It is noted, for example with three angles, even if there are many other
representations:
 
φ
 θ  (1.16)
 

ψ
Where the components are the angles of roll (φ), yaw (θ) and pitch (ψ) of the
mobile relative to the local tangent coordinate system (cf figure 1.9).

19
 1 Generic problems

Pose The pose is defined by the position and attitude of an object. In 2D, it is
usually represented by the position x and y and the yaw angle θ (also called heading
angle) :
 
x
q= y  (1.17)
 

Linear velocities  
vx
 vy  (1.18)
 

vz

Angular rate  
ωx
 ωy  (1.19)
 

ωz

Acceleration  
ax
 ay  (1.20)
 

az

Example of a state vector We consider the state of a car in a plane universe. ω

is the yaw rate, v represents the module of the velocity and a its acceleration.
 
x

 y 

θ
 
X= (1.21)
 
 

 ω 

v
 
 
a

1.3.3 Evolution models

Point kinetics model They are used for object tracking when one knows poorly
the dynamics of the object which is often considered a movable point. There are
many models. Consider the one at constant velocity:

ẋ = vx




ẏ = vy






ż = v


y
(1.22)




v̇x =0
=0




v̇y
=0


v̇
z

h iT
If X = x y z vx vy vz is the state, the evolution model is:

20
 1 Generic problems

0 0 0 1 0 0
 

 0 0 0 0 1 0 

0 0 0 0 0 1
 
Ẋ = = A.X (1.23)
 
 .X
0 0 0 0 0 0

 
 
0 0 0 0 0 0
 
 
0 0 0 0 0 0

Planar odometric model An odometric model takes into account the fact that
wheels undergoes non-holonomic constraints, that is to say, the velocity vector to a
wheel is parallel to its longitudinal axis (in a frame tangent to the trajectory of the
vehicle):

= v. cos(θ)

ẋ




ẏ = v. sin(θ)
θ̇ = ω



in which (v, ω) is the input or a measurement of the input.

1.3.4 Observation models

General form When the physical phenomenon is described, generally one estab-
lishes an implicit formula:

h(y, x, m) = 0 (1.24)
Where y corresponds to the measurements, x to the unknown state and m denotes
the map (landmarks or beacons locations).
Often we seek an expression in the form:

y = g(x, m) (1.25)

Line of Sight

zu

Beacon
Sensor
Frame u
zb
Ru  yu
u

 u
yb

xu
u
xb

Figure 1.10: Goniometric (angles) measures expressed in the sensor frame u.

21
 1 Generic problems

Goniometry
λ = atan2 (
u
y b , u xb ) 


u (1.26)
 σ = arctan √u 2zb u 2
yb + xb

Where u represents the sensor frame and b the beacon.

Telemetry by round-trip time of flight There are two main technologies:

• Laser beams,
• Radio waves (sound or hyper-frequency).
In the first case, a laser diode emits a highly focused signal (typically infrared).
When the beam encounters a surface which reflects in a diffuse way (that is the
opposite of a mirror so that the beam would bounce), some of the signal returns to
the transmitter. If the rangefinder is digital, it arms a counter at the time it sends
the laser pulse and stops the counter when the signal returns. It thus measures the
time of flight.
A similar mechanism exists with radio waves. If the transmitter sends a wave to
a receiver who responds, then there is a method to subtract the clock offset of the
two devices.
In both methods, the time it takes the wave to travel and return to the emitter is
a direct measurement of the distance between the emitter and the reflector. Under
the hypothesis that the signal evolves in light of sight (no reflexion), this distance ρ
is obtained by multiplying the time of flight by the velocity of light (denoted c) and
by dividing by two:
Travel_time.c
ρ=
2
If the position (xb , y b ) of the reflector (or the beacon) is known:
q
ρ= (x − xb )2 + (y − y b )2 (1.27)

Pseudo-distance When the measurement wavelength is only one trip, there are
inevitable offsets between the clocks of the transmitter and receiver.
In this case, we speak of "pseudo-distance" (TOA - Time Of Arrival).
q
ρ= (x − xb )2 + (y − y b )2 + d (1.28)
Where d is the elongation due to the offsets between the clocks.
In 2D, it takes 3 measurements to determine the three unknowns (x, y, d).
GNSS pseudo-distances are typical examples of this kind of technology.

Difference of time of flight One way to remove clock offsets is to make differences
between measurements. We speak of "simple difference" (term used by GPS) or "time
difference of arrival" (Time Difference Of Arrival - TDOA ) in this case.
Let consider two time of flight measurements made with the same clock at the
same time. The offset d is the same in both cases:
q
ρ1 = (x − xb1 )2 + (y − y b1 )2 + d (1.29)

q
ρ2 = (x − xb2 )2 + (y − y b2 )2 + d (1.30)

22
 1 Generic problems

q q
ρ12 = ρ2 − ρ1 = (x − xb2 )2 + (y − y b2 )2 − (x − xb1 )2 + (y − y b1 )2 (1.31)

We speak of hyperbolic resolution in this case. The focuses of the hyperbolas are
the two beacons. With 3 beacons, we obtain two equations to calculate the two
unknowns (x, y).

Kinematic observation models It is often of measures relying on the Doppler

effect which appears when the relative distance between the receiver and the trans-
mitter changes.
It is widely used in GPS.

1.3.5 Mobile robot modeling

For a mobile robot to be autonomous, it must have sensors, a map of the environment
and a control computer that allows it to actuate its motion.
As it is a dynamical system, it can be modeled with state equations:

ẋ(t) = f (x(t), u(t))

(
(1.32)
y(t) = g (x(t), m)
where
• The state x(t) contains at least its pose in the working frame, but this vector
can include other parameters useful for its navigation, such as the position
and speed of other dynamical agents in its environment,
• The input u(t) is the vector containing its inputs,
• The output y(t) contains in general the measurements,
• f () is the evolution model,
• g () the observation model,
• m is the map on the environment. As it evolves in general slowly, it does
not depend on time. The map m can be vectorial such that it contains the
coordinates of landmarks that are useful to localize the robot or it can contain
vectorial features that describe its environment for path planing. The map
can be also discrete in the form of 2D or 3D grids.

1.4 Classical navigation problems in robotics

• Ego-localization (or pose estimation). The mobile robot seeks to determine
its pose knowing observations (or measurements) on beacons or landmarks
with known positions. It usually uses an exteroceptive sensor (like a lidar or
a camera) with the help of proprioceptive sensors like an odometer or a yaw
rate gyro. When its uses only its proprioceptive sensors, the robot estimation
of the pose is done by Dead-Reckoning (DR). In this case, the estimate drifts
without bounds.
• Mapping knowing the pose (or mobile mapping). This is the dual: to map the
detected landmarks knowing the pose of each measure.

23
 1 Generic problems

• Simultaneous localization and mapping (SLAM). The environment is assumed

to be perfectly static and the mobile robot seeks to localize itself and map
the environment. From the localization point of view, SLAM enhances Dead-
Reckoning with the ability to correct drift when the robot reaches a place
where it has been before (this is the loop-closure stage).

• Moving objects tracking (MOT). This is a problem of detecting, localizing and

tracking moving objects in the mobile frame.

• Drivable space determination. Sometimes the robot evolves in an uneven or

constraint terrain. It has to determine where it can move safely.

• Simultaneous localization, mapping and moving objects tracking (SLAMMOT).

This is the problem of SLAM with moving objects in the field of perception.
This is the most complicated problem.

24
2 Static problems in estimation and
multi-sensor fusion
We are interested in this chapter to a class of problems in which the unknowns
are related to the measurements by a noisy observation equation generally over
determined:
y = g(x) + β
Where

• dim(y) = m ≥ dim(x) = n (over-determined)

• the vector β refers to observation errors.

2.1 Linear case

2.1.1 Problem statement
Without any knowledge of measurement noise except that it is centered (with zero
mean) and if the observation equation is linear, the fusion problem can be solved by
least squares.
y = Cx + β (2.1)
y1
 
 y2 
Where x represents the unknown to be computed and y =  
the measure-
...
 
 
ym
ments.
Here, m > n and the C matrix is called vertical with more rows than columns.
See Fig. 2.1 for an illustration.

25
 2 Static Estimation

Telemeter 1

Telemeter 2

Best solution

Figure 2.1: Linear Least Squares example: The robot tries to localize itself on the
abscissa axis with two redundant and noisy measurements. Each tele-
metric measurement provides a different solution. Merging the two gives
a more accurate estimate of the true position.

Least Squares are a generic solution to estimate the parameter vector x knowing
the model (2.1) and the measurement vector y.
Least Squares are a powerful estimation method when the measurements are af-
fected only by noise (and when the model is well adapted to explain the data). When
there are also outliers, Least Squares have to robustified by using M-estimators or
the Ransac method. This is not discuss in this chapter.

2.1.2 Least Squares

The least squares solution is to find the parameter vector x that minimizes the
following quadratic criterion:

J(x) = (y − Cx)T (y − Cx) (2.2)

This criterion can also be written as the Sum of Squared Residuals (SSR):
m m
J= (yj − C(j, :)x)2 = rj2
X X

j=1 j=1

Where C(j, :) represents the j-th row of matrix C and the rj are the model resid-
uals, i.e. the differences between the measures yj and their predictions.
J can be considered as a measure of the distance between the experimental data
and the model explaining the data.
Let’s start by presenting the least squares in a simple way, which is useful to
remember the expression of the solution. If the noise is neglected, Eq. (2.1) becomes:

Cx = y (2.3)

26
 2 Static Estimation

But C is not invertible (because it has more rows than columns).

Let’s use the pseudo-inverse concept. In equation 2.3, multiplying each side by
C gives:
T

C T Cx = C T y
If the matrix C T .C is invertible1 then a solution is given by:
 −1
x̂ = C T C CT y (2.4)
Notes:
 −1
• The matrix C T C C T is called the pseudo-inverse of C (also called Moore–
Penrose inverse).

• This way of calculating the estimated x̂ is in general not numerically optimal.

SVD or QR decomposition have to be used instead.

Let us show that this is the solution to the least squares sense.
The criterion J(x) = (y−Cx) (y−Cx) of equation (2.2) is a real-valued quadratic
T

form since it is a scalar product.

It is a convex function which has a unique minimum.
We look for the minimum that is obtained when the gradient ∇J is zero (see the
appendix on page 42 for more information on the gradient vector).
h iT
∇J = ∂J
∂x1
... ∂J
∂xn

Derive the criterion (see the appendix on page 42):

∇J = −2C T (y − Cx)
The quadratic form has a unique minimum. Let x̂ be the vector which cancels the
gradient vector:
C T (y − Cx) = 0
C T y = C T C x̂
So  −1
x̂ = C T C C T .y

2.1.3 Weighted Least Squares

One may have to merge measures that are affected with noises of various power as
illustrated by Fig. 2.2.

1
In practice, this calculation produces a good estimate only if the matrix is well conditioned.

27
 2 Static Estimation

Telemeter 1

Telemeter 2

Best solution

Figure 2.2: Linear Weighted Least Squares example: The robot tries to localize itself
on the abscissa axis with two redundant and noisy measurements but one
measurement is more accurate than the other. The solution is more on
the right side compared to Fig. 2.1 since telemeter 1 is more accurate.

Suppose we know the covariance matrix R of the noise on the observations y 2 .

Consider the following quadratic criterion:

J(x) = (y − Cx)T R−1 (y − Cx) (2.5)

If matrix R is diagonal, we can rewrite the criterion as follows:

σ12 0 0
 

R =  0 ... 0 


0 0 σm2

1m m
rj2
J= (yj − Cx) =
2
X X
2 2
j=1 σj j=1 σj

We see that each residual is weighted by the inverse of its standard deviation
squared3 . So, the weighted least squares solution consists in finding the parameters
x that minimize the criterion of Eq. 2.5.
In Eq. 2.3, first multiplying each side by R−1 :

R−1 Cx = R−1 y

Then by C T :
C T R−1 C.x = C T R−1 y (2.6)

2
We will come back later on these concepts
3
A measure with large noise has a smaller contribution in the calculation of the solution than a
measure with low noise

28
 2 Static Estimation

If the matrix C T .R−1 .C is invertible then the solution of the weighted least squares
is given by:  −1
x̂ = C T R−1 C C T R−1 y (2.7)

Demonstration:
If the noise on y is Gaussian with R as covariance then the likelihood is:
1 T
p(y|x) = η. exp(− (y − Cx) .R−1 .(y − Cx))
2
where η is a normalizing constant.
The optimal parameter vector is the one which makes maximum the likelihood of
the observations.
The maximum of likelihood is obtained by differentiating the logarithm with respect
to x (because the logarithm is monotonous):
1 T
J = ln (p(y|x)) = ln(η) − (y − Cx) .R−1 .(y − Cx)
2
The gradient of this quadratic form is (cf appendix [Link] on page 42):
1
 
∇J = − (−2) C T R−1 (y − Cx)
2
The maximum of likelihood is sought by searching the point x̂ where the gradient
is zero:
C T R−1 (y − C x̂) = 0
C T R−1 y = C T R−1 C x̂
 −1
x̂ = C T R−1 C C T R−1 y

2.1.4 Example: combination of observations

Some authors propose to combine observations before making the estimation of the
parameter, when they are numerous in order to compress the information for an
easier transmission to a remote center for example.
Let us pose (see equation 2.6):

z = C T R−1 y

Since y = Cx (m > n), with M = C T R−1 C, we get:

z = Mx

It has to be noted that dim(z)=n and dim(M )=nxn.

We have therefore combined the measures y together to create a new vector z that
is smaller than y. Notice that z contains the same amount of information than y.
The solution with the combined measures is simply given by:

x̂ = (M )−1 z (2.8)

This is the same solution as the least squares one:

 −1
x̂ = (M )−1 z = C T R−1 C C T R−1 y

29
 2 Static Estimation

2.1.5 Robustification
The methods presented before are sensitive to outliers (measurements with large
errors).
There exists two kinds of classical methods to improve least squares in this context:

• RANSAC (Random sample consensus) [1] which was initialy proposed to solve
the Location Determination Problem (LDP), where the goal is to determine
the points in the space that project onto an image with landmarks of known
locations. It is a non-deterministic algorithm in the sense that it produces a
reasonable result only with a certain probability, with this probability increas-
ing as more iterations are allowed.

• M-estimators ("M" for "maximum likelihood-type") [10] for which an iteratively

re-weighted least squares fitting algorithm is performed.

2.2 Nonlinear Problems

2.2.1 Searching for the zeros of a function: the iterative
Newton-Raphson method
Scalar case
One seeks the solution x of the problem f (x) = 0, assuming of course that it
exists.
Let be a point x(i) close to the solution. This is an initial guess.
Consider a first order Taylor expansion of f :
′
    
f (x) ⋍ f x(i) + f x(i) x − x(i)

This allows to determine the equation of the tangent at point x(i) :

′
    
y = f x(i) + f x(i) x − x(i) (2.9)

f(x)

x(i+1)
1 x0(i)
X X

Figure 2.3: Graphical illustration

30
 2 Static Estimation

Let find the point x(i+1) where the tangent intersects the x-axis as shown in Figure
2.3.
′−1
   
x(i+1) − x(i) = −f x(i) f x(i) (2.10)

′−1
   
x(i+1) = x(i) − f x(i) f x(i) (2.11)

If we denote dx(i) = x(i+1) −x(i) , the calculation is stopped when dx(i) ≤ Threshold.
We can also consider other stop tests as
• the maximum number of iterations
 
• a test on the value of the function f x(i) to see if it is close to zero.
It should be noted that is not guaranteed to converge as shown in Figure 2.4.

f(x)

Figure 2.4: A problematic case

Finally, one can always appreciate at the end of treatment if an acceptable solution
has been found.

Vectorial case
→
−
The Newton-Raphson method also works in the case where f (x) = 0 is a vector
function, i.e. that x is a vector:
f1 (x)
   
x1
x=
 . . .  and f (x) =  . . . 
  
(2.12)
xn fn (x)
′
 
In this case, f x(i) is a Jacobian matrix4 and it will be denoted with brackets
h ′
 i
x(i) .
f
We calculate the partial derivatives with respect to each component and each
term is evaluated in point x0 :
∂f1 (x(i) ) ∂f1 (x(i) )
 
...
  
(i)
h ′
 i ∂f x  ∂x1 ∂xn 
f x(i) = = ... (2.13)
 
 
∂x 
∂fn (x(i) ) ∂fn (x(i) )

∂x1
... ∂xn
4
It can be noted that each row of the Jacobian matrix is the transpose of the gradient vector ∇f

31
 2 Static Estimation

First order Taylor expansion of f :

  
  ∂f x(i)  
f (x) ⋍ f x(i) +   x − x(i)
∂x
h ′
 i
By denoting M = f x(i−1) , we have:
 
x(i+1) = x(i) − M −1 f x(i) (2.14)
The method works in the same way but several observations can be made:
• We must invert this time the Jacobian matrix

• The stop condition has to be changed. For example, it may be considered that
the solution has been reached when dx(i) ≤ Threshold.

2.2.2 Adaptation of the method to state estimation

Consider an estimation problem with the following observation model:

y = g(x) (2.15)

where dim(y) = dim(x) = n. Please note that there is no noise.

y = g(x) is not exactly the same mathematical formalization as f (x) = 0.
The idea is to write:
y − g(x) = f (x) = 0 (2.16)
Two Jacobian matrices are interesting here :
  
∂f x(i)
M = 
∂x
  
∂g x(i)
H=  = −M
∂x
At iteration i of the Newton-Raphson method, we have:
  
x(i+1) = x(i) + H −1 y − g x(i) (2.17)

2.2.3 Non-linear least squares: Gauss-Newton method

Consider an estimation problem with the following observation model:

y = g(x) + β (2.18)

where dim(y) = m > dim(x) = n and β refers to errors.

This is a non-linear over-determined problem since we have more equations than
unknowns.
Again, we define f (x) = y − g(x).
→
−
We look for a solution to f (x) ≈ 0 in the non-linear least squares sense.

f1 (x1 , . . . , xn )
   
x1
x =  ...   ...  m>n (2.19)
   

xn fm (x1 , . . . , xn )

32
 2 Static Estimation

The Jacobian matrix is then at point x(i) :

∂f1 (x(i) ) ∂f1 (x(i) )
 
...
  
∂f x(i)  ∂x1 ∂xn 
=M = ... (2.20)
   

∂x 
∂fm (x(i) ) ∂fm (x(i) )

∂x1
... ∂xn

It is vertical as it has more rows than columns.

Resume the developments made before. Look for the new solution x(i) (cf 2.9) :
   
⃗0 = f x(i) + M. x(i+1) − x(i) (2.21)
Be careful this time since M is not invertible.
The idea is to calculate its pseudo-inverse:
   
M x(i+1) − x(i) = −f x(i) (2.22)
   
M T M x(i+1) − x(i) = −M T f x(i) (2.23)
 −1  
x(i+1) = x(i) − M T M M T f x(i) (2.24)
After convergence, we find an estimate x̂ which is the best estimate from the least
squares sense i.e. that minimizes the residuals of 2.21 at this particular point.
This iterative method is called “Gauss-Newton” . It finds the estimate that min-
imizes the residuals after convergence.

Let denote dy (i) = y − g(x(i) ) the residuals.

We have:  −1
dx(i) = − M T M .M T dy (i) (2.25)
If we use the Jacobian matrix of the observation model “g”, we get the formula
classically used in multi-sensor fusion:
 −1
dx(i) = H T H .H T dy (i) (2.26)

which can be interpreted like this: “Compute the residuals dy (i) of the current
solution and improve the solution in the state space by the increment dx(i) ”.

Notes
• Equation 2.26 is always valid even when there are exactly as many measure-
ments as unknowns. Indeed, the pseudo-inverse of an invertible matrix is the
inverse of the matrix. In this case, the method works like the Newton-Raphson
method.
• The Gauss-Newton method solves in fact an unconstrained non-linear opti-
mization problem: to find the state x such as the residuals (the difference
between the measurements and the predicted measurement knowing the cur-
rent solution and the observation model) are minimum. This is a particular
“gradient descent method” until having reach the minimum of the cost.
• Other techniques follow the “steepest descent” direction. The Levenberg-
Marquardt (LM) method [18] is very popular as it uses a search direction
that is a cross between the Gauss-Newton direction and the steepest descent
direction. The LM method is able to find a solution even if it is started far
from a minimum. It is the method that is used by the Fsolve Matlab function

33
 2 Static Estimation

x = fsolve(fun,x0)
%starts at x0 and tries to solve the equations fun(x) = 0.

• Another popular method in robotics is called BFGS (Broyden, Fletcher, Gold-

farb, Shanno) [3]. It is very fast and has a reduced computational complexity
for embedded computation.

2.3 Examples of localization problems

2.3.1 Localization of a robot with goniometric measurements
Let be a robot which measures azimuth angles on beacons of known location with
a camera (see figure 2.5).
The robot is trying to determine
  its position and heading.
x
The pose vector is X =  y .
 

i
i
i
i

Y
q

i
i

Figure 2.5: The robot in the process of measuring the azimuth angle of a beacon

The observation equation is given by:

λ = atan2 (yb − y, xb − x) − θ (2.27)

The function atan2(y, x) is a so-called "four quadrants" function. It returns a

result between −π and π. It is not to be confused with the function arctan( xy ).
If the robot measures the azimuth angles of three beacons, we have to solve the
following system:
λ1 = atan2 (yb1 − y, xb1 − x) − θ



λ = atan2 (yb2 − y, xb2 − x) − θ
 2
λ3 = atan2 (yb3 − y, xb3 − x) − θ



That we put in vector form:  

λ1
λ =  λ2 
 

λ3

34
 2 Static Estimation

λ = g (X, M )
M denotes the map that contains the coordinates of the beacons.
To solve it, we put it in the form:

λ − g (X, M ) = f (X) = 0

Every scalar function involving a particular measurement and a particular beacon

is:
gi (X, M ) = atan2 (ybi − y, xbi − x) − θ
The Jacobian matrix is given by:
 yb1 −y x−xb1 
−1
 (xb1 −x) +(yb1 −y) (xb1 −x) +(yb1 −y)
2 2 2 2
"# 
∂g  yb2 −y x−xb2
−1

H= = 
 ( b2 ) ( b2 ) (xb2 −x) +(yb2 −y)
 x −x 2 + y −y 2 2 2 
∂X y −y x−xb3

b3
−1
 
(xb3 −x) +(yb3 −y) (xb3 −x) +(yb3 −y)
2 2 2 2

%Beacons locations
xb=[0; 10;10;0 ]; yb=[0; 0 ;10;10];
%real pose (x,y,heading) of the robot
xr=[6;6;pi/4];
%Measurement generation with noise
lbd=atan2(yb-xr(2),xb-xr(1))-xr(3)+randn(length(xb),1)*1/180*pi;
%Initial guess
x=[2;3;pi/2];
%Gauss Newton iteration
for iter=1:6,
H=jacob(x,xb,yb);%Jacobian
xnew=x+H\dy(x,lbd,xb,yb);%left division (pseudo-inverse)
if (norm(x-xnew)<1e-4),break,end;
x=xnew;
end;
%Residuals computation
function out=dy(x,lbd,xb,yb)
out=lbd-(atan2(yb-x(2),xb-x(1))-x(3));
end
%Jacobian computation
function H=jacob(x,xb,yb)
for i=1:length(xb),
H(i,:)=jacob_line(x,xb(i),yb(i));
end;
end
function line=jacob_line(x,xb,yb)
line=[(yb-x(2))/((xb-x(1))^2+(yb-x(2))^2),...
(x(1)-xb)/((xb-x(1))^2+(yb-x(2))^2),-1];
end

Figure 2.6: Matlab script that solves the problem in simulation. This algorithm
works if there are at least as many measurements as unknowns. In par-
ticular, if there are exactly as many measurements as unknowns, it im-
plements a Newton-Raphson method (and not a Gauss-Newton method).

35
 2 Static Estimation

Initializing the program with x=[2;3;pi/2], Figure 2.7 shows the convergence of
the method in 6 iterations.

12

10

-2
-2 0 2 4 6 8 10 12

Figure 2.7: Solving the problem of the robot (the real pose is red while the successive
estimates are in green).

Other initialization values as x=[0;2;%pi/2] make the method diverge (Fig-

ure 2.8).

2.5e+006

2.0e+006

1.5e+006

1.0e+006

5.0e+005

0.0e+000

-5.0e+005
-5.0e+006 0.0e+000 5.0e+006 1.0e+007 1.5e+007 2.0e+007 2.5e+007

Figure 2.8: A case where the method gives an incorrect result.

Finally, we note that the program works well with four beacons, which corresponds
to an overdetermined problem (Figure 2.9).

36
 2 Static Estimation

12

10

-2
-2 0 2 4 6 8 10 12

Figure 2.9: The method works with 4 beacons.

We can use this simulation program to run 1000 trials from the same initial esti-
mate but each simulation has different measurement noises. The result is obtained
in Figure 2.10 on which we can see an inaccuracy area that is characteristic of an
ellipse. We will come back on this concept later in this course.

Figure 2.10: Result of 1000 executions of the solver with each time a different noise.

37
 2 Static Estimation

2.3.2 Computation of a GPS fix

[Link] Observation equation associated with the C/A code
Principle The propagation time of the wave is measured knowing that one knows
the time of transmission (in the GPS system time) .
The pseudo-random sequences generated by the satellites are time dependent. As
the C/A code is known, the receiver can locally generate the same sequences as
those of the satellites.
Thus, the GPS receiver generates a replica at any time of the pseudo-random codes
of the different satellites in view; the signals thus generated are compared with the
received ones. Now the satellites have orthogonal codes, ie their cross-correlation
is zero. Thus, when a received signal and a replicated signal are correlated, the
receiver has detected a satellite.
In practice, the calculation of the correlation is more difficult because
1. The signal is very noisy
2. there is frequency offset due to the high velocity of the satellites (Doppler
effect).
Consider a particular satellite.
If the clocks of the satellite and of the receiver would be perfect, the time of flight
would be simply △t = tr − te.

Real case In practice, neither the satellite nor the receiver are perfectly synchro-
nized with the GPS time: they both have an offset dtu and dts with respect to the
GPS time.
Notations: s = satellite i ; u = receiver (user).
• tu (t) = t + dtu (t) (time in the time scale of the satellite)
• ts (t) = t + dts (t) (neglecting relativistic effects)
We note
1. ts (te ) the date on which the sequence has been issued in the time scale of the
satellite i,
2. tu (tr ) the date on which the sequence was received in the time scale of the
receiver.
The receiver matches the two sequences to obtain a measurement of the time of
flight, denoted △tm ("m" means measured).
△tm = tu (tr ) − ts (te )
Since tu (tr ) = tr + dtu (tr ) and ts (te ) = te + dts (te )
△tm = tr − te + dtu (tr ) − dts (te )

It is assumed that the wave signals propagate at the speed of light (denoted by
c).
The pseudo-range is denoted ρ (in meters) between the satellite and the receiver
is given by the relationship:
ρ = c.△tm
= c(tr − te ) + [Link] (tr ) − [Link] (te )
= R + [Link] (tr ) − [Link] (te )
where R is the geometric range between the satellite and the receiver.

38
 2 Static Estimation

Phenomena degrading the code measurement

Effect of the ionosphere I Between 50 and a few hundred kilometers above, the
ionosphere is a high layer of the atmosphere ionized by the action of solar radiation.
It depends on the latitude, solar activity, time of year, day and night.
When the signal wave crosses this medium, everything happens as if the mea-
sured distance (in pseudo-range measure) was longer than the true geometric range
receiver-satellite. This is due to the fact that the electromagnetic waves passing
through the medium are delayed.
The effect on the measurement is an elongation I that may vary from 0 to 50 m.

Effect of the troposphereT Between the ground and 8-17 kilometers above the
troposphere is the lowest layer of the atmosphere.
It depends on the temperature, pressure, humidity and the elevation of the satel-
lite, but not in frequency. One has to use models to reduce it.
Signal delays are due to two phenomena:

• The path of the waves passing through this medium is curved path length is
increased,

• The propagation speed decreases.

The effect on the measurement can vary from 2 to 30 m according to elevation of

the satellite.

Relativistic effects Rel Relativistic effects are due to the fact that there is a
difference in the gravitational field and especially a big difference in speed between
the satellite and the receiver. This results in an expansion of time and frequency
offset. Thus, will be modified:

• The clock offset satellite / GPS time

• The clock offset of the receiver / GPS time

• Satellite/receiver signal bends and delays due to Earth’s gravity.

Multipath M The presence of obstacles (buildings, vegetation, ...) causes signal

reflections and attenuates it. This results in delays, the path being longer.
This phenomenon cannot be modeled as two measurements at different time in-
tervals do not give the same result. This effect depends only on the geometry of the
obstacle.
The effect on the measurements can be several tens of meters (often 10 to 20 m)
for the pseudo-ranges.
To reduce this, one can:
- Improve to design of the antenna,
- Place the stations in places in open sky areas.
Note that the satellites located vertically above the receiver will be less sensitive
to multipath.
This phenomenon is particularly problematic for the calculations in real time with
a short-term observation, as is the case for a mobile station. Moreover, the satellite
moving quickly, a multipath on a satellite does not last more than a few minutes.

39
 2 Static Estimation

Noise measurements b The signal from the satellite being highly attenuated,
the receiver is forced to increase. Unfortunately the noise is also amplified. It is
considered that the noise of the measurement error causes a minimum of 1% of the
wavelength of the signal on which the measurement is made; for example, the C/A
code of wave length being of 300m this induces an error of 1 to 3 meters. This
noise affects the measurement of the time delay and therefore the calculation of the
pseudo-range.

Complete expression of the code measurement Taking into account the various
degradation, we obtained

ρ = R + [Link] − [Link] + Rel + T + I + M + b

Navigation message: GPS data The data message is transmitted (on L1 and L2)
at a rate of 50 bits/sec and provides information such as the health of the satellite,
its identification, ephemeris (orbit), its clock correction ionospheric correction ...
The complete message, 1500 bits long, is transmitted in 30 seconds but is frag-
mented into 5 portions of 300 bits long. Each portion contains 10 words of 30 bits
each.
The first two words of each section are identical; They contain general information
such as the number of satellite, its condition and the number of the GPS week.
Portion 1 shows the parameters of the satellite clock.
Portions 2 and 3 provide the satellite ephemeris containing the precise orbit of
the satellite, the clock correction data and ionospheric propagation.
Portion 4 provides correction settings ionospheric and tropospheric corrections.
Portion 5 provides satellite almanac. Almanacs contain less information than the
satellite ephemeris but give information on the other satellites. It is for this reason
that the entire message (1500 bit 30 sec) is repeated 25 times with different almanacs
(nevertheless, the 1-3 portions remain unchanged). Transmission of all the almanacs
lasts 25 * 30sec = 12.5 min.

[Link] Calculation of the position with the code measurements

One speaks in this case of GPS standalone positioning. One has to calculate the
position (x, y, z) of the receiver in the reference GPS (WGS-84 ECEF).

Compensation of the error terms Thanks to the navigation message, it can be

estimated:

1. The offset of the satellite clock dtˆs (te )

2. The ionospheric elongation Iˆ

3. The tropospheric elongation T̂

ˆ
4. The relativistic errors Rel.

These corrections are applied and we get corrected pseudoranges:

ρ = R + [Link] (tr ) − c.(dts (te ) − dtˆs (te )) + Rel − Rel

ˆ + T − T̂ + I − Iˆ + M + b

ρ = R + [Link] (tr ) + M + b + residaul errors

40
 2 Static Estimation

Getting equations by neglecting errors The observation model is:

ρ = R + [Link] (tr ) + errors

To apply Newton’s method, we neglect the errors:

ρ = R + [Link] (tr )

Suppose that at a given moment, m satellites are visible and it is assumed m>4.
We proceed at a reception time noted tr . The satellites are at different distances
and thus the sending instants te,i of their C/A codes are different from each other.
Because the satellites broadcast orbits parameters to rebuild their position, we de-
note [xi (te,i ), yi (te,i ), zi (te,i )] T their coordinates at the emission times (and they are
assumed to be known without error).

d(tr ) = [Link] (tr )

The system to be solved is:

 q




ρ1 (tr ) = (x(tr ) − x1 (te,1 ))2 + (y(tr ) − y1 (te,1 ))2 + (z(tr ) − z1 (te,1 ))2 + d(tr )

...




 q

ρi (tr ) = (x(tr ) − xi (te,i ))2 + (y(tr ) − yi (te,i ))2 + (z(tr ) − zi (te,i ))2 + d(tr )

...




 q
ρ (t ) = (x(t ) − x (t ))2 + (y(t ) − y (t ))2 + (z(t ) − z (t ))2 + d(t )


m r r m e,m r m e,m r m e,m r

By noting Xtr = [x(tr ), y(tr ), z(tr ), d(tr )] T an d ρtr = [ρ1 (tr ), ..., ρm (tr )] T , ob-
tained in vector form:

ρtr = g(Xtr , M )
where M contains the positions of the satellites at their emission time expressed
in the ECEF frame at the reception time.
With m >4, we are led to solve a nonlinear redundant system. This is a problem
of multi-sensor static fusion that we known how to solve.

41
 2 Static Estimation

Appendix: Gradient vector

Let f (x) be a real-valued function of a vector x.
By convention, the gradient vector of f at a particular point x is a n × 1 vector.
h i h iT
∇f (x) = ∂f
∂x
(x) = ∂f (x)
∂x1
... ∂f (x)
∂xn

The gradient is often displayed by a vector field as shown in Figure 2.11.

The gradient points to the direction of greatest increase. Keeping following the
gradient and one will reach a local maximum.

Figure 2.11: Surface of function z = 10x1 exp(−x21 − x22 ) − 1 and vector field of its
gradient.

Figure 2.12: Bird view of the vector field of the gradient.

Appendix: Gradient vector of quadratic forms

First, consider a simple case. Let consider a real-valued function that takes a vector
as input:
f (x) = xT .M.x (2.28)
If dim(x)=n, then M is a n × n matrix. It is assumed here that M is a real,
symmetric and positive definite5 matrix. So that the real number xT .M.x is actually
positive.
5
Its eigenvalues are positive

42
 2 Static Estimation

The derivative of f with respect to x gives the gradient vector:

∇f (x) = 2.M.x

Example
2 1
" #" #
h i x1
f (x) = x1 x2 = 2x21 + 2x1 x2 + x22
1 1 x2

4 2 4x1 2x2
" #" # " #
x1
∇f (x) = =
2 2 x2 2x1 2x2

We can recognize the expression with the partial derivatives:

 
∂f (x)
∇f (x) =  ∂x1
∂f (x)

∂x2
Replace now x by C.x in (2.28):

f (x) = (Cx)T .M.C.x = xT .C T .M.C.x

The matrix C T .M.C is still real, symmetric and positive definite, so,

∇f (x) = 2.C T .M.C.x

Replace now x by y − C.x in (2.28) to obtain a well-known form:

f (x) = (y − Cx)T .M.(y − C.x)

= y T .M.(y − C.x) − (Cx)T .M.(y − C.x)

= y T .M.y − y T M.C.x − (Cx)T .M.y + (Cx)T .M.C.x
= y T .M.y − y T M.C.x − xT .C T .M.y + xT .C T .M.C.x
Yet, y T M.C.x is a scalar. Take its transpose does not change its value:

(y T M.C.x)T = xT .C T .M.y

We are thus led to derive:

f (x) = y T .M.y − [Link] .C T .M.y + xT C T .M.C.x

The first term does not depend on x, the second one is a linear form 6 and the third
one a quadratic form.

∇f (x) = −2.C T .M.y + 2.C T .M.C.x

∇f (x) = −2.C T .M.(y − Cx)

6
If f (x) = xT a is a linear form then its gradient vector is ∇f = a

43
3 Dynamic state estimation for
linear systems
One addresses a problem of state estimation, as soon as the system is dynamic, that
is to say that one has a model of the evolution of the state vector.
A state estimator is also called a state observer.

3.1 Reminders on deterministic state observation

State observation is said deterministic, if there is no random phenomenon.
Considering a linear system without direct transmission of the input to the output:

xk+1 = [Link] + [Link]

(

yk = [Link]
with dim(x)=n and dim(y)=m.

3.1.1 Observability of the state

Definition
A system is said to be "state observable" if by observing the inputs and outputs
on a finite time interval, one can determine the initial state.
Indeed, knowing the initial state and the input, one can reconstruct the state at
any time through the evolution model.
_________________________________________________
Rank condition for a linear system
A system is observable if its observability matrix O has rank n, where n is the
size of the state.
_________________________________________________
The observability is a structural property of the system.
If one can show that the state is observable, one can reconstruct all the components
of the state even if some are not measured.
Before applying a multi-sensor fusion technique based on the formalism of state
representation, it is desirable to check the observability of the state. If the state is
not observable, then the operation of the observer may be unstable or will estimate
the observer will have no practical meaning.
But beware: this is not because the state is observable that any observer will
converge. Indeed, the study of observability neglects sources of error (model errors,
disturbances and noise).

3.1.2 Dynamic observation

In the deterministic context, a state observer is sometimes called "reconstructor".
We are dealing with "dynamic" observers (with infinite impulse response).

44
 3 Dynamic State Estimation

Problem It is achievable to determine a stable and linear system such that:

lim (x̂(t) − x(t)) → 0

t→∞

u(t) y(t)
System
with state x(t)

x(t)
State Observer

Figure 3.1: State observation of a dynamical system. It can be noted that the ob-
server has two inputs.

General solution For the system:

xk+1 = [Link] + [Link]

(

yk = [Link]
We propose a dynamic observer of the type:

x̂k+1 = Ax̂k + Buk + K · (yk − C x̂k ) = (A − KC) x̂k + Buk + Kyk

Estimation error: ek = x̂k − xk

ek+1 = (A − KC) x̂k + Buk + Kyk − (Axk + Buk )

= (A − KC) x̂k + KCxk − Axk
= (A − KC) .ek
The error tends to 0 if the matrix (A − KC) is a stability matrix (whose eigen-
values are less than 1 in module in the discrete case).
In practice, the problem is to find a "good value" for the gain K. A high gain K
make rapid the observer but sensitive to disturbance.

x(t)

xˆ (t )

Figure 3.2: A convergent state observer

45
 3 Dynamic State Estimation

3.1.3 Limitations of deterministic observation

The deterministic state observation has several drawbacks:

• It does not exploit statistical knowledge of the measurement errors made by

the sensors,

• It is well known that any equation and model is inaccurate,

• It is not possible to give any confidence indicator associated with the estimate.
However, in many applications, confidence indicators are very important.

For these reasons, we are interested in the following by focusing on probabilistic

approaches that address this problem by considering the uncertainties as random
phenomena.

46
 3 Dynamic State Estimation

3.2 Some elements on random signals theory

3.2.1 Variables and random vectors
[Link] Random variables of the second order
A random variable is a real variable whose value depends on randomness. Rigor-
ously, this dependence is expressed by a probability distribution function, but in
this chapter we limit the statistical study to the second order that is to say, we
consider that a random variable is characterized by its centered statistical moments
of the first and second order (mean and covariance). This way of studying random
variables is generally incomplete, except in the Gaussian case, where the probability
density function is known as soon as the mean and the variance are known.

[Link] Centered statistical moments

Mean
The statistical mean is also called the mathematical expectation:
mx = E(x)
Where, p(x) is the probability density function and S the support on which the
random variable is defined.
The mathematical expectation is defined as:
Z
E(x) = α.p(α)dα
S
Generally, the operator "expectation" applies to any function ϕ of a random vari-
able:
Z
E(ϕ(x)) = ϕ(α).p(α)dα
S
One sees immediately that this is a linear form, thus:

E(a.x + c) = aE(x) + c
since p(x)dx = 1.
R
S

Variance
The variance is also called mean square deviation or central moment of order two:

Px = E((x − mx )2 )
It is easily shown that Px = E(x2 ) − E(x)2

Standard deviation

σx2 = Px
It characterizes the dispersion around the mean value.

47
 3 Dynamic State Estimation

[Link] Random Vector

Couple of random variables
A couple of random variable is a random vector of dimension 2.
The cross-centered moment, also called covariance is defined by:

Pxy = E((x − mx )(y − my ))

Note: E(xy) is the correlation (cross non-centered moment) less used.

The correlation coefficient is defined by:
Pxy
ρ=
σx σy
The correlation coefficient 1 reflects the similarity between two random variables.
If the module is equal to one, we say that the variables are fully correlated.
If one has a measurement ym of y, we obtain a new random variable "x know-
ing that y=ym " denoted “x|y=ym ”. Two variables are independent if (x|y=ym )=x
whatever y (a knowledge about y provides no information on x).
Note: two independent variables are uncorrelated. The converse is generally false
except in the particular Gaussian case.
Property: for two independent variables we have E(x.y)=E(x).E(y).
Examples:

1. 2 constant variables (1 and 2)

2. Two 6-sided dices (E(x)=E(y)=3,5 and the average product of all possible
combinations gives 3,52 ).

Random vectors
We generalize the dimension n, the concepts of the preceding paragraph.
Its mean is the mean vector of the components:
h iT
E(x) = mx = mx1 mx2 . . . mxn
We call covariance matrix2 :
Px = E[(x − mx )(x − mx )T ]
 
Px1 x1 Px1 x2 ...
Px =  Px2 x1 Px2 x2 ... 


... . . . P xn xn
 
Px1 Px1 x2 . . .
Px =  Px1 x2 Px2 . . . 


... . . . P xn
Note: xT .x is a scalar product, but [Link] is a matrix (n.n).
The Pxi xj are the covariances between the components of x.
Px is square and symmetric.

1
Strictly speaking, this coefficient should have been called covariance coefficient.
2
also known as variance-covariance matrix.

48
 3 Dynamic State Estimation

Cross-covariance matrix
The covariance matrix of a vector reflects in particular the correlations between
components of this vector. The cross-covariance matrix of two vectors reflects the
correlations between the components of the two vectors.
Let be x and y two vectors not necessarily of the same size:
h iT h iT
x = x1 x2 . . . xn and y = y1 y2 . . . yp
The cross-covariance matrix is:

Pxy = E[(x − mx )(y − my )T ]

 
Px1 y1 Px1 y2 . . .
Pxy =  Px2 y1 Px2 y2 . . . 
 

... . . . P xn yp
Pxy est (n.p). It is not square (so neither symmetric).

Pxy = Pyx
T

[Link] Operations on random vectors

Sum of two vectors
Let x and y be two vectors of the same dimension.
Let z=x+y.

Mean
mz =mx +my

Covariance matrix
Pz =Px +Py +Pxy +Pyx
Where Pxy and Pyx are square cross-covariance matrices here because x and y are
of the same size.

Proof:
Pz =E{(x + y − mx − my ).(x + y − mx − my )T }
= E{(x − mx + y − my ).(x − mx + y − my )T }
By developing:
Pz =E{(x − mx ).(x − mx )T }+E{(x − mx ).(y − my )T }
+E{(y − my ).(x − mx )T }+ E{(y − my ).(y − my )T }
= Px +Py +Pxy +Pyx

In the particular case where the vectors are uncorrelated:

P z = Px + P y

Difference of two vectors

Let x and y be two vectors of the same dimension.
Let z=x − y.

Mean
mz = mx − my

49
 3 Dynamic State Estimation

Covariance matrix
Pz = Px + Py − Pxy − Pyx

Proof:
Pz =E{(x − y − mx + my ).(x − y − mx + my )T }
= E{(x − mx − (y − my )).(x − mx − (y − my ))T }
=E{(x − mx ).(x − mx )T }-E{(x − mx ).(y − my )T }
-E{(y − my ).(x − mx )T }+ E{(y − my ).(y − my )T }
= Pz =Px +Py -Pxy -Pyx

In the particular case where the vectors are uncorrelated:

Pz = Px + P y

Affine transformation of a vector

Let y = M.x + b where M is a deterministic matrix not necessarily square and b
a constant vector.

Mean
my = [Link] + b

Covariance matrix
Py = [Link] .M T

Proof:
Py =E{y − my ).(y − my )T }
= E{(M.x + b − [Link] − b).(M.x + b − [Link] − b)T }
= E{M.(x − mx ).(x − mx )T .M T }
= M .E{(x − mx ).(x − mx )T }.M T

Generalization: linear combination of uncorrelated vectors

Let z=A.x+B.y where A and B are deterministic matrices not necessarily square.

Mean
mz =[Link] + [Link]

Covariance matrix
Pz = [Link] .AT + [Link] .B T

50
 3 Dynamic State Estimation

3.2.2 Random signals

[Link] Second order modeling of a random signal
A random signal is a function of time and randomness. It is characterized by:

1. Statistical properties which are also called "spatial distribution"

2. Temporal or frequential properties.

Indeed, the spatial distribution is not sufficient to characterize a signal.

In the figure below, the two signals have the same spatial distribution (same
variance), but one is less "nervous" than the other.

2 2

1 .5 1 .5

1 1

0 .5 0 .5

0 0

- 0 .5 -0 .5

-1 -1

- 1 .5 -1 .5

-2 -2
0 10 20 30 40 50 60 70 80 90 100 0 10 20 30 40 50 60 70 80 90 100
t t

Figure 3.3: Two random signals with the same variance

The auto-correlation function Ψxx characterizes the temporal aspects (slow vari-
ations, fast ...). It is defined as:

Ψxx (t, τ ) = E {[x(t) − E(x(t)] . [x(t + τ ) − E(x(t + τ )]}

Note: Ψxx (t, 0) = Var(x(t))
The auto-correlation function therefore characterizes the variance.
Thus, a random signal is characterized at the second order by:

1. Its statistical mean,

2. Its auto-correlation function.

[Link] Stationarity
By definition, x(t) is stationary if its spatial and temporal characteristics do not
depend on time:

1. E[x(t)]=cste

2. Ψxx (t, τ )=Ψxx (τ ) and thus Var[x(t)]=cste=σx2

This is the steady state, that is to say that we "forgot" initial conditions.
The figure below shows two signals that are obviously non-stationary because
for one, the variance increases with time and the other has a correlation function
depending on the time, since his "nervousness" increases.

51
 3 Dynamic State Estimation

200 2 .5

2
150
1 .5

100 1

0 .5
50
0

0 -0 .5

-1
-5 0
-1 .5

-1 0 0 -2
0 10 20 30 40 50 60 70 80 09 0 1100 0 20 30 40 50 60 70 80 90 100
t t

Figure 3.4: Two non-stationary random signals

[Link] Ergodicity
An ergodic signal has its temporal moments that are identical to its statistical
moments which means, in other words, that a realization of an ergodic signal of
infinite duration contains the same information as an infinite number of realizations
of finite duration.
Consider for example a discrete signal xk . Under assumption of ergodicity, its sta-
tistical average can be calculated by computing the temporal mean and the variance
of a single infinitely long realization by:
lim 1X n
( )
mx = E(xk ) = xk
n→∞ n k=0

lim 1X n
( )
V ar(xk ) = (xk − mx )2
n→∞ n k=0
The discrete auto-correlation is given by:

lim 1 n−h
( )
Ψxx (h) = (xk − mx ) · (xk+h − mx )
X
n→∞ n k=0
In practice, the sequence is not of infinite size. In this case, we calculate estimates3 :
1X n
m̂x = xk
n k=0
1 n−h
Ψ̂xx (h) = (xk − m̂x ) · (xk+h − m̂x ) f or 0 < h < n − 1
X
n − h k=0
The auto-correlation h<0 is deduced from the positive part by symmetry.

Example: Let’s consider gyrometers that measure angular velocities. To properly

characterize the signal, it would be necessary to take a large number of them (e.g.
100), record them for a few seconds and calculate the statistical values on these
data. This can be very difficult to do. If now the signals produced by the gyros are
ergodic, it is enough to take only one gyro and record it for a long time. It is then
possible to easily calculate the average and auto-correlation.
We can verify that the hypothesis of ergodicity is valid by doing the same manip-
ulation with two different gyros. The results should be almost the same.
3
These formulas provide estimates called "biased."

52
 3 Dynamic State Estimation

[Link] White Noise

Continuous white noise
Let B(t) be a white noise.
Its auto-correlation function Ψ(t) is a Dirac delta function4 . A white noise is
a stationary signal since its auto-correlation function does not depend on time. In
addition, knowledge of B(t) provides no information on B(t + τ ): thus this is a
completely uncorrelated signal.
White noise contains all frequencies at the same power, so it has a generic char-
acter, ie it allows to synthesize any frequency by convolution. Its adjective "white"
comes from the analogy with white light that can synthesize any color with filters.
V ar[B(t)] = Ψ(0) = ∞
Physically, such a signal does not exist!

Discrete white noise

Let wk be a white discrete sequence of weight q.

We have:
E[wk .wl ] = 0 if k̸=l
= q if k=l

In particular, E[wk2 ] = var(wk ) = q.

The auto-correlation function has a Kronecker delta (also called pseudo-Dirac
delta) at the origin.

Figure 3.5: Auto-correlation of a discrete white noise

4
The Dirac delta is a real function which is zero everywhere except at the origin, where it is
infinite: (
+∞, x = 0
δ(x) =
0, x ̸= 0
R∞
and which is also constrained to satisfy the identity −∞ δ(x) dx = 1.
R∞
It has also the property −∞ f (x)δ(x) dx = f (0)

53
 3 Dynamic State Estimation

3.3 Uncertainty ellipsoid

In this section, we present several useful formulas to obtain a covariance matrix from
an uncertainty ellipse 5 and vice versa.
For the sake of convenience, assume that the random phenomena are Gaussian.
This assumption is valid often in practice if the random phenomenon under consid-
eration is the conjunction of several random phenomena. This is the central limit
theorem: any sum of independent and identically distributed random variables tends
to a Gaussian random variable.

3.3.1 Equiprobability contour

Consider a Gaussian random vector x = [x1 , . . . , xn ]T with the mean mx and covari-
ance matrix P .
Its multivariate Gaussian joint probability density is written in the form:
1 1
 
p(x) = q · exp − (x − mx )T P −1 (x − mx ) (3.1)
(2π)n . det(P ) 2
This expression is used to define the contour of equiprobability which defines the
domain centered around the estimate in which the true state is located for a given
probability P r.
Consider an equiprobability cutting of the probability density:

(x − mx )T P −1 (x − mx ) = k 2

where k is homogeneous to a number of standard deviations.

As P is a positive definite matrix, the contour of equal probability is an ellipsoid6
in Rn , called "probability ellipsoid".
The probability Pr associated to the ellipsoid is linked to the factor k and depends
on the size of x.
For a Gaussian a relationship can be established mathematically. For instance,
for n = 1, the probability is defined as:

1
h i
Z mx +kσ (x−mx )2
−
Pr = √ ·e 2σ 2

mx −kσ 2π · σ
In this case, for a given value k, we have:
1
Pr = − √ + 2Erf(k)
2π
with Erf(z) = exp(−t2 )dt
Rz
√2
π 0

3.3.2 Ellipse of a Gaussian of dimension two

We consider the random vector [x, y]T of mean [mx , my ]T . Its covariance can written
as (see chapter 3):
" #
σx2 ρσx σy
P =
ρσx σy σy2
5
This is the problem that typically arises to convert the ellipse of inaccuracy of the GPS NMEA
GST frame into a covariance.
6
Viet theorem on conic

54
 3 Dynamic State Estimation

Its inverse is given by:

1
" #
σy2 −ρσx σy
P −1
= 2 2 (3.2)
σx σy (1 − ρ2 ) −ρσx σy σx2
The probability density function is then expressed as:

1 1 (x − mx )2 2ρ(x − mx )(y − my ) (y − my )2
( " #)
p(x, y) = √ exp − − +
2πσx σy 1 − ρ2 2(1 − ρ2 ) σx2 σx σy σy2

It is displayed in Fig. 3.6.

Figure 3.6: Probability density of a Gaussian in the two dimensional case.

Figure 3.7: The bird view of a 2D Gaussian makes ellipses appear.

In 2D (n = 2), the relationship between the probability Pr and the factor k is

given by:
!
−k 2
Pr = 1 − exp (3.3)
2

55
 3 Dynamic State Estimation

Pr k
40% 1
87% 2
99% 3
99.97% 4

Table 3.1: Special values in 2D. Note that in 1D, a 1-sigma domain corresponds to
a probability of 68% which is significantly different to the 2D case.

It can be noticed on Fig. 3.8 that the evolution of k versus Pr is quite linear
up to 99% but for higher probabilities it becomes highly exponential. For high
probabilities, a small estimation error on the Gaussian will propagate strongly. In
other words, no Gaussian should be used in this case.

5.5

4.5
k value

3.5
X 0.9889
Y 3.001
3

2.5

2
0.95 0.96 0.97 0.98 0.99 1
Probability

Figure 3.8: Evolution of the factor k depending on the probability in 2D.

For each value of k the contour of the probability ellipse is written in the form:

(x − mx )2 2ρ(x − mx )(y − my ) (y − my )2
− + = k2 (3.4)
σx2 (1 − ρ2 ) σx σy (1 − ρ2 ) σy2 (1 − ρ2 )
Figure 3.9 indicates particular values that allows to see how the ellipse changes
with respect to the correlation coefficient ρ.

y
kx
ky

ky 1  
2

ky

my
kx
mx x
kx 1   2

Figure 3.9: Ellipse associated to mx , my , σx , σy , ρ and k.

56
 3 Dynamic State Estimation

3.3.3 Circle encompassing an ellipse

In some applications, it is preferred to characterize the circle encompassing the
ellipse as uncertainty is characterized in a simple way in this case by a scalar (so it
can be easily thresholded).
To achieve this, simply select the circle of radius equal to the major axis of the
ellipse, that is to say the smallest eigenvalue λP −1 ,min of P −1 :
v
k2
u
a=
u
t
λP −1 ,min
This eigenvalue is nothing other than the largest eigenvalue of P :
1
λP,max =
λP −1 ,min
It is possible
" to calculate
# an analytical expression of this eigenvalue.
2 2
σx σxy
If P = 2 then
σxy σy2
v
!2
σx2 + σy2 ut σx − σy
2 2
u
λP,max = + + σxy
2
2 2

Therefore, the radius of the circle is obtained as follows:

q q
a= k 2 · λP,max = k · λP,max
It depends on the chosen number k of standard deviations.

3.4 Second order state estimation

In the stochastic context, a state observer is often called "filter".

3.4.1 Problems considered

Consider the simplified state space to simplify calculations:

xk+1 = [Link] + [Link] + αk

(

yk = [Link] + βk
Where dim(xk )=n dim(yk )=m and dim(uk )=p.
Noises αk (model noise) and βk (observation noise) are supposed centered which
means that the equations of the state space are on average accurate (no bias).

E(αk ) = 0 E(βk ) = 0

Then it is assumed that their respective covariance matrices Q and R are known
(and constant to ease equations).

var(αk ) = Q var(βk ) = R

57
 3 Dynamic State Estimation

More they are assumed to be white. Finally, we assume that they are uncorre-
lated7 :
E(αk .βkT ) = 0
The problem is to find a good estimate of the state xk given observations yk from
the initial time to the instant l. The estimate of xk is denoted x̂k|l that is the
estimate of xk knowing everything that happened until the instant l.
The covariance matrix of the estimation error will be noted:
   T 
Pk|l = E x̂k|l − xk · x̂k|l − xk

Depending on the values of k and l, there are three categories of problems:

1. Smoothing l>k,

2. Filtering l = k,

3. Prediction l<k.

In real-time applications, it is most often prediction and filtering problems.

Notations:

• x̂k|k updated estimate

• ek|k = x̂k|k − xk estimation error

• Pk|k covariance matrix of the estimate error

• x̂k+1|k (one-step) prediction

• ek+1|k = x̂k+1|k − xk+1 prediction error

• Pk+1|k covariance matrix of the prediction error

3.4.2 Estimator/Predictor observer type

[Link] Structure
We consider observers in the form of
 
= x̂k|k−1 + K. yk − C.x̂k|k−1
(
x̂k|k
x̂k+1|k = A.x̂k|k + [Link]
Where K is a gain matrix to define and not necessarily constant.
It is essential to note the recursive structure of such an observer. In this way, it
doesn’t have to memorize the inputs {u1 ,...,uk−1 } and the observations {y1 ,... ,yk−1 }.
Processing can be done with just uk and yk .
At a given instant k, the output of the observer is x̂k|k but the structure "Estima-
tor/Predictor" is very important in real time. Let’s see why.
In theory, a discrete-time observer provides its results at the same time k where
the observations are available.

7
or independent because we stop at the second order.

58
 3 Dynamic State Estimation

k k+1 t

x̂ k / k x̂ k 1 / k 1

Figure 3.10: Ideal state observer from a temporal point of view

In practice, this is impossible because the computer must 1) recover the observa-
tions, 2) make processing, 3) provide a result.
Thus, the prediction step is done in anticipation for the next sampling instant.
The delay of the estimate is thus minimized.
acquisition

prediction
estimation
output

k k+1 t

x̂ k / k

Figure 3.11: Optimized implementation (from a temporal point of view) of a state

observer

[Link] Recurrence of estimation and prediction errors

We have :
ek|k = x̂k|k − xk  
= x̂k|k−1 + K yk − C x̂k|k−1 − xk
 
= x̂k|k−1 + K Cxk + βk − C x̂k|k−1 − xk
   
= x̂k|k−1 − xk + K −C x̂k|k−1 − xk + βk
= (I − KC) ek|k−1 + Kβk

ek+1|k = x̂k+1|k − xk+1

= Ax̂k|k − Axk − αk
= Aek|k − αk

[Link] Recurrence of covariance matrices

Recurrence giving Pk|k knowing Pk|k−1

59
 3 Dynamic State Estimation

We have seen that

ek|k = (I − KC) ek|k−1 + Kβk
Yet, ek|k−1 = Aek−1|k−1 − αk−1 is a prediction using observations until the time
instant k − 1. It is affected by a model error at time k − 1 also.
So, as βk is a white noise, βk and ek|k−1 are uncorrelated8 .
We apply the results for the linear combination of uncorrelated random vectors
and therefore:

Pk|k = (I − KC) · Pk|k−1 · (I − KC)T + K · R · K T (3.5)

Recurrence numerically well conditioned known as a "Joseph’s form".

Note: n o
We have not assumed here that E ek|k−1 = 0, i.e. an unbiased prediction.

Recurrence giving Pk+1|k knowing Pk|k

We have seen that

ek+1|k = Aek|k − αk = A (I − KC) ek|k−1 + AKβk − αk

With the same reasoning, ek|k−1 is uncorrelated of the white noises.

Now, by hypothesis, βk est uncorrelated of αk and so:

Pk+1|k = A · Pk|k · AT + Q (3.6)

3.5 Optimal linear estimation

Let be two random vectors x and y which can be vectors of different sizes. We
have a prior knowledge about x : its mean mx (mathematical expectation) and its
covariance matrix Px are known. We have an observation ym (or a measurement)
and we know a model that allows us to go from x to y.
If the model is y = Cx + β where β is a centered uncorrelated variable with
covariance matrix R, then we know the first and second statistical moments of the
predicted observation:

m
y = Cmx
(3.7)
 Py = CPx C T + R
Let us suppose that the two random variable are correlate: Pxy ̸= 09 . This means
that a knowledge on y (which is the observation ym ) provides some information on
x.
We seek an estimate x̂ knowing an observation ym . We look for it in a linear form:

x̂ = M ym + b (3.8)
Where M and b have to be determined.
Figure 3.12 displays how the problem can be illustrated.
8
Generally speaking, any expectation of a white noise with errors or estimates made at the
previous sampling time will be zero (this property will be used many times thereafter).
9
We will see that the cross-covariance Pxy is given by Pxy = Px C T

60
 3 Dynamic State Estimation

Px
x2 y2 Py
mx
^x my

ym Observation

x1 y1

Figure 3.12: Graphic illustration of the different steps used in linear estimation

Unbiased estimate
Let seek first an estimate with zero-mean error (called unbiased):

E(x̂ − x) = [Link] + b − mx = 0

b = mx − [Link]
So, equation 3.8 becomes:

x̂ = M ym + mx − [Link] = mx + M (ym − my )

Minimum variance estimate

It now remains to determine matrix M . Let us show that the solution that minimizes
the variance of the estimation error is:

M = Pxy .Py−1

Proof: Let us show that any other solution is less optimal.

Since we are doing linear estimation, any other solution would be written as:

M = Pxy .Py−1 + L
where L is a real matrix of any value.

x̂ = mx + (Pxy .Py−1 + L).(y − my )

x̂ = mx + Pxy .Py−1 .(y − my ) + L.(y − my )

x̂ − x = −(x − mx ) + Pxy .Py−1 .(y − my ) + L.(y − my )

 
Compute P = E (x̂ − x).(x̂ − x)T
 
P = E[ −(x − mx ) + Pxy .Py−1 .(y − my ) + L.(y − my ) .
 
−(x − mx )T + (y − my )T .Py−1 .Pyx + (y − my )T .LT ]

61
 3 Dynamic State Estimation

P = Px − Pxy .Py−1 .Pyx − Pxy .LT

−Pxy .Py−1 .Pyx + Pxy .Py−1 .Py .Py−1 .Pyx + Pxy .Py−1 .Py .LT
−[Link] + [Link] .Py−1 .Pyx + [Link] .LT
Carry out simplifications line by line:

P = Px − Pxy .Py−1 .Pyx − Pxy .LT

+Pxy .LT
+[Link] .LT
Finally:

P = Px − Pxy .Py−1 .Pyx + [Link] .LT

The quadratic term [Link] .LT is always positive whatever L.
Therefore, P is minimum for L = 0.

The optimal linear estimate is therefore:

x̂ = mx + Pxy .Py−1 .(ym − my )

(3.9)
P = Px − Pxy .Py−1 .Pyx

3.6 Kalman filter

The Kalman filter is an "estimator/predictor" observer whose gain K is calculated
so that the estimate is optimal [19].

3.6.1 Update stage

Let suppose that we have a predicted state x̂k|k −1 with its covariance matrix Pk|k−1 ,
as displayed by Fig. 3.13. This is the best estimate we can get at this stage.
As the measurement noise is centered, the best predicted observation is therefore:

ŷk|k −1 = C x̂k|k −1
Let us compute the optimal linear estimate of the state using Eq. 3.9:

x̂k|k = x̂k|k −1 + Pxy .Py−1 .(yk − C x̂k|k −1 )

which can be written as

x̂k|k = x̂k|k −1 + K.(yk − C x̂k|k −1 )

with

K = Pxy Py−1 (3.10)

called “the Kalman gain”.
The covariance matrix of the error is given by (see Eq. 3.9)

Pk|k = Pk|k −1 − Pxy .Py−1 .Pyx (3.11)

Py corresponds to the covariance matrix of the innovation.

62
 3 Dynamic State Estimation

Pk|k-1
x2 y2 Py
^xk|k-1
y^k|k-1
x^k|k
Pk|k

yk 2) Observation

x1 y1

Figure 3.13: Kalman filter update illustration

3.6.2 Expressions of the Kalman gain

The cross-covariance matrix Pxy is given by:
n o
Pxy = E (xk − mx ) · (yk − my )T
which is
   T 
Pxy = E xk − x̂k|k −1 · yk − C x̂k|k −1

Since yk = Cxk + βk :
   T 
Pxy = E xk − x̂k|k −1 · Cxk + βk − C x̂k|k −1
   T 
=E −ek|k−1 · −Cek|k −1 + βk
n o n o
= E ek|k −1 · eTk|k −1 · C T − E ek|k −1 · βkT
Since βk is white:
n o
Pxy = E ek|k −1 · eTk|k −1 · C T
= Pk|k −1 C T
Look at now at the covariance matrix Py .
n o
Py = E (y − my ).(y − my )T
n o
= E (yk − C x̂k|k −1 ).(yk − C x̂k|k −1 )T
n o
= E (Cxk + βk − C x̂k|k −1 ).(Cxk + βk − C x̂k|k −1 )T
n o
= E (−Cek|k −1 + βk ).(−Cek|k −1 + βk )T
Since βk is white:
n o n o
Py = C.E ek|k −1 · eTk|k −1 · C T + E βk .βkT (3.12)
and so

Py = C.P k|k −1 · C T + R (3.13)

63
 3 Dynamic State Estimation

This expression clearly shows that this is the covariance matrix of the innovation.
The Kalman gain is therefore:
 −1
K = Pk|k −1 C T [Link]|k −1 .C T + R (3.14)

3.6.3 Expressions of the covariance matrix update

As the Kalman filter is an Estimator/Predictor, we can reuse the Joseph’s form we
have seen before:

Pk|k = (I − KC) · Pk|k −1 · (I − KC)T + K · R · K T (3.15)

There are two other expressions.
We have seen in Eq. (3.11) that

Pk|k = Pk|k −1 − Pxy .Py−1 .Pyx

Since K = Pxy Py−1 ,

Pk|k = Pk|k −1 − [Link]

Pxy = Pk|k −1 C T =⇒ Pyx = CPk|k −1

Pk|k = Pk|k −1 − KCPk|k −1

This gives the Kalman’s form10 :

Pk|k = (I − KC) Pk|k−1 (3.16)

A third form can be obtained as follows. Let rewrite Eq. (3.11):
 
Pk|k = Pk|k −1 − Pxy .Py−1 . Py .Py−1 .Pyx
By rearranging the brackets:
   
Pk|k = Pk|k −1 − Pxy .Py−1 .Py . Py−1 .Pyx
We have

K = Pxy .Py−1
By taking the transpose

K T = Py−1 .Pxy
T
= Py−1 .Pyx
So

Pk|k = Pk|k −1 − [Link] .K T

Using Eq. (3.13), we get a third form:
 
Pk|k = Pk|k −1 − K CPk|k −1 C T + R K T (3.17)

10
Another demo is given in Appendix 3.6.7 on page 68 starting from the Joseph’s form

64
 3 Dynamic State Estimation

Notes:
1. The Joseph’s form (Eq.3.15) proves that Pk|k remains positive definite because
it represents the sum of positive definite matrices
2. The last expression (Eq. 3.17) indicates that trace(Pk|k )< trace(Pk|k−1 ) and
therefore the estimation step increases confidence.
3. The Joseph’s form may seem unnecessarily complicated, but is numerically
well conditioned. In addition, it is valid for any “estimator/predictor” observer
whatever its gain. The other forms are only valid for the Kalman filter.

3.6.4 Prediction stage

The best prediction is simply given by applying the evolution model like any Esti-
mator/Predictor observer:

x̂k+1|k =A.x̂k|k +[Link]

Eq. 3.6 gives the propagation of the covariance of the error:

Pk+1|k = A · Pk|k · AT + Q

3.6.5 Sequential Program

With a linear system of the type:

xk+1 = [Link] + [Link] + αk

(

yk = [Link] + βk
The state recursively estimated with the following algorithm particularly well
suited to real time implementation.

Algorithm 3.1 A program performing a discrete Kalman filter optimized for a

real-time implementation
 −1
x̂ = x0 P = P0 K = P · C T · C · P · C T + R %init
Sampled loop
y←Get_observation() %sensor data

%Update stage
x̂ = x̂ + K · (y − C · x̂)
P = (I − KC) · P · (I − KC)T + K · R · K T %Joseph’s form

%Provide the estimate x̂ and

%the estimated variance of the estimate error P
%to client applications

%Prediction or time update

x̂ = A · x̂ + B · u %u known from the control program
P = A · P · AT + Q −1
K = P · CT · C · P · CT + R
End Loop

65
 3 Dynamic State Estimation

3.6.6 Comments
• This estimation method bears the name of Kalman because he was the first to
found the expression of the optimal gain11 . Note that to calculate this term
one must compute the covariance matrix of the estimation error.

• A discrete Kalman filter implies the calculation of two steps during the same
sampling period: an estimation step and a prediction step.

• The expression Ax̂k|k + [Link] is the best prior prediction of x̂k+1|k , that is to
say, not taking into account observation yk+1 . This is consistent with common
sense: in the absence of additional information (namely observations yk+1 ),
it is natural to make the prediction follow the certain part of the evolution
model.
 
• The correction term K yk − C · x̂k|k −1 reflects the influence of the difference
between the observation yk and "the predicted observation" C · x̂k|k −1 . From
the difference thus obtained, a linear correction is developed, based on the
optimum gain matrix K.

• The expression of the optimal gain K can also be subject to simple consid-
erations: if one has great confidence in previous estimates and if the current
observation is considered doubtful, K must be small; conversely, if the above
estimates are unreliable and if the current observation is considered correct,
K should be high.

And thus putting C = I for simplification we have K = P.(P + R)−1

“{Confidence in the previous estimates and little in the observation}
→P small and R high → K small”
“{Confidence in the observation and little in the previous estimates}
→P high → K close to 1 and the new estimate will be the observation”

• The initial state x0 has a temporary impact on the estimation process.

• If the matrices of the system and the noises covariances are constant, then the
gainK converges towards a constant. In this case, it can be calculated offline
and implement in an asymptotic filter. There is no matrix to calculate online.

• Numeric instability. Because of rounding errors, the covariance matrix can

become non-symmetric and/or non-positive definite. One solution is to use a
"reduced" form of the filter.

3.6.7 Case of observations with uncorrelated noises

The Kalman filter has a remarkable property if the observations are disturbed by
uncorrelated noises (R is diagonal).
In this case, one can perform sequential estimation steps by correcting the esti-
mated state given by the previous observation. Pay attention to this point.
This reduces the size of the matrix to be inverted. If all observations are scalar
then the update step can be greatly accelerated.

11
that is to say, here, the gain that minimizes the variance of the estimation error.

66
 3 Dynamic State Estimation

Algorithm 3.2 Sequential Kalman filter with two uncorrelated observation errors
x̂ = x"0 P = P0 #%init
R1 0
R=
0 R2
Loop
h i
y1 y2 ←Get_observations(); %sensor data
%Do two estimation steps sequentially
%First update
 −1
K1 = P · C1T · C1 · P · C1T + R1
x̂ = x̂ + K1 · (y1 − C1 · x̂)
%x̂ on the right hand side is the previous prediction
%x̂ on the left hand side is a new estimate
P = (I − K1 C1 ) · P · (I − K1 C1 )T + K1 · R1 · K1T
%Second update
 −1
K2 = P · C2T · C2 · P · C2T + R2
x̂ = x̂ + K2 · (y2 − C2 · x̂)
%x̂ on the right hand side is the previous estimate
P = (I − K2 C2 ) · P · (I − K2 C2 )T + K2 · R2 · K2T
%Provide the estimate x̂ and the P variance
%Time update for the next step
x̂ = A · x̂ + B · u %u is known
P = A · P · AT + Q
End Loop

Furthermore, it is shown that result of the fusion does not depend on the order
in which the observations are used12 .

12
It is quite easy to demonstrate since every use of an observation is optimal

67
 3 Dynamic State Estimation

Appendix: Kalman’s form of the covariance matrix

update from the Joseph’s form
By developing the Joseph’s form:

 
Pk|k = Pk|k −1 − KCPk|k −1 − Pk|k −1 C T K T + K · CPk|k −1 C T + R · K T

Yet  −1
K = Pk|k−1 C T CPk|k −1 C T + R
 −1 T  −1
Therefore K = T
CPk|k −1 C + R
T T
CPk|k −1 = CPk|k −1 C + R
T T T T
CPk|k−1
Since Pk+1|k and R are covariance matrices that are symmetric, we have:
−1 = Pk|k −1 and R = R
T T
Pk|k
So  −1
K T = CPk|k −1 C T + R CPk|k −1

To facilitate the mathematical developments, let denote P = Pk|k−1 and S =

CPk|k −1 C T + R
So,
K = P C T S −1
K T = S −1 CP

Pk/k = P − KCP − P C T K T + KSK T

Let us show that P C T K T = KSK T

Develop each terms
P C T K T = P C T S −1 CP

KSK T = P C T S −1 SS −1 CP = P C T S −1 CP
Therefore
Pk|k = P − KCP

68
 3 Dynamic State Estimation

Appendix Graphical representation of an ellipse

Let a symmetric positive definite matrix M . Any non-degenerated ellipse may be
defined as follows:
n o
Ξ = x ∈ Rn | (x − mx )T · M · (x − mx ) = k 2 and M > 0
where mx is the center of the ellipse and M is the matrix which defines the
direction and the surface.
" #
A B
Let M = P −1 =
B C
Then with equation (??) A = σ2 (1−ρ
1
2 ) , B = σ σ (1−ρ2 ) and C = σ 2 (1−ρ2 )
x y
−ρ 1
x y
Eigenvalues of M :
q
A+C ∓ (A − C)2 + 4B 2
λ1,2 =
2
" #
ux
λ1 has the eigen-vector ⃗u =
uy
M · ⃗u = λ1 · ⃗u
The non-normalized coordinates of u (ux , ūy ) verify the linear system:

(A − λ1 )ūx + B ūy = 0
(

Bux + (C − λ1 )ūy = 0
−(A−λ1 )
By choosing ux = 1 ⇒ uy = B
= −B
q (C−λ1 )
For normalization divided by norm = u2x + u2y
1
" #
ux
u=
norm uy
The angle of the axis associated with ⃗u is denoted by tan(φ) = −(A−λ B
1)

2 2
In its own frame, the ellipse is written as x + yb2 = 1 and we have λ1 ·x2 +λ2 ·y 2 = k 2
q 2 a2 q
Lengths of the two semi-axes: a = λk 1 b = λk 2
2

x = a cos(θ)
(
Then with the equations , for θ ∈ [0, 2π], we can represent the
y = b sin(θ)
ellipse in its own coordinate system.
To get it in the reference frame, it suffices to rotate by φ.

69
 3 Dynamic State Estimation

Algorithm 3.3 Matlab function to plot an ellipse

function ellipse(X,P,PROBA)
imax=20; %1/4 of the plotted points
k=sqrt(-2*log(1-PROBA));
Cx=P(1,1);Cy=P(2,2);
a=1/(Cx*(1-Ro^2));
b=-Ro/(sqrt(Cy*Cx)*(1-Ro^2));
c=1/(Cy*(1-Ro^2));
% Test if b=0 then chose a small value
% this is the case for ellipses with axes (ox,oy)
if (sqrt(b*b)<1e-9), b=1e-9; end;
%eigenvalues
%the largest (lambda1) is associated to the smallest axis.
delta=(a-c)*(a-c)+4*b*b;
lambda1=0.5*(a+c+sqrt(delta));
lambda2=0.5*(a+c-sqrt(delta));
%direction vector of the major axis
aux=(lambda2-a)/b; deno=sqrt(1+aux*aux);
Ux=1/deno; Uy=aux/deno;
%length of the axes in the own coordinate system
axeX=k/sqrt(lambda2); axeY=k/sqrt(lambda1);
%plot
dq=pi/2/imax;x=X(1);y=X(2);
point_ellipse=zeros(4*(imax+1),2);
for (i=1:4*(imax+1)),
%own coordinate system
x0=axeX*cos(dq*i); y0=axeY*sin(dq*i);
%initial frame R0
point_ellipse(i,:)=[x+x0*Ux-y0*Uy, y+x0*Uy+y0*Ux];
end;
plot(x,y,point_ellipse(:,1),point_ellipse(:,2));

70
4 Non linear systems
Here we are interested in observing the state of nonlinear systems.

4.1 Problem statement of probabilistic

transformations of random vectors
For simplicity, we assume Gaussian probability densities. This level of abstraction
is pedagogically valuable to understand the basics.
The linear transformation of a Gaussian random variable provides another Gaus-
sian random variable (Fig. 4.1).

Figure 4.1: Linear transformation of a Gaussian

The transformation of a Gaussian random variable with a non-linear function is

no longer a Gaussian random variable as shown in figure 4.2.

71
 4 Non linear systems

Figure 4.2: Non-linear transformation of a Gaussian

Therefore, in practice it is often limited to the second order, forgetting the mo-
ments of order greater than two. Transformations then become approximate.

4.2 Extended transformation of random vectors

4.2.1 Method
T1 (x1 , ..., xn )
 

Let y = T (x) =  ...

 

Tp (x1 , ..., xn )
where T is a nonlinear vector function of Rn in Rp .
To find the first two statistical moments of y, one approximates T by its first
order Taylor expansion.
Introduce a centered random vector denoted △x such that

x = mx + △x
One can note that P△x = Px .
If △x is "small" with respect to the non-linearity then to the first order we have:
" # " #
∂T ∂T
y = T (mx ) + (x − mx ) + ... = T (mx ) + .△x + ....
∂x mx
∂x mx
h i
where ∂T
∂x mx
is the Jacobian matrix of T at point mx .
 
∂T1 (mx ) ∂T1 (mx )
h i ∂x1
... ∂xn
∂T
= is a p.n matrix.
 
∂x mx  ... ... ... 

∂Tp (mx ) ∂Tp (mx )
∂x1
... ∂xn
Mean

my = T (mx )

72
 4 Non linear systems

 h i  h i
Since my = E T (mx ) + ∂T
∂x mx
· ∆x = T (mx ) + ∂T
∂x mx
.E {∆x} and ∆x is
centered (E {∆x} = 0).
The linear transformation is applied to a random vector, which gives the covari-
ance matrix:
" # " #T
∂T ∂T
Py = .Px .
∂x mx
∂x mx

Note: This method requires relatively small deviations from the mean, which
means that, if an element of Px is "large", then Py will be a rough estimate.

Figure 4.3: Extended transformation of a Gaussian. The blue Gaussian is an illus-

tration of the second order moment of the distribution after the trans-
[Link] green one is an illustration of the second order moment
computed by the extended transformation.

4.2.2 Examples
[Link] Localization uncertainty from pseudo-distances
We saw previously (cf section [Link]) that if we concatenate the pseudo-range mea-
surements in a vector y = (ρ1 , ..., ρm )T , the unknown state x = (x, y, z, d)T relates
to measures by a nonlinear observation model:

y = g(x)
We can solve this system by the Newton’s method that we have already seen.
Let x̂ be this estimate.
Suppose that the covariance of the measures Py is known. We seek Px .
Apply the extended transformation:
" # " #T
∂g ∂g
Py = .Px .
∂x x̂
∂x x̂
h i
Note H = ∂g
∂x x̂
and multiply on the right by H:

73
 4 Non linear systems

Py H = HPx H T H
If H T H is invertible then
 −1
Py H H T H = HPx
The procedure is similar on the left:
 −1  −1
HT H H T Py H H T H = Px
If it is assumed that the pseudo-range errors are uncorrelated with each other
and statistically identical, the Py matrix is a diagonal matrix whose diagonal is σ 2
everywhere.

Py = σ 2 Im
Then it comes:
 −1  −1  −1
Px = σ 2 H T H H T Im H H T H = σ2 H T H (4.1)
 −1
The matrix H T H therefore indicates how measurement inaccuracy is trans-
ferred to the state x. It is homogeneous to a covariance matrix.
The trace of a covariance matrix generally reflects inaccuracy (it is a measure of
the volume of the uncertainty ellipsoid).
We call Global Dilution Of Precision:
q
GDOP = trace (H T H)−1
The GDOP therefore reflects how the inaccuracy of measurements reports on the
position and time estimates (x, y, z, d) by neglecting the cross-correlations between
the measurement noises. It reflects the effect of the geometrical configuration.
Similarly, we define the PDOP as inaccuracy on position (x, y, z), the HDOP as
inaccuracy on 2D position (x, y) in the tangent plane to the Earth, the TDOP as
inaccuracy on the clock drift, and VDOP as inaccuracy on speed.

[Link] Fusion of redundant direct measurements with unknown variance

Suppose we have m measurements of an unknown real value x concatenated in a
vector y.
h iT
y = Hx with H = 1 ... 1
Suppose Py is diagonal with σ 2 everywhere (the measures are uncorrelated with
the same variance):

Py = σ 2 .Im
By using Eq. 4.1:
 −1 σ2
Px = σ 2 H T H = σ 2 .m−1 =
m
If the variance of the noise on y is unknown (which can occur frequently in prac-
tice), an (unbiased) estimate of its variance is given by
1 X m
σ̂ 2 = (yi − x̂)2
m − 1 i=1

74
 4 Non linear systems

 −1
with x̂ = H T H H T y the least-squares estimated value.
So :
i=1 (yi
Pm
− x̂)2
Px =
m(m − 1)

4.2.3 Expression of the variance of the estimation error with

Weighted Least-Squares
We have seen that
 −1  −1
Px = H T H H T Py H H T H (4.2)
This expression is equivalent to:
 −1
Px = H T Py−1 H (4.3)
The demonstration of Eq (4.3) starting from Eq (4.2) is not straightforward
and

don’t
−1
try to demonstrate
−1
it by doing the expansion of the inverse. Indeed,
T
H H ̸= H −1
H T
since H is not invertible.
The good way is to restart from the general expression of the Weighted Least
Squares solution. The solution increment of the “ith” iteration is:
 −1
dx̂(i) = H T Py−1 H H T Py−1 .dy (i)
We
n have
o dy = [Link] + β, the last term being the noise with covariance matrix
(i) (i)

E β.β = Py .
T

The offset dx(i) between the current estimate and the real value is defined by:

x = x̂(i) + dx(i)
Therefore, we have:
 −1  
dx̂(i) = H T Py−1 H H T Py−1 [Link](i) + β
 −1  −1
dx̂(i) = H T Py−1 H H T Py−1 Hdx(i) + H T Py−1 H H T Py−1 β
 −1
dx̂(i) = dx(i) + H T Py−1 H H T Py−1 β
The estimation error of the new estimate is given by:
 
e = x̂ − x = x̂(i) + dx̂(i) − x̂(i) + dx(i) = dx̂(i) − dx(i)

Therefore:
 −1
e = H T Py−1 H H T Py−1 β
 
By definition, Px = E [Link]
(  T )
 −1 −1
Px = E H T
Py−1 H H T
Py−1 β. H T
Py−1 H H T
Py−1 β

 −1 n o  −1 T
Px = H T Py−1 H H T Py−1 E ββ T . H T Py−1 H H T Py−1

75
 4 Non linear systems

 −1   −1 
Px = H T
Py−1 H H T
Py−1 Py Py−1 H H T
Py−1 H

since Py is symmetric.
Some simplifications occur. Let consider them one by one:
 −1   −1 
Px = H T
Py−1 H H T
Py−1 H H T
Py−1 H
 −1  −1
Px = H T Py−1 H H T Py−1 H H T Py−1 H
 −1
Px = H T Py−1 H
which demonstrates Eq. (4.3).

4.3 Unscented transformation

4.3.1 Method
The unscented transformation addresses the general problem of approximating a
non-linear transformation of a random variable. More generally, for a random vector
x of dimension n, with a mean mx , and a covariance matrix Px , we seek to estimate
the first two moments my and Py of the random vector y of dimension p such as:

y = T (x)
where T (.) is a non-linear transformation that can be explicit (square, integral,...)
or implicit (min, max,...).
The unscented transformation algorithm is based on the idea that "it is easier to
approximate the transformation of a Gaussian probability density by points than to
approximate the non-linear transformation".
Let be a set of N points {xi }i=1,...,N , called “sigma points”, created from the mean
mx and the covariance Px . At each point xi , we associate a weight wi . There is a
good sampling condition because the points and weights must be chosen so that the
mean and variance of the points are respectively equal to mx and Px .
The good sampling condition is therefore verified if the two following conditions
are verified:
N
w i x i = mx
X

i=1

N   T
wi xi − mx xi − mx = Px
X

i=1

The mean and covariance of y can be estimated as follows:

1. Evaluate the non-linear function at each point xi :

 
y i = T xi i = 1, ..., N

76
 4 Non linear systems

2. Estimate the mean of y using the transformed points and associated weights:

N
my = wi y i
X

i=1

3. Estimate the covariance matrix:

N   T
Py = wi y i − my y i − my
X

i=1

Sigma points xi
Covariance

Mean

my=T(mx)
y=T(x) T yi=T(xi)
 T   T 
Py     Px   

  x  x 

Covariance
Covariance
Covariance

my
Mean my=f(mx)

Transformed sigma points

Figure 4.4: Comparison of the extended (center) and unscented (right) transforma-
tions. A 2x2 covariance matrix is represented by an ellipse.

4.3.2 Symmetric elementary sampling

There are many solutions to choose sigma points.
For a random vector x of dimension n, a simple method is:

For i=1,..,n√
xi = mx + n.M
√ (:, i) wi = 2n
1

xi+n = mx − n.M (:, i) wi+n = 2n

1

End

So, there are N = 2n points.

M represents the square root matrix of (matrix positive defined) Px such as:

M · M T = Px

M (:, i) denotes the ith column of matrix M . The calculation of this matrix can
be done in an efficient numerical way with Cholesky’s factorization. For example,
with Matlab, we just have to do:

M=chol(P,’lower’);%Returns a matrix M such as M*M’=P

77
 4 Non linear systems

4.3.3 Sampling with 2n+1 points

There are many variants: the most common is to add a point to the mean (which
gives a tuning parameter denoted w0 ) :

x0 = mx w0 = w0
For i=1,..,nq
1−w0
xi = mx + 1−w
n
0 .M (:, i) wi = 2n
0
xi+n = mx − (:, i) = 1−w
q
n i+n
1−w0
.M w 2n
End

This sampling is widely used in practice and generalizes the previous one with w0 =
0. See Fig. 4.5

Sigma points Sigma points

3 8

2.5 6

2
4

1.5
2
1
0
0.5

−2
0

−0.5 −4

−1 −6
−1.5 −1 −0.5 0 0.5 1 1.5 2 2.5 3 3.5 −8 −6 −4 −2 0 2 4 6 8 10

Figure 4.5: Sigmas points calculated with the 2n+1 points method: w0 = 0 on the
left and w0 = 0, 9 on the right.

Here is a Matlab function that calculates sigma points (be careful, this code is
not optimized and written in an pedagogical form):

function [xPts, wPts, nPts] = SigmaPoints(x,P,w0)

%Sampling with 2n+1 sigma points
n=length(x); nPts=2*n+1;
%the sigma points are concatenated in a matrix
xPts = zeros(n,2*n+1); %memory allocation
M=chol(P,’lower’);
scale=sqrt(n/(1-w0));
for i=1:n,
xPts(:,i)=x+scale*M(:,i);
xPts(:,i+n)=x-scale*M(:,i);
end
xPts(:,nPts)=x;%we add at the end the point of the mean
%Vector of the weights
wPts=ones(1,nPts-1)*(1-w0)/(2*n);
wPts(nPts)=w0;

Here is a more optimized version for Matlab because the for loop is eliminated. This
program is less easy to read.

78
 4 Non linear systems

function [xPts, wPts, nPts] = SigmaPoints(x,P,w0)

%Sampling with 2n+1 sigma points
n=length(x); nPts=2*n+1;
scale=sqrt(n/(1-w0));
M=scale*chol(P,’lower’);
xPts=[-M M zeros(n,1)];%the mean is added to the points
xPts = xPts + repmat(x,1,nPts);
wPts=ones(1,nPts)*(1-w0)/(2*n);
wPts(nPts)=w0;

4.3.4 Sampling along the principal components

Another possibility is to set the sigma points along the principal directions of the
covariance matrix which requires first to compute the eigenvectors and standard
deviations in this frame.

x0 = mx w0 = w0
[V,D] = eig(P);%Eigenvectors and eigenvalues matrices
For i=1,..,n q 0
xi = mx + 1−w D (i, i).V (:, i) wi = 1−w
q
n
0. 2n
q
1−w0
xi+n = mx − D (i, i).V (:, i) wi+n =
q
n
1−w0
. 2n
End

Where V (:, i) is the ith eigenvector and D (i, i) is its eigenvalue which represents
the variance along this direction, and so the square-root has to be taken.
This choice for the sigma points has two advantages :

• The sampling is along the main axes of the covariance which is well conditioned
(Fig 4.6),

• The eigenvalues decomposition is faster than the Cholesky’s one.

Sigma points

-1

-2 -1 0 1 2 3 4

Figure 4.6: Sigmas points calculated with the 2n+1 points method along the prin-
cipal components with w0 = 0, 2.

79
 4 Non linear systems

4.4 Extended Kalman Filter

Consider now a non-linear system represented by the following equations:

xk+1 = f (xk , uk ) + αk
(
(4.4)
yk = g (xk ) + βk
The idea is to come back, after a linearization to a first order, to the linear filter
described in chapter 3.
Linearization can be done around a predefined reference path: in this case we get
the Linearized Kalman Filter 1
Linearization can also be done around the previous predicted or estimated values
(Extended Kalman Filter EKF2 ).
The second solution is the most widely used because it can be used even if the
path is unknown.

4.4.1 Linearization around the current estimates

The Kalman formalism extended to the first order represents a rather simple solution
to nonlinear estimation problems.
Consider the first order Taylor expansion of the vector function g, around the
known predicted value x̂k|k−1 :
′
g (x) = g (x0 ) + g (x0 ) · (x − x0 ) + ...
x = xk
(
Let us pose
x0 = x̂k|k−1
" #
  ∂g    
g (xk ) = g x̂k|k−1 + x̂k|k−1 · xk − x̂k|k−1 + ...
∂x
The linearization is good if xk is close to x̂k|k −1 , that is to say, if the prediction
error is small, the linearization is valid.
The same reasoning applies to the evolution model. Consider the first order Taylor
expansion of the vector function f , around the known estimated value x̂k|k
" #
  ∂f    
f (xk , uk ) = f x̂k|k , uk + x̂k|k , uk . xk − x̂k|k + ...
∂x
There is no need to derive f with respect to u, since u is deterministic and known.
The functions f and g are replaced by the above linearizations in the nonlinear
model (4.4).
At every sampling time k denote
h  i
Ak = ∂f
∂x
x̂k|k , uk
h  i
Ck = ∂g
∂x
x̂k|k−1
   
Consider ǔk = f x̂k|k , uk −Ak x̂k|k as a new input and zk = yk −g x̂k|k −1 +Ck x̂k|k −1
a new output (which is in fact an artificial observation).
We obtain the following linear system (Bk = Ip ):
xk+1 = Ak xk + ǔk + αk
(

zk = C k xk + βk
1
for example, in the case of a rocket, the path is known in advance.
2
The first applications were for the Apollo mission (1961 - 1962).

80
 4 Non linear systems

4.4.2 Filter Steps

Now the same methodology as a linear filter can be applied around each linearization
point.

[Link] Observation update

 
x̂k|k = x̂k|k−1 + Kk zk − Ck x̂k|k−1
   
= x̂k|k−1 + Kk yk − g x̂k|k−1 + Ck x̂k|k−1 − Ck x̂k|k−1
  
= x̂k|k−1 + Kk yk − g x̂k|k−1
 
Note that this expression becomes non-linear because of the term g x̂k|k−1 . So
we can now forget about the z-output that only served to state the problem.
The estimation error follows the recursion:

ek|k = x̂k|k − xk   
= x̂k|k−1 + K yk − g x̂k|k−1 − xk
  
= x̂k|k−1 + K g (xk ) + βk − g x̂k|k−1 − xk
      
≈ x̂k|k−1 − xk + K g x̂k|k−1 + Ck xk − x̂k|k−1 + βk − g x̂k|k−1
   
≈ x̂k|k−1 − xk + K −Ck x̂k|k−1 − xk + βk
= (I − KCk ) ek|k−1 + Kβk

So

Pk|k = (I − KCk ) · Pk|k −1 · (I − KCk )T + K · R · K T

This the Joseph’s form we have seen already.
To make the stage optimal we have to calculate the gain as follows:
 −1
Kk = Pk|k −1 CkT Ck Pk|k −1 CkT + R
Please notice that the gain changes Kk over time.

[Link] Prediction
x̂k+1|k = Ak x̂k|k + ǔk 
= Ak x̂k|k + f x̂k|k , uk − Ak x̂k|k
 
= f x̂k|k , uk
So we can forget ǔk which was only used to state the problem.

ek+1|k = x̂k+1|k
 − xk+1
= f x̂k|k , uk − (f (xk , uk ) + αk )
      
≈ f x̂k|k , uk − f x̂k|k , uk + Ak . xk − x̂k|k − αk
≈ Ak ek|k − αk
So

Pk+1|k = Ak Pk|k ATk + Q

81
 4 Non linear systems

4.4.3 Calculation of Jacobian matrices

To implement an EKF, one has to calculate the literal expression of the Jacobian
matrix and evaluate its components at each time step. The Matlab environment now
includes symbolic computation capabilities that are interesting. Here is a sample
calculation of Jacobian with Matlab.

%Cartesian-polar transformation
syms x1 x2; %definition of symbolic objects
% function definition
f = [x1*cos(x2); x1*sin(x2)];
%Jacobian
jacob= [diff(f(1),x1) diff(f(1),x2);
diff(f(2),x1) diff(f(2),x2)]

The program returns:

jacob = [ cos(x2), -x1*sin(x2)]

[ sin(x2), x1*cos(x2)]

4.4.4 Sequential Program of an EKF

With a nonlinear system of the type :

xk+1 = f (xk , uk ) + αk
(

yk = g (xk ) + βk
The state is recursively observed with the following algorithm particularly well
suited to real time implementation.

%Initialization
x̂ = xh0 P = iP0
C = ∂x ∂g
(x̂)
 −1
K = P · CT · C · P · CT + R
Sampled loop
%Sensor data
y←Get_observation()
%Update stage
x̂ = x̂ + K · (y − g (x̂))
P = (I − KC) · P · (I − KC)T + K · R · K T
%Provide the estimate x̂ and the estimated variance
%of the estimation error P to client applications

%next hstep is iprepared

A = ∂f ∂x
(x̂, u)
x̂ = f (x̂, u) %u known from the control program
P =A h · P · iA + Q
T

C = ∂x (x̂)
∂g
 −1
K = P · CT · C · P · CT + R
End Loop

82
 4 Non linear systems

4.5 Iterated Extended Kalman filter

When the effects of linearization errors tend to degrade the performance of an EKF
filter, relinearization of the observation equation around the current state can re-
duce the problems. This method is known as the Iterated Extended Kalman Filter
(IEKF)[12].
Consider a system represented by the same non-linear equation (4.4).
In the IEKF, only the observation update of an EKF is modified to improve the
linearization point. The improvement is achieved by local iterations of the EKF
observation update like a Gauss-Newton method with prior information.
Let us do the linearization around a state x̂ik . We define zk as:
 
zk = yk − g x̂ik + Cki x̂ik
 
x̂k|k = x̂k|k−1 + Kk zk − Cki x̂k|k−1
 
= x̂k|k−1 + Kk yk − g (x̂ik ) + Cki x̂ik − Cki x̂k|k−1
  
= x̂k|k−1 + Kk yk − g (x̂ik ) − Cki x̂k|k−1 − x̂ik

Step 1 (initialization): Set i = 0 and choose the estimate x̂ik = x̂k|k−1 (the super-
script i denotes the number of iteration) for the linearization.
Step 2 (observation update iterations): Compute the Jacobian matrix at the best
state estimate available, the Kalman gain, and the next iteration of the state
estimate as
h i
• Jacobian matrix : Cki = ∂g
∂x
(x̂ik )
 −1
• Kalman gain Kki = Pk|k −1 Cki Cki Pk|k −1 Cki + R
T T

n  o
• x̂i+1
k = x̂k|k−1 + Kki yk − g (x̂ik ) − Cki x̂k|k−1 − x̂ik

Repeat Step 2 with i = i + 1 until ∥x̂i+1

k − x̂ik ∥ < ε (ε is a threshold)
Step 3 (update of the state and of the covariance matrix)

• x̂k|k = x̂i+1
k

T
• Pk|k = (I − Kki Cki ) Pk|k −1 (I − Kki Cki ) + Kki RKki
T

When there is no iteration, the IEKF is an EKF. Indeed, since x̂k|k−1 = x̂0k , we
have:
n  o n  o
x̂k|k = x̂k|k−1 + Kki yk − g x̂0k = x̂k|k−1 + Kki yk − g x̂k|k−1
 
In step 2 when the solution x̂ik evolves, the term −Cki x̂k|k−1 − x̂ik shows that
the optimization problem takes into account the prediction. Therefore, the IEKF
is not a Maximum Likelihood (ML) estimator but a Maximum A Posteriori (MAP)
estimator.
Please note that like any Gauss Newton method, the convergence of the iteration
method is not guaranteed.

83
 4 Non linear systems

%Initialization
x̂ = x0 P = P0
Sampled loop
%Sensor data
y←Get_observation()
%Update stage
xi = x̂ %x̂ is the prediction
bool = True
While bool
h doi
C = ∂x ∂g
(xi )
 −1
K = P · CT · C · P · CT + R
xi+1 = x̂ + K · (y − g (xi ) − C (x̂ − xi ))
bool=(norm(xi+1 − xi ) >Threshold)
xi = xi+1
End While
x̂ = xi+1
P = (I − KC) · P · (I − KC)T + K · R · K T
%Provide the estimate x̂ and the estimated variance
%of the estimation error P to client applications

%nexth step i
A = ∂f ∂x
(x̂, u)
x̂ = f (x̂, u) %u known from the control program
P = A · P · AT + Q
End Loop

84
 4 Non linear systems

4.6 Unscented Kalman filter

When the state evolution and observation models are highly nonlinear, the EKF can
give particularly poor performance because the covariance is propagated through lin-
earization. The Unscented Kalman filter (UKF) published by Julier et Uhlman in
1997 [14] uses a deterministic sampling technique known as the unscented transfor-
mation (UT) to pick a minimal set of “sigma points” around the mean. The sigma
points are then propagated through the nonlinear functions, from which a new mean
and covariance estimate are then formed.
This technique removes the calculation of Jacobian matrices which is sometimes
not possible if the functions are not differentiable.
Here are the advantages mentioned by Julier et Uhlman:
1. A better estimation of error variances (and therefore a better consistency),
2. Better numerical stability than EKF,
3. Easier parameter setting3 (often, to set an EKF, the noise parameters are
artificially increased).

4.6.1 Update
The Kalman gain is given by the same formula K = Pxy Py−1 . We use sigma points
to compute Py−1 and Pxy .

[Link] Computation of Py−1

We have N sigma points xi coming from the previous prediction stage and we know
a prediction x̂k|k−1 .
We compute the sigma points in the observation space :
 
For i = 1..N, y i = g xi

The predicted observation is

N
ŷk = wi y i (4.5)
X

i=1

The covariance matrix of the predicted observation is:

n o
Py = E (yk − g(x̂k|k −1 )).(yk − g(x̂k|k −1 ))T
n o
Py = E (g(xk ) + βk − ŷk ).(g(xk ) + βk − ŷk )T
As the noise βk is white and uncorrelated with the prediction:
n o n o
Py = E (g(xk ) − ŷk ).(g(xk ) − ŷk )T + E βk .βkT
The first term is the contribution of the prediction error and, the second one,
the one of the observation error. Yet, the sigma points have been computed in
the observation space with the predicted state, so they include the prediction error.
In other words, the prediction uncertainty has been propagated to the observation
space through the sigma points. We use them to compute the first term. Then, we
add the covariance of the noise.
3
In fact, the UT transformation adds parameters that also need to be adjusted...

85
 4 Non linear systems

N   T
Py = wi y i − ŷk y i − ŷk +R (4.6)
X

i=1

[Link] Computation of Pxy

The cross-covariance matrix is:
   T 
Pxy = E xk − x̂k|k −1 · yk − g(x̂k|k −1 )
   T 
Pxy = E xk − x̂k|k −1 · g(xk ) + βk − g(x̂k|k −1 )

As the noise βk is white and uncorrelated with the prediction, we can remove it
from the computation:
   T 
Pxy = E xk − x̂k|k −1 · g(xk ) − g(x̂k|k −1 )
n  o
Pxy = E xk − x̂k|k −1 · (g(xk ) − ŷk )T
So the cross-covariance matrix can be computed using the sigma points of the
predicted state and the ones of the predicted observation:
N   T
Pxy = wi xi − x̂k|k−1 y i − ŷk (4.7)
X

i=1

[Link] Update
We can compute the Kalman gain:

K = Pxy · Py−1 (4.8)

x̂k|k = x̂k|k−1 + K.(yk − ŷ) (4.9)

Pk|k = Pk|k−1 − [Link] .K T (4.10)

Please note that we use here the third form (P = P − [Link] .K T ) which is nice in
this problem since we don’t manipulate any Jacobian matrix.

86
 4 Non linear systems

Px
Py
x2 y2
mx
my
x

y Observation

State space x1 Observation space y1

Figure 4.7: Illustration of the UKF Update.

4.6.2 Prediction
After having done the update, we have an estimate and a covariance matrix which
allows to compute new sigma points.
We pass the sigma points through the evolution model:
 
For i = 1..N, xi = f xi
and so
N
x̂k+1|k = w i xi (4.11)
X

i=1

We compute their variance and add the variance of the model error:
N   T
Pk+1|k = wi xi − x̂ xi − x̂ +Q (4.12)
X

i=1

Px
x2 x2
mx

x1 Instant: tk+1 x1
Instant: tk

Figure 4.8: Illustration of the UKF prediction.

87
 4 Non linear systems

4.6.3 UKF Sequential Program

%Initialization
x̂ = x0 P = P0
Sampled loop
%----- Update-------------
%Sampling of the points
[xi , wi , N ]=sigma_points(x̂, P )
%Instanciation of the points with the observation model
For i = 1..N , y i = g (xi ) End
%Prediction of the observation
ŷ = N i i
P
i=1 w y
%Covariance matrix of the predicted observation
T
Py = N i=1 w (y − ŷ) (y − ŷ) + R
i i i
P

%Cross-covariance matrix
T
Pxy = N i=1 w (x − x̂) (y − ŷ)
i i i
P

%Update with observation yk

K = Pxy · Py−1
x̂ = x̂ + K.(yk − ŷ)
P = P − [Link] .K T %third form
%Provide the estimate x̂ and P to client applications
%-----Prediction for the next step-----------
%Sampling of the points
[xi , wi , N ]=sigma_points(x̂, P )
For i = 1..N , xi = f (xi ) End
x̂ = N w i xi
P
Pi=1 T
P = N i=1 w (x − x̂) (x − x̂) + Q
i i i

End Loop

Pleas note that there is no Jacobian matrix.

88
5 Bayesian filtering for nonlinear
systems
5.1 Introduction
In the linear case (with a linear state space representation) with white Gaussian
noises, it is sufficient to estimate sequentially the mean of the state and the covari-
ance of the estimation error. In this case, the optimal solution is the Kalman filter.
If the state equations of the system are nonlinear, then the probability density that
characterizes the state does not remain a Gaussian, even if the noises are Gaussian.
We should therefore consider all the statistical moments and not just the mean and
covariance. Finally, if the noises are not Gaussian and the system is non linear, we
have to estimate the probability density function (pdf ) as a whole.
Bayesian filtering is a technique that allows the propagation of probability densi-
ties according to the predictor / estimator mechanism initialized with a prior infor-
mation denoted p(x0 ). In general, there is no analytical solution. Several numerical
approaches are good approximations.

5.2 Bayesian Reminders

5.2.1 Continuous Case
Let a probability density (or mass function or probability distribution or distribution
in short1 ) p(x) defined on a domain S. This support is the set of values for which
the distribution is not zero. In the continuous case, we have
Z
p(x)dx = 1 et p(x) > 0 ∨ x ∈ S
S

To simplify the notations, we write p(x)dx = p(x)dx.

R R
S

p(x)

support S

Figure 5.1: A continuous probability density function

1
The term “distribution” is usually used in the discrete case but in this course we use it also in
the continuous case

89
 5 Bayesian filtering

[Link] Bayes Theorem

For two variables, the joint distribution is p(x, y).
Integrating p(x, y) over the variable x gives the marginal p(y):
Z
p(y) = p(x, y)dx

The conditional p(x|y) is defined by

p(x, y)
p(x|y)=
ˆ
p(y)
This can be written as,

p(x, y) = p(x|y)p(y) = p(y|x)p(x)

This gives the Bayes Theorem:

p(y|x)p(x)
p(x |y ) = (5.1)
p(y)
The marginal distribution p(y) can be seen as a function that normalizes p(x |y )
(such that its integral over the support equals one).
p(y) can also be rewritten as:
Z
p(y) = p(y|x)p(x)dx

This equation is also known as the total probability theorem.

Bayes’ formula can therefore be rewritten as:

p(y|x)p(x)
p(x |y ) = R
p(y|x)p(x)dx
Eq 5.1 can be rewritten:

p(x |y ) = ηp(y|x)p(x), η being a normalizing function

From this point of view, the Bayes Theorem can be interpreted as the multiplica-
tion of two functions.
Bayes’ formula is generalized as follows:

p(y|x, z)p(x|z)
p(x|y, z) =
p(y|z)

p(x|y, z) = ηp(y|x, z)p(x|z)

[Link] Probabilistic data fusion

Let us interpret Bayes’ formula ??. The unknown state is x and y refers to an
observation (the information provided by a sensor for instance).
We have different distributions:

• p(x) is the prior distribution and indicates what values of x might be expected
before having an observation,

• p(x |y ) is the posterior distribution,

90
 5 Bayesian filtering

• p(y|x) is the conditional distribution and serves the role of a sensor model
when we do estimation.

The conditional distribution p(y|x) can be thought in two ways:

• When building a sensor model, the distribution is constructed by fixing the

value x = xp (“p” as “Particular value”). The distribution depends then only
on the variable y.
p(y|x = xp ) is considered as a distribution in y.

• Conversely, once the sensor model exists, observations (numbers not distribu-
tions) are made:
y = ym (“m” as “Measured”). This is a fixed value.
From this, we want to infer the state x.
p(y = ym |x) is now a distribution in x. This distribution is known as the
Likelihood Function.

In a practical implementation, p(y|x) is constructed as a function of both variables

(or a matrix in discrete form). For each fixed value of x, a distribution in y is
defined. Therefore as x varies, a family of distribution in y is created.

Gaussian example Consider a continuous valued state x, the position of a robot

to a wall for example, and an observation y. Suppose the observation is a direct
measurement of the state. The observation model is here:

y =x+β
A commonly used probabilistic model for such an observation is a Normal (Gaus-
sian) distribution with mean x and a variance σ 2 as [6]:

1 (y − x)2
p(y|x) = η exp(− ) (5.2)
2 σ2
It should be clear that this is a simple function of both y and x.
If we know the true value of the state, x = xr , then the distribution is a function of
y only; describing the probability of observing a particular value of range (Normally
distributed around the true range x with variance σ 2 ).
Conversely, if we make a specific observation, y = ym , then the distribution is
a function of x only; describing the probability of the true range value (Normally
distributed around the range observation ym with variance σ 2 ). In this case, the
distribution is the Likelihood Function.
Now assume that we have some prior belief about the true state x encoded in a
Gaussian prior as

1 (x − xp )2
p(x) = ηp exp(− )
2 σp2
This is a function of a single variable x (with xp fixed). Bayes theorem can be
directly applied to combine this prior information with information from a sensor,
modeled by 5.2. First, an observation (technically, an experimental realization), ym ,
is made and instantiated in Equation 5.2. This is the sensor model:

1 (ym − x)2
p(y = ym |x) = η exp(− )
2 σ2

91
 5 Bayesian filtering

Then the prior and sensor model are multiplied together to produce a posterior
distribution (which is a function of x only):

1 (ym − x)2 1 (x − xp )2
p(x|y = ym ) = η2 exp(− ) exp(− )
2 σ2 2 σp2
η2 is a normalizing constant that doesn’t depend on x.
The product of two Gaussians is a Gaussian (Fig. 5.2):

1 (x − x)2
p(x|y = ym ) = η2 exp(− )
2 σ2
The values x and σ 2 are easy to find.

0.05
likelihood
0.045 prior
posterior
0.04

0.035

0.03

0.025

0.02

0.015

0.01

0.005

0
0 1 2 3 4 5 6 7 8 9 10

Figure 5.2: Data fusion of two Gaussians. It can be seen that the result is less
uncertain.

5.2.2 Discrete case

[Link] Discrete world
The state space can be discrete by nature (as the map-matching shown later) or
discretized in space (e.g. a grid to solve a localization problem - see example later).

p(x)

support S

Figure 5.3: A discretized distribution on a continuous space. Each cell is represented

by an arrow having a length proportional to the probability.

92
 5 Bayesian filtering

In discretized space2 , the distribution in this case and is given by:

S
p(x) = ω i .δ(x − xi )
X

i=1
where
• S is the number of elements xi of the support, d
• δ() isRthe Dirac delta function such that δ(x − a) = 0 for every x ̸= a and such
that δ(x)dx = 1,
• and the probabilities (or masses) ω i = P r(x = xi )3 of the components comply
with the following conditions:
ω i = 1 et ω i > 0 ∨ xi ∈ S
X

i
It can be noted that the identifier of a discrete element is an exponent, not to be
confused with indexes that indicate a temporal sampling as xk .
In practice, the distribution is represented by a vector:

p(x) = [ω 1 , ..., ω i , ..., ω N ]

where N is the number of elements of the support.

In the discrete case, the Bayes formula is written as:

p(y|x).p(x)
p(x |y ) = P
p(y|x).p(x)
x
It can be written with a normalization term η:

p(x |y ) = η.p(y|x).p(x)

1
η=P (5.3)
p(y|x).p(x)
x

Equation (5.3) shows that the normalization term η can be calculated once every
p(y|x).p(x) has been calculated to make the distribution sum to one.
Please note that with discrete distributions, the product of the Bayes formula
is an element-wise product (term by term) of two vectors. Sometimes it is clearly
stated by using the symbol ⊗ (element-wise product).

If x and y are two discrete variables, then a marginal is calculated by summing

over y the joint distribution:

p(x) =
X
p(x, y)
y

p(x, y) can be computed as:

p(x, y) = p(x|y).p(y)
again with an element-wise product.
2
Attention: this is a spatial discretization of the support S (not a time discretization)
3
P r is used for “Probability”

93
 5 Bayesian filtering

[Link] Joint, marginal and conditional distributions with two variables

Suppose that two discrete variables can take values in {1, 2, 3}.
The joint distribution is given by the following table. Check that the sum of all
the elements equals 1.
p(x, y) x=1 x=2 x=3
y=1 0.1 0.2 0
y=2 0.1 0.3 0.1
y=3 0 0.1 0.1

The marginal p(x) is obtained by summing the y elements of the columns. It is

now a vector.
x=1 x=2 x=3
p(x) 0.2 0.6 0.2

The conditional distribution p(y|x) is obtained such that every column sums to
1. Please note that it remains a 2D matrix.
p(y|x) x=1 x=2 x=3
y=1 0.5 0.33 0
y=2 0.5 0.5 0.5
y=3 0 0.166 0.5

For each fixed value of x, a distribution in y is defined. In other words, if x is

known then we get a distribution of y for this value of x. Therefore as x varies, a
family of distributions in y is created.
The conditional distribution p(x|y) is obtained such that every row sums to 1.

p(x|y) x=1 x=2 x=3

y=1 0.33 0.66 0
y=2 0.2 0.6 0.2
y=3 0 0.5 0.5

For each fixed value of y, a distribution in x is defined.

94
 5 Bayesian filtering

[Link] Example: probabilistic map-matching

Consider a simple example of the application of Bayes theorem to estimating a
discrete parameter on the basis of some observation and some prior information.
We want to know on which route travels a vehicle.
The environment of interest is modeled by a single state x which can take on one
of two values:
• x1 : the vehicle is on road 1

• x2 : the vehicle is on road 2

We have two criteria y for map-matching. They are observations made with the
current estimate of the pose:
• y 1 : normalized angle difference between the estimated heading and the head-
ings of the roads candidates

• y 2 : normalized distance between the estimated position and the considered

roads

x2

x1

Figure 5.4: Estimated pose of the vehicle with respect to the map

For the current situation, we have the following models described by following
likelihood matrices p(y|x).
Since the heading is close to road 2:
x1 x2
y1 0.1 0.9
Since the distance is almost the same with the two roads:
x1 x2
y2 0.45 0.55
Every row is a Likelihood Function.
Suppose we have no prior information (first map-matching), that is P r(x = x1 ) =
0.5 and P r(x = x2 ) = 0.5.
So, the prior distribution is the vector

p(x) = [0.5, 0.5].

The likelihood distribution p(y 1 |x) is

95
 5 Bayesian filtering

p(y 1 |x) = [0.1, 0.9].

The posterior distribution p(x|y 1 ) is obtained trough the Bayes formula

p(x |y ) = ηp(y|x).p(x)
η being a normalization constant.
By using the element-wise product ⊗ to make that clear, we have:

p(x|y 1 ) = ηp(y 1 |x) ⊗ p(x)

The result is
p(x|y 1 ) = [0.1, 0.9].

Suppose now that, from a previous stage, we knew that the vehicle was more
likely on road x2 with the prior knowledge P r(x = x1 ) = 0.3 and P r(x = x2 ) = 0.7.
This is a sequential map-matching.

p(x) = [0.3, 0.7].

We use again p(y |x) = [0.1, 0.9] and we get:
1

p(x|y 1 ) = [0.0455, 0.9545].

The belief in road x2 is increased.

Let us consider the second criterion. Note that it is in accordance with the first
one but provides little information.

p(y 2 |x) = [0.45, 0.55].

The Bayes formula gives

p(y 2 |x, y 1 ).p(x|y 1 )

p(x|y 1 , y 2 ) =
p(y 2 |y 1 )
The notion of conditional independence appears here.
It is reasonable to assume that the only thing the two observations have in common
is the underlying state x. So the observations are independent once the state is
known; that is, the observations are conditionally independent given the state. That
means

p(y 2 |x, y 1 ) = p(y 2 |x)

So we can write:

p(x|y 1 , y 2 ) = η2 p(y 2 |x).p(x|y 1 ) = η3 p(y 2 |x).p(y 1 |x).p(x)

So we have tree distributions to multiply element by element.
In fact, the development of p(x|y 1 ) is not necessary since we have already com-
puted it before. We can do a sequential update of the belief.
We get :

p(x|y 1 , y 2 ) = [0.0375, 0.9624]

The belief to be on road x2 is increased again.

96
 5 Bayesian filtering

5.3 Bayesian Filtering

Bayesian filtering consists in sequentially estimating the posterior distribution p(x|y)
characterizing the distribution of the state x in the state space knowing the measures
y from prior distribution. p(x|y) is generally difficult to characterize while the
likelihood p(y|x) is easier to calculate from an observation model.
A Bayesian filter works in three stages, in a way similar to an estimator/predictor
filter [4, 21]:

• Measurement update

• State estimate computation

• Time update (or prediction)

Consider the non-linear system represented by the following discrete time equations:

xk+1 = f (xk , uk ) + αk
(
(5.4)
yk = g (xk ) + βk
where αk and βk represent the model and observation noises with distribution pα
and pβ respectively. They are supposed additive, white and independent. Here, the
input uk is known.
Based on these hypotheses, the system is said to be “Markov”. The Markov
hypothesis states that any knowledge about the system is contained in the current
state xk .

5.3.1 Observation Update

Denote y1:k = {yi }i=1,··· ,k all observations up to time k.
Consider the nonlinear observation model yk = g (xk ) + βk .
Use Bayes’ rule:

p(y|x).p(x)
p(x|y) =
p(y)
Bayes’ theorem allows to move from a prior distribution p(x) to a posterior p(x|y)
using the likelihood p(y|x) of the observations.
Considering all the measures y1:k , we have:

p (yk |xk , y1:k−1 ) .p (xk |y1:k−1 )

p (xk |y1:k ) = p (xk |yk , y1:k−1 ) = (5.5)
p (yk |y1:k−1 )
Use again the conditional independence assumption:

p (yk |xk , y1:k−1 ) = p (yk |xk )

The likelihood function p (yk |xk ) is given by the model of the observation noise.

p (xk |y1:k ) = ηk .p (yk |xk ) .p (xk |y1:k−1 ) (5.6)

where ηk is the normalization term.

97
 5 Bayesian filtering

5.3.2 State and Covariance Estimates

The goal is to find the first two statistical moments. From the estimation of the
posterior distribution p (xk |y1:k ), an estimate4 can be obtained by considering the
following integral over the support of the distribution:
Z
x̂k = xk .p (xk |y1:k ) dxk (5.7)
This is the definition of mathematical expectation. For a discrete distribution, it is
a weighted sum of every element.
The covariance matrix can be calculated from:
Z
Pk = (xk − x̂k ) (xk − x̂k )T p (xk |y1:k ) dxk (5.8)
For a discrete distribution, it is a weighted sum of matrices.

5.3.3 Prediction (time-update)

We are here interested in the prediction step and the calculation of p(xk+1 |y1:k ).
An idea is to rewrite it as a marginal distribution with respect to xk−1 :
Z
p (xk+1 |y1:k ) = p (xk+1 , xk |y1:k ) dxk (5.9)

Use p(x, y) = p(x |y)p(y) into the function to be integrated (5.9):

p (xk+1 , xk |y1:k ) = p (xk+1 |xk , y1:k ) p (xk |y1:k ) (5.10)

The Markov hypothesis states that any knowledge about the system is contained
in the state xk . This conducts to the conditional independence.

p (xk+1 |xk , y1:k ) = p (xk+1 |xk ) (5.11)

Using Eq. (5.11), a new expression is obtained5 (5.9):
Z
p (xk+1 |y1:k ) = p (xk+1 |xk ) p (xk |y1:k ) dxk (5.12)
This is a convolution product.

4
called MMSE - Minimum Mean-Square Error
5
Known as Chapman-Kolmogorov equation

98
 5 Bayesian filtering

5.4 Grid filtering

Bayesian filtering is often impossible to solve analytically, unless the system is linear
and if the noises follow Gaussian distributions. In this case, the solution is given by
the Kalman filter.
The grid filter is an exact and optimal numerical solution. It characterizes the
distribution throughout the discretized space. It is applicable if and only if the state
space is discrete and if it contains a finite number of states [21].
A discretized distribution is managed by N Dirac deltas δ() each one having a
weight ω.
Mathematically, a distribution is a weighted sum over all the Dirac deltas:
N
p (xk ) = ωki .δ(xk − xik ) (5.13)
X

i=1

where:

ωki = Pr(xk = xik )

N
ωki = 1
X

i=1

In a grid filter, the center of each cell contains a Dirac delta. The grid is handled
by nxn table. In 2D, it is a matrix. The elements of the grid contains the values ωki .

5.4.1 Static example

Imagine that the size of the state equals 2. The distribution p(x, y) can be described
in this case by a matrix, the values of which are the probabilities of the cells (Fig.
5.5).

Figure 5.5: Discrete Gaussian distribution. The grid is displayed in a 2D plane.

Consider now the data fusion problem of two sensors that are fixed in the envi-
ronment and that observe a common space to localize a target that is fixed.
We have the prior knowledge p(x, y) shown in Fig. 5.6.

99
 5 Bayesian filtering

Figure 5.6: Discrete prior distribution on a grid

A sensor (called sensor 1) located at x = 35, y = 0 km now takes an observation

of the target. The likelihood function p(z 1 |x, y) generated from this sensor following
an observation z 1 is shown in figure 5.7. This likelihood consists of a general location
probability defined on the x-y grid. It is described by a matrix which has exactly
the same size as the matrix describing p(x, y).
The likelihood shows that the bearing resolution of the sensor is high, whereas it
has almost no range accuracy (the likelihood is long and thin with probability mass
concentrated on a line running from sensor to target).

Figure 5.7: Likelihood p(z 1 |x, y) of the first sensor

The posterior distribution having made this first observation is shown in figure
5.8and is computed from the point-wise product of prior and likelihood matrices :

p(x, y|z 1 ) = η.p(z 1 |x, y)⊗p(x, y)

100
 5 Bayesian filtering

The resulting matrix is normalized such that the sum of all its elements sum-up
to one.
It can be seen (Fig 5.8) that the distribution defining target location is now
approximately restrained to a line along the detected bearing.

Figure 5.8: Posterior distribution after having used sensor 1

A second sensor (sensor 2) now takes observations of the target from a location x =
0, y = 50. Figure 5.9 shows the target likelihood p(z 2 |x, y) following an observation
z 2 by this sensor. It can be seen that this sensor (like sensor 1) has high bearing
resolution, but almost no range resolution. However, because the sensor is located
at a different site, we would expect that the combination of bearing information
from the two sensors would provide accurate location data.

Figure 5.9: Likelihood p(z 2 |x, y) of the second sensor

Indeed, following point-wise multiplication of the second sensor likelihood with

the new prior (the posterior p(x, y|z 1 ) from the previous two observations of sensor

101
 5 Bayesian filtering

1), we obtain the posterior p(x, y|z 1 , z 2 ) shown in figure 5.9 which indicates that the
probability mass is now highly concentrated around a single target location.

Figure 5.10: Final posterior

5.4.2 Dynamic grid filter

Suppose a prediction p (xk |y1:k−1 ) is known.

Observation Update
To obtain the posterior, we use Bayes’ rule to update the weights of the cells ac-
cording to their measurement likelihoods as we did before.

p (xk |y1:k ) = ηk .p (yk |xk ) .p (xk |y1:k−1 )

We use the likelihood of the observation for each cell and multiply it with the
prior. By normalizing the weights, we obtain:

p (yk |xik ) ωk|k−1

i
i
ωk|k = PN 
j

j
j=1 p yk |xk ωk|k−1

The new weights describe the updated distribution:

N
p (xk |y1:k ) = i
.δ(xk − xik )
X
ωk|k
i=1

Estimation of the state

The question that now arises is which state estimate can be computed with the grid.
The grid elements provide an estimate of the probability of the state distribution.
If it is known (or if it is assumed) that the distribution estimate is uni-modal
(there is only one solution), then we can say that the estimate of the state is
1. The highest weight cell6 ,
6
this is the maximum a posteriori MAP

102
 5 Bayesian filtering

2. or the grid centroid7 .

Conversely, if the distribution estimate is multi-modal, then there may be many

several possible solutions. This property is of great interest in applications such as
target tracking, when we don’t know a priori how many targets there can be.

Prediction
The state of the system is changing over time.
We use the evolution model to make the grid evolve.
Using Eq. (5.13) in Eq. (5.12), we get:
N
Z !
p (xk+1 |y1:k ) = p (xk+1 |xk ) i
xik )
X
ωk|k .δ(xk − dxk
i=1

The integral of a sum is the sum of the integrals:

N Z
p (xk+1 |y1:k ) = i
.p (xk+1 |xk ) δ(xk − xik )dxk
X
ωk|k
i=1

A fundamental property of the Dirac delta function is to transform an integral

into a simple sum.
Look at cell i:
 
Z XN  
i
ωk|k .p (xk+1 |xk ) δ(xk − xik )dxk =  ωk|k
j
.p xik+1 xjk  δ(xk − xik )
j=1

The mass in cell i is the sum of the mass of all cells that move into it with respect
to their probability thanks to the evolution model.
Denote
N  
i
= j
ωk|k .p xik+1 xjk
X
ωk+1|k
j=1

Therefore, the predicted distribution is characterized by another table:

N
p (xk+1 |y1:k ) = i
.δ(xk − xik )
X
ωk+1|k
i=1

In practice, to efficiently update the grid upon robot motions, one typically as-
sumes a bounded Gaussian model for the motion uncertainty [24]. The update is
then done by shifting the data in the grid according to the measured motion (cf
figure 5.11). In a second step, the grid is then convolved using a Gaussian Kernel
(cf figure 5.13) to take into account the uncertainty on the knowledge of the motion
(cf figure 5.12). Indeed, the probability density of the sum of two independent real
random variables is the convolution product of the probability densities of these two
variables.
The evolution model can be given by another grid containing estimates of the
speed of every occupancy cell (this technique is called Bayesian Occupancy Filter -
BOF) [5].

7
this is the MMSE estimate

103
 5 Bayesian filtering

Figure 5.11: Grid uncertainty after having applied the motion of the robot.

Figure 5.12: Grid uncertainty after convolution with the Gaussian kernel. It can be
noted that the uncertainty is increased.

104
 5 Bayesian filtering

Figure 5.13: Gaussian kernel representing the model uncertainty.

5.4.3 Comments on the grid filter

The grid filter requires a bounded state space, which is not always possible.
The grid filter becomes very computational when the size of the state space is
high since:

• The tables encoding the distribution become of high dimension,

• The prediction itself needs many computation since all the cells that change
have to be evaluated with a convolution product.

In practice, one prefers to use alternative methods such that the particle filter.

105
 5 Bayesian filtering

5.5 Particle filtering

The grid filter is optimal (if the grid is fine) but it requires a lot of calculation if the
space is large and it requires a bounded state space, which is not always possible.
A sub-optimal solution of Bayesian filtering (equations 5.12 and 5.6) is particle
filtering. It calculates sequentially p(xk |y1:k ) from a given initial distribution using
random numbers generation.

5.5.1 Principle
The main idea of particle filtering is to approximate p (xk |y1:k ) by a finite set of
N
particles with associated weights {xik , ωki }i=1 . The weight of each particle reflects
the probability that the particle is at a relevant position of the state space. This
approach is called importance sampling.
Sequential Monte Carlo methods allow an implementation of a Bayesian filter
through numerical approximations. This is the Particle Filter. Discrete samples are
obtained by simulation to approximate a probability distribution that we can not
calculate analytically. The greater the number of particles, the better the approxi-
mation. With the evolution of computer technology, these filters have become very
popular and are applied in robotics applications, such as positioning problems [7].

5.5.2 Monte Carlo Integration and random generator issue

When we estimate the state, we want to compute an integral like follows:
Z
E (h (x)) = h (x) .p (x) dx

For instance, Z
x̂k = xk .p (xk ) dxk
N
If it is possible to generate samples {xi }i=1 according to p (x), then a Monte Carlo
approximation of the integral is given by:

1 XN  
E (h (x)) ≈ h xi
N i=1

Imagine that we cannot generate samples from p (x) but there is another distri-
bution q (x) from which we can generate samples.
The integral in this case is
Z
p (x)
E (h (x)) = h (x) q (x) dx
q (x)

q (x) is the random generator (or instrumental generator).

1 XN   p (xi )
E (h (x)) ≈ h xi
N i=1 q (xi )
Let denote

p (xi )
ω =i
q (xi )

106
 5 Bayesian filtering

the weights. We therefore have

1 XN  
E (h (x)) ≈ h xi ω i
N i=1

Example By Monte Carlo integration, we try to calculate the mean and variance
of a normal distribution with mean m = 5 and standard deviation σ = 1, 5.

e− 2 ( )
1 x−m 2
p(x) = √1
σ 2π
σ

Imagine that we only have a uniform random number generator (which in addition
leads to very simple expressions as we shall see).
For example, generate 100 samples xi on a fairly wide support (it is better to have
a too large than a too small support).
Calculate the mean (5.7):
Z
p (x)
m= x q (x) dx
q (x)
q (x) being constant, we have:
1Z
m= xp (x) q (x) dx
η
where η = S1 is the inverse of the length S of the support of the uniform law.
We then calculate the value of each sample by the function of interest (which is
here the probability density p (x)).
Drawing N samples according to q, it is achieved to calculate the mean of p (x)
by a Monte Carlo method:

xi p (xi )
PN
i=1
m ≈ S.
N

0.30

0.25

0.20

0.15

0.10

0.05

0.00
-2 0 2 4 6 8 10 12

Figure 5.14: 100 samples drawn uniformly

With 100 samples, we get: m

c = 5.0282.

107
 5 Bayesian filtering

0.30

0.25

0.20

0.15

0.10

0.05

0.00
-2 0 2 4 6 8 10 12

Figure 5.15: 1000 samples drawn uniformly

With 1000 samples, we get: m c = 5.0121, what is better and perfectly illustrates
the fact that as the number of samples increases, the numerical calculation by Monte
Carlo is better.

function y=p(x) //Normal Law

m=5; std=1.5;
y=1/(std*sqrt(2*%pi))*exp(-0.5*((x-m)/std)^2);
endfunction
t=linspace(-2,12,1000); plot(t,p(t));
N=1000; support=10;
for i=1:N x(i)=support*(rand()); end
plot(x,0.01*ones(N,1),’.’); plot(x,p(x),’r.’)
m_hat=support*sum(x.*p(x))/N

Table 5.1: Scilab program. The instrumental generator is uniform with a support of
length 10

5.5.3 Computation of the weights of the particles

We will see in this section that the weights can be recursively estimated [22].
Suppose we known the posterior distribution which is a weighted sum over all the
Dirac deltas:
N
p (xk |yk ) = ωki .δ(xk − xik )
X

i=1

Let us sample the particles with a random generator q (x). We need to modify
the weights accordingly:

p (xik |yk )
ωki =
q (xik |yk )

108
 5 Bayesian filtering

p (yk |xik ) p (xik )

ωki =
q (xik |yk )
For every particle there is a relationship between ωki and ωk−1
i
.
A way to establish this relationship is to consider a more general case in which
one is interested not only in the current state xk but the entire sequence from the
beginning of time x0:k [17].

p (y1:k |xi0:k ) p (xi0:k )

ωki =
q (xi0:k |y1:k )

Yet
       
q xi0:k |y1:k = q xi0:k−1 , xik |y1:k = q xi0:k−1 |y1:k · q xik xi0:k−1 , y1:k

As we use a causal instrumental generator (the past doesn’t depend on the present)
and yk has to be removed:
   
q xi0:k−1 |y1:k = q xi0:k−1 |y1:k−1

With the Markov and conditional assumptions:

   
q xik xi0:k−1 , y1:k = q xik xik−1 , yk

and so:
p (y1:k |xi0:k ) p (xi0:k )
ωki =    
q xi0:k−1 |y1:k−1 · q xik xik−1 , yk
   
Multiplying the numerator and the denominator by p y1:k−1 xi0:k−1 p xi0:k−1 :
   
p y1:k−1 xi0:k−1 p xi0:k−1 p (y1:k |xi0:k ) p (xi0:k ) 1
ωki =   ·    ·  
q xi0:k−1 |y1:k−1 p y1:k−1 xi0:k−1 p xi0:k−1 q xik xik−1 , yk

p (y1:k |xi0:k ) p (xi0:k ) 1

ωki = ωk−1
i
·    ·  
p y1:k−1 xi0:k−1 p xi0:k−1 q xik xik−1 , yk
Develop the two terms of the numerator and note that measures y1:k−1 can not
depend on xik :
       
p y1:k xi0:k = p yk , y1:k−1 xi0:k = p yk y1:k−1 , xi0:k .p y1:k−1 xi0:k−1

     
p xi0:k = p xik xi0:k−1 .p xi0:k−1
     
p (yk |y1:k−1 , xi0:k ) .p y1:k−1 xi0:k−1 .p xik xi0:k−1 .p xi0:k−1 1
ωki = ωk−1
i
·     ·  
p y1:k−1 xi0:k−1 p xi0:k−1 q xk xik−1 , yk
i

Simplifications appear:
 
p (yk |y1:k−1 , xi0:k ) .p xik xi0:k−1
ωki = ωk−1
i
·  
q xik xik−1 , yk

109
 5 Bayesian filtering

Use the Markov assumption:

   
p xik xi0:k−1 = p xik xik−1

   
p yk y1:k−1 , xi0:k = p yk xik
Finally, we obtain the following expression:
 
p (yk |xik ) p xik xik−1
ωki = ωk−1
i
·   (5.14)
q xik xik−1 , yk
 
The random generator q xik xik−1 , yk can be chosen to compute samples using
the previous state and the current measurements.

5.5.4 Bootstrap Particle Filter

Equation (5.14) provides a sequential mechanism
 to estimate
 the weight of impor-
tance, given the instrumental law draw q xk xk−1 , yk . The choice of this distribu-
i i

tion is a crucial point.

A common choice (called "bootstrap") is
   
q xik xik−1 , yk = p xik xik−1 (5.15)

In other words, the particles are simply drawn by the prediction model8 without
using the current measurement.
It is very popular since it leads to a simple formulation
 
ωki = ωk−1
i
· p yk xik (5.16)
Thus, updating the weight of the particles simply consists in multiplying the
weight of each particle by the likelihood.
The Bootstrap Particle Filter is not the more powerful technique, but is very easy
to implement.

5.5.5 State and covariance estimation

Suppose we do a Bootstrap PF and we have a model of the measurement noise
distribution (denoted pβ ), for instance a Gaussian model.
The weights of the particles are updated as follows:

  
ω ik = ω ik| · pβ yk − g xik|k−1 i = 1, . . . , N
The likelihood of each particle is evaluated individually.
We normalize the weights:
ωi
ω̃ki = P k i
ωk
Estimates of the state vector and of the covariance matrix are given by:
x̂k|k =
PN
i=1 ω̃ki · xik
   T
Pk|k =
PN i i i
i=1 ω̃k · xk − x̂k|k · xk − x̂k|k
8
That is to say, the evolution model of the state and the model noise

110
 5 Bayesian filtering

5.5.6 SIS Algorithm (Sequential Importance Sampling)

For operating a particulate filter, it is necessary to know the noise models and in
addition to have a random number generator. The bootstrap particle filter algorithm
for a non linear state space can be summarized as follows:

%Initialization
Generate N particles xi using the prior distribution
i.e. distribute them and give them a weight ω i
Sampled loop
y←Get_measure() %sensor data
%Update the weight of each particle according to its likelihood
  
ω i = ω i · p β y − g xi i = 1, . . . , N
i
%Normalize the weights ω i = Pω ωi
%Estimate the state and cov and provide the result
x̂ = N i
i=1 ω · x
i
P
T
P = N i=1 ω · (x − x̂) · (x − x̂)
i i i
P

%Prediction (next step is prepared)

%Draw randomly N samples α according to pα

αi ← M yRandomGenerator(N, pα )
%Add them to the particles passed into the evolution model
 
xi = f xi + αi
End Loop

Table 5.2: SIS particle filter (estimator/predictor implementation)

5.5.7 SIR Algorithm: resampling the particles set

The SIS particle filter presented above is not viable in practice because we can see
that weights quickly degenerate to zero. This is quite logical since this filter updates
only the weight of the particles, leaving them explore the state space using the noisy
model. Those who go astray have a weight becoming infinitely small and after few
samples the number of significant particles becomes too low.
To address this problem, we proceed with particles resampling according to their
weight: those with a low weight are eliminated and those that have a high weight
are possibly duplicated several times (their clones will follow different paths in the
state space as each model noise will be different). Generally, after a resampling step,
all particles have the same weight as shown in Figure 5.16.

111
 5 Bayesian filtering

prediction

Likelihood

Update

Resampling

Figure 5.16: Graphic illustration of a SIR Algorithm with resampling

Resampling is a fundamental step of particle filtering to prevent degeneration.

A SIS particle filter with resampling is called SIR - Sequential Importance Re-
sampling.
An implementation is presented in the following algorithm.

%Initialization
Generate N particles xi using the prior distribution
i.e. distribute them and give them a weight ω i
Sampled loop
y←Get_measure() %sensor data
%Update the weight of each particle according to its likelihood
%Normalize the weights
%Estimate the state and cov and provide the result
Resample the particles set
%Prediction
%Draw randomly N samples α according to pα
%Add them to the particles passed into the evolution model
End Loop

Table 5.3: SIR particle filter with a resampling done at every step

112
 5 Bayesian filtering

Initialization

Time and measurement

updates

Resampling

Time and measurement

updates

Time

Figure 5.17: Spatio-temporal illustration of the operations of a SIR filter

5.5.8 Resampling the SIR when necessary

Resampling can be done at each stage but, as this step requires a lot of computing,
it slows down the processing. Moreover, a too frequent resampling can induce a
impoverishment of the particle set.
A good technique is to do it only when it is necessary. This can be done by taking
into account the proportion of significant particles, i.e. the percentage of particles
that have relatively high enough weights.
To estimate the number of effective particles, one can use the following estimate:
1
Nef f = PN 2 (5.17)
1 (ωki )

• The number of effective particles is maximized when all the weights are equal.
In this case, Nef f = N .

• It is minimum when only a weight is not zero. In this case, the weight of this
particle is 1 and Nef f = 1.

So, when Nef f falls below a chosen threshold, we do a resampling.

113
 5 Bayesian filtering

%Initialization
Generate N particles xi using the prior distribution
i.e. distribute them and give them a weight ω i
Sampled loop
y←Get_measure() %sensor data
%Update the weight of each particle according to its likelihood
%Normalize the weights
%Estimate the state and cov and provide the result
If Nef f ≤ T h Then Resample the particles set
%Prediction
%Draw randomly N samples α according to pα
%Add them to the particles passed into the evolution model
End Loop

Table 5.4: SIR particle filter with a resampling done when necessary.

5.5.9 Strengths and weaknesses

The particle filter seems to be the perfect estimation technique. But keep in mind
the following disadvantages:

1. Heaviness of calculation, and difficulty of real-time implementation (even if it

can be parallelized with multiple processors),

2. Difficulty to determine the proper number of particles,

3. Risk of divergence in case low measurement noise variance or in case of aberrant

measure,

4. Convergence slowed due to random sampling.

There are many particle filtering enhancements, the most important are:

1. One can adapt the number of particles as a function of the complexity of the
problem (KLD-sampling).

2. If the state space contains contains linear parts (in evolution or observation),
one may proceed to do some factorizations (Rao-blackwellized particle filter).

3. One can improve the resampling and fight against its impoverishment with
MCMC (Markov Chain Monte Carlo) methods. The best known are Gibbs
and Metropolis-Hasting samplings [17].

4. You can change the random drawing law q (xk |x0:k−1 , y1:k ) for which takes
into account the current measurements. A simple and effective technique is
to modify the bootstrap filter by drawing several random particles for every
particle (typically between 3 and 10). The particle with the highest likelihood
is kept. This way, the drawing incorporates the current measure. This en-
hancement allows you to run a particle filter when the observation noise is
low. It also reduces the number of particles.

114
6 Frames transformation with
associated uncertainties
6.1 Introduction
Localizing a robot means knowing its position in a given frame. Since we use direct
orthonormal frames, each frame is completely defined by its pose, which gives the
position (2D or 3D) of the origin and the orientation (2D or 3D) of the frame. This
chapter is inspired from Elwan Héry’s PhD Thesis [8].
The operators ⊕ and ⊖ introduced by Smith, Self and Cheeseman [23] for frame
transformations are very efficient tools. These notations make it possible to lighten
the equations, by manipulating the transformations in frames as easily as the compo-
sition of the vectors in a Cartesian frame. In this document, we are only interested
in 2D poses characterized by vectors of dimension three, containing a Cartesian
position and a heading angle.
The pose k qi describing the translation and rotation to apply to the Rk frame to
transform it into the Ri frame is :
 k 
xi
k
qi = (6.1)
 k
 yi 

k
θi
k
qi represents also frame Ri in frame Rk , the heading angle of the abscissa axis of
Ri being k θi .
In addition, transformations have uncertainties; they are probabilistic with covari-
ance matrices. The propagation of these uncertainties during transformations can
use first order approximation of the equations that are non-linear due to rotations.
These error propagation mechanisms are only valid for small errors with respect to
non-linearities.

6.2 Notations
The pose q = (x y θ)T represents the position p = (x y)T and the orientation θ of a
robot. It is important to express each of these quantities in a given reference frame.
In case several robots are present, it is also useful to be able to differentiate the data
of the different robots. We use indices and exponents to solve these problems.
Table 6.1 represents the different parts of a given pose.
Table 6.2 shows the notations of the indices and exponents used for any given
data z . The index i at the bottom right represents the robot, linked to the data
z. The exponent in the upper left corner indicates the frame in which the data z is
expressed. Thus a given index i can refer to the pose or the frame. ẑ represents the
estimate of z, z ∗ its ground truth. The covariance matrix of z is expressed as Pz and
the cross-covariance matrix between two vectors z1 and z2 is expressed as Pz1 ,z2 .

115
 6 Frames transformation with associated uncertainties

Figure 6.1: Transformations of frame. The curved arrow k qi represents the transla-
tion and rotation to be applied to the Rk frame to transform it into the
Ri frame. The curvature of the arrow indicates the sign of the rotation
.

Table 6.1: Pose notations.

Symbol Signification Description
x Abscissa
y Ordinate
θ !
Orientation
x
p Position
y
 
x
q  y  Pose
 

By combining tables 6.1 and 6.2, we can define any element of the state for any
robot in a given reference frame. For example, i qj refers to the pose of robot j in
the frame i.

6.3 Transformation operators

6.3.1 Compounding operator ⊕
The compounding operator ⊕ is defined as follows (see Fig. 6.1):
k
qi =k qj ⊕ j qi (6.2)

It can be noted that the indices and exponents "j" are linked in a simple way.
The concepts of (direct orthonormal) frames, poses and frame transformations
contain the same information. By considering the poses as frame transformation

116
 6 Frames transformation with associated uncertainties

Table 6.2: Notations of indices and exponents.

Symbol Description
j
zi Data z of i expressed in the frame of j. j
may be omitted when this frame corresponds
to the world frame 0.
ẑ Estimate of z.
z ∗
Ground truth of z.
Pz Covariance matrix of z.
Pz1 ,z2 Cross-covariance matrix between z1 and z2 .

functions, we can then see the operator ⊕ as the operator ◦ of function composition
as well:
k
qi =k qj ⊕ j qi (6.3)
 
k
qi () =k qj j
qi ()
=k qj ◦ j qi () (6.4)

The resulting geometric operation of the operator ⊕ represents a transformation

from Rk to Rj (via the pose k qj ) followed by a transformation of Rj to Ri (j qi )
depending on the angle k θj . In other words, we combine the pose k qj (in blue on the
figure 6.1) and the pose j qi (in green) to obtain the pose k qi (in red). A common use
of this composition is the transformation of a pose expressed in theRj frame (the
right term) into another pose expressed in Rk via the left term.
We have:
k
qi =k qj ⊕ j qi
    
xj + j xi cos θj −j yi sin

k k k
θj
=
    
 yj + xi sin θj +j yi cos

k j k k
θj 

k
θj + j θi
   
cos 0
 
k k
θj −sin θj
=k qj +  (6.5)
    j
 sin cos 0 

 qi
k k
θj θj
0 0 1

Note that the operation to be applied to the right term depends on the left term.
The operator ⊕ is not commutative but is associative like the function composi-
tion.
In order to propagate the uncertainty represented by a covariance matrix, a first-
order approximation can be made:
!
Pk qj Pk qj ,j qi
Pk qi ≈ J⊕ J⊕T (6.6)
Pj qi ,k qj Pj qi
The cross-covariance sub-matrix Pk qj ,j qi is the symmetric matrix of the sub-matrix
Pj qi ,k qj and are often taken null in practice if this assumption holds.
The Jacobian matrix J⊕ is defined by :

117
 6 Frames transformation with associated uncertainties

   
∂ k qi ∂ k q j ⊕j qi ∂ k qj ⊕j qi
J⊕ = k j = = (6.7)
∂ ( q j , qi ) ∂ (k qj , j qi ) ∂ (k xj ,k yj , k θj , j xi , j yi , j θi )
       
1 0 −j xi sin θj −j yi cos cos 0
 
k k k k
θj θj −sin θj
=
       
 0 1 xi cos θj −j yi sin sin cos 0 
 
j k k k k
θj θj θj 
0 0 1 0 0 1
By calculating the pose k qi before the Jacobian, the latter can then be expressed
directly from the new coordinates without recalculating some sine and cosine. We
then obtain the original formula of Smith, Self and Cheeseman [23]:
     
1 0 − cos 0
 
k
yi − k yj k
θj −sin k
θj
J⊕ =  (6.8)
     
 0 1 sin cos 0 
 
k
xi − k xj k
θj k
θj 
0 0 1 0 0 1

6.3.2 Inversion operator ⊖

We can then define the inversion operator of the pose j qi :
i
qj = ⊖j qi
−j xi cos (j θi ) − j yi sin (j θi )
 

=  xi sin (j θi ) − j yi cos (j θi ) 
 j 

−j θi
−j xi cos (−j θi ) + j yi sin (−j θi )
 

=  −j xi sin (−j θi ) − j yi cos (−j θi )  (6.9)

 

−j θi
cos (−j θi ) −sin (−j θi ) 0 
 

=  sin (−j θi ) cos (−j θi ) 0  −j qi (6.10)
 

0 0 1
By considering the poses as functions for changing frames, the operator ⊖ is
equivalent to inverting this function:

i
qj = ⊖j qi (6.11)
i
qj () =j qi−1 () (6.12)
In the same way as for the operator ⊕, by applying a first-order approximation,
we can propagate the covariance matrix:

Pi qj ≈ J⊖ Pj qi J⊖T (6.13)
with
−cos (j θi ) −sin (j θi ) xi sin (j θi ) − j yi cos (j θi ) j
 
∂ qj ∂ (⊖ qi ) 
i j
J⊖ = j = =  sin (j θi ) −cos (j θi ) j
xi cos (j θi ) + j yi sin (j θi ) 
∂ qi ∂ j qi

0 0 −1
(6.14)
This expression can be simplified from the coordinates of qj :
i

−cos (j θi ) −sin (j θi ) i yj
 

J⊖ =  sin (j θi ) −cos (j θi ) −i xj  (6.15)

 

0 0 −1

118
 6 Frames transformation with associated uncertainties

Property
We have:

j
qi = ⊖i qj = ⊖ ⊖ j qi (6.16)

If we reverse a transformation and reverse it a second time, we find the initial

transformation.
In addition, if we transform the covariance matrix in one direction and transform
it backwards, it is unchanged.

Demonstration
By propagating the uncertainty of the covariance matrix Pj qi twice:

Pj qi ≈ J⊖′ J⊖ Pj qi J⊖T J⊖′T (6.17)

with
−cos (i θj ) −sin (i θj ) j yi
 

 sin (i θj )
J⊖′ =  −cos (i θj ) −j xi 
 (6.18)
0 0 −1
−cos (j θi ) sin (j θi ) j
 
yi
=  −sin ( θi ) −cos ( θi ) − xi 
j j j
 

0 0 −1

By developing

c2 + s2 cs − sc −i yj c − i xj s − j yi
 

J⊖′ J ⊖ =  sc − cs s2 c2

−i yj s + i xj c +j xi  (6.19)
0 0 1

with c = cos (j θi ) and s = sin2 (j θi ).

From the equation (6.9) (for the equation j qi = ⊖i qj )
       
−i yj cos j
θi − i xj sin j
θi − j yi = −i yj cos i θj + i xj sin i θj − j yi = 0 (6.20)
       
−i yj sin j
θi + i xj cos j
θi +j xi = i yj sin i θj + i xj cos i θj +j xi = 0 (6.21)
Then we have
1 0 0
 

J⊖′ J ⊖ =  0 1 0  (6.22)
 

0 0 1

As
1 0 0
 
 T
J⊖T J⊖′T = J⊖′ J ⊖ = 0 1 0 

 (6.23)
0 0 1

We have
Pj qi = J⊖′ J⊖ Pj qi J⊖T J⊖′T = Pj qi (6.24)

119
 6 Frames transformation with associated uncertainties

6.3.3 Composition of the operators

The two operators can be combined. For example, k qj can be determined based on
two poses with uncertainty noted k qi and j qi .
We can write k qj =k qi ⊕i qj but we know j qi and not i qj .
   
cos 0
 
k k
θi −sin θi
k
qj =k qi + 
    i
 sin cos 0 

 qj
k k
θi θi
0 0 1
 
=k qi ⊕ ⊖j qi (6.25)
   
cos 0
 
k k
θi −sin θi cos (−j θi ) −sin (−j θi ) 0 


=k qi +  sin (−j θi ) cos (−j θi ) 0  j
   
sin cos 0
 

k
θi k
θi 
  − qi
0 0 1 0 0 1
   
cos 0
 
k
θi −j θi −sin k
θi −j θi
 j 
=k qi + 
   
 sin cos 0

k
θi − θi j k
θi − θi j  − qi

0 0 1
    
xi + −j xi cos θi −j θi + j yi sin

k k k
θi −j θi
=
    
 yi + − xi sin θi −j θi − j yi cos

k j k k
θi −j θi 

k
θi −j θi

By convention we note  
k
qi ⊕ ⊖j qi = k qi ⊖ j qi (6.26)
So,

k
qj = k qi ⊖ j qi (6.27)
Now, let us start from:

k
qi =k qj ⊕ j qi (6.28)

we can write

k
qi ⊖ j qi =k qj ⊕ j qi ⊖ j qi (6.29)

which gives the same result after simplification:

k
qi ⊖ j qi =k qj (6.30)

The new covariance matrix is approximated by:

! !
Pk qi Pk qi ,i qj Pk qi Pk qi ,j qi J⊖T
Pk qj ≈ J⊕ J⊕T = J⊕ J⊕T (6.31)
Pi qj ,k qi Pi qj J⊖ Pj qi ,k qi J⊖ Pj qi J⊖T

Similarly, we can determine j qi from j qk and k qi by starting from the classical

composition:

120
 6 Frames transformation with associated uncertainties

j
qi =j qk ⊕ k qi
 
= ⊖k qj ⊕ k qi (6.32)
By developing, we obtain:

   
cos −k θj 0
  
−sin −k θj
  k  k
j
qi =  (6.33)
   
 sin − θj cos −k θj 0 

 − qj  ⊕ qi
 k 

0 0 1
   
cos −k θj 0
 
−sin −k θj
 
= k
   
 sin − θj cos −k θj 0 

 − qj
k

0 0 1
   
cos −k θj 0
 
−sin −k θj
+ k
   
 sin − θj cos −k θj 0 
 
 qi
k

0 0 1
   
cos sin 0
 
k k
θj θj  
= k
qi − k qj
   
cos 0 
 
k k
 −sin θj θj 
0 0 1
       
xi − k xj cos θj + yi − k yj sin
 
k k k k
θj
=
        
xi − k xj sin θj + k yi − k yj cos

.
k k k
 − θj 
k
θi − k θj
By convention we will note:
 
⊖k qj ⊕k qi = ⊖k qj ⊕k qi , (6.34)
which allows for elegant manipulations.
For example, starting from the first equation (6.2)

k
qi =k qj ⊕ j qi , (6.35)
we write
⊖k q j ⊕ k qi = ⊖k q j ⊕k q j ⊕ j qi , (6.36)
we have the same result after simplifications:

⊖k qj ⊕ k qi =j qi . (6.37)
The covariance matrix is then obtained:

! !
Pj qk Pj qk ,k qi J⊖ Pk qj J⊖T Pk qj ,k qi J⊖T
Pj qi ≈ J⊕ J⊕T ≈ J⊕ J⊕T . (6.38)
Pj qk ,k qi Pk qi J⊖ Pk qj ,k qi Pk qi
Smith and al. [23] showed that it is faster to calculate these new covariance
matrices sequentially: estimate Pi qj and i qj before estimating Pk qj ; or Pj qk and j qk
before Pj qi . Indeed, expressing Jacobian matrices from the initially known data
complicates their expressions.

121
 6 Frames transformation with associated uncertainties

6.4 Operator ⊕ applied to a position

The operator ⊕ can be used to transformation the frame from a position instead of
a pose [15]. Thus, the position k pi of the origin of the frame i in the frame k can
be expressed from the pose k qj of the frame j in the frame k and the position j pi of
the origin of the frame i in the frame j:

k
pi =k qj ⊕j pi
     
k
xj + j xi cos k
θj −j yi sin k
θj
=     
k
yj + j xi sin k
θj +j yi cos k
θj
     
cos k
θj −sin k
θj
=k pj +       j pi (6.39)
sin k
θj cos k
θj

Note that only the right term can be a position since a pose is required to make the
frame transformation. By considering the poses as functions for changing frames,
this new operation is equivalent to apply the function defined by this pose to the
position:
k
pi =k qj ⊕j pi , (6.40)
 
k
pi =k qj j
pi . (6.41)

The operator ⊖ does not exist for positions. A transformation of reference frame
is not possible with a position. It is not possible to determine i pj from j pi only, the
relative orientation j θi is necessary.
The covariance matrix can then be expressed
!
Pk qj Pk qj ,j pi
Pk pi ≈ J⊕ J⊕T , (6.42)
Pj qi ,k pj Pj p i

with the Jacobian matrix J⊕ :

         
∂ pk ∂ k
qj ⊕j pi 1 0 − k
yi − k yj cos k
θj −sin k
θj
J⊕ = = =  .
∂ j , j pi )
(k q ∂ ( k q j , j pi )
    
0 1 k
xi − k xj sin k
θj cos k
θj
(6.43)

6.5 Overview
Table (6.3) summarizes the usual operations commonly used in practice.

6.6 Non linearity issues from uncertainty propagation

We assume here that the uncertainty is Gaussian. It can then be represented by
an ellipse. To propagate this uncertainty in non-linear operations, we use Taylor
approximations of order 1. Thus the various operations of changing the reference
frame make it possible to find a new pose as well as its covariance matrix. A typical
example is given on figure 6.2.
Consider an example. The positions k pi and k pj are 5 m apart (Fig. (6.3)).

122
 6 Frames transformation with associated uncertainties

Table 6.3: Operators ⊕ and ⊖ for 2D frames.

Operator Equation Operations
k

j
!
xj + j xi cos k θj k

−j yi sin θj
⊕ k
qi = qj ⊕ qi
k j k
yj + j xi sin k θj + yi cos k θj


k j

θj + θik

∂ (k qj ⊕j qi )

!
− k yi − k yj −sin k θj
J⊕
1 0

cos k θj

0

∂ (k qj ,j qi )
k
0 1 xi − k xj sin θj cos k θj 0
0 0  1
0
0  1
k
xj + j xi cos k θj −j yi sin k θj
⊕ with position k
pi =k qj ⊕j pi k


yj + j xi sin k θj +j yi cos k θj


∂ (k qj ⊕j pi )




1 0 − k yi − k yj cos k θj −sin k θj
J⊕ ∂ (k qj ,j pi ) k




0 1 xi − k xj sin k θj cos k θ
j j

j j

! j
− xi cos θi − yi sin θi
⊖ i
qj = ⊖j qi j j
xi sin

j
θi j
− yi cos


θi
−j θi
∂ (⊖j qi )


!
−cos j θi −sin j
θi i
yj
J⊖ ∂ j qi
j
sin


θi −cos j
θi


−i xj
0 0 −1

f
ql Rl
Rf

R0

Figure 6.2: Cooperative localization with frame transformations. The following car
is able to localize the leader vehicle in its body frame (f ql ).

2 7
Figure 6.3: Frames and poses representing transformations in frames.

123
 6 Frames transformation with associated uncertainties

(a) Propagation of uncertainty from the (b) Inversion of the pose i qj = ⊖j qi repre-
kq kq
pose j to the pose i by integrating sented by the green ellipse and cone.
the uncertainty of the transformation of
pose k qi = k qj ⊕ j qi . The green cloud
represents the uncertainty of the rela-
tive position returned to the true refer-
ence point Rj .

(c) Propagation of uncertainty from the (d) Uncertainty of the relative pose esti-
kq kq mated from the two absolute
pose i to the pose j by integrat-   poses using
ing the uncertainty of the transforma- the operation qi = ⊖ qj ⊕ k qi .
j k
tion of pose j qi through the operation
k q = k q ⊕ ⊖j q . The point clouds


j i i
represent the distributions of poses ap-
proximated by normal distributions il-
lustrated by ellipses and cones of uncer-
tainty.

Figure 6.4: The point clouds represent the distributions of the poses approximated by
normal distributions illustrated by ellipses and cones of 95% uncertainty.
The elements associated with the poses k qi , k qj , j qi and i qj are represented
respectively in red, blue, green and purple. The relative poses j qi and i qj
k
are directly represented in the
k
124frames associated with the poses qj and
qi .
 6 Frames transformation with associated uncertainties

Figures 6.4a, 6.4b, 6.4c and 6.4d present the two elementary operations k qi =k
qj ⊕ j qi (Fig. 6.4a) and i qj = ⊖j qi (Fig. 6.4b) as well as two combinations of these
two operations k qj = k qi ⊕(⊖j qi ) (Fig. 6.4c) and j qi = ⊖k qj ⊕ k qi (Fig. 6.4d). Each
figure shows the distributions of the input positions with their average poses as well
as their ellipses and uncertainty cones. These distributions are represented by 10,000
positions with standard deviations of 0.316 m along the abscissa and ordinate axis.
The orientation of the distribution at a standard deviation of 16.9°. Each operation
is applied to the input means and their uncertainties to determine the average of
the result of this operation with its uncertainty. The uncertainty in position is
illustrated by an ellipse of uncertainty. However, by applying the operation to a
random sample of input distributions, a banana distribution is obtained due to the
non-linearity of the operation. Thus, the uncertainty ellipse of the result does not
correctly represent the final distribution. We can see that the banana distribution
of the operation k qj = k qi ⊕ (⊖j qi ) is more pronounced than those of the other
operations. This result is due to the fact that the final position k pj depends on
the two input orientations k θi and j θi as can be seen in the equation (6.25). This
operation is therefore more sensitive to non-linearities. These problems of banana
distribution are observable and very significant when the errors of orientation and
inter-distances are significant.
To reduce these non-linearity problems, the unscented transformation [27] with
sigma points often determines a Gaussian that better represents the true final dis-
tribution that is no longer a Gaussian.

125
7 Multi-Sensor Data Fusion
7.1 Introduction
In this chapter, we present methods for merging random vectors in a Gaussian linear
framework (which is a special case of stochastic estimation up to the second order).
An adaptation of these concepts to the non-linear case is immediate via an extended
or an unscented approach.
Then, we will focus on data fusion and introduce consistency checks so valuable
in practice to tune filters or to verify proper operation. Consistency tests are also
useful before doing the multi-sensor fusion, since they can be used to either reject
outliers or do basic data association.

7.2 Static Data Fusion

7.2.1 Merging two uncorrelated estimates of the same unknown
vector
Let x1 and x2 be consistent estimates of a vector quantity x affected by uncorrelated
estimation [Link] Gaussian Bayesian fusion of these two quantities is given as
follows.
We consider that

1. x1 with the covariance matrix P1 is the prediction of a Kalman filter.

2. x2 with the covariance matrix P2 is an observation with the model C = In .

So, an estimation stage of the linear filter canbe directly applied:

−1
We know from page 64 that K = Pk|k−1 C T CPk|k −1 C T + R , so:

K = P1 (P1 + P2 )−1 (7.1)

x = x1 + K (x2 − x1 ) (7.2)

P = (I − K) P1 (7.3)
Please note that from now on :

• a prediction will be noted (x1 , P1 )

• an observation (x2 , P2 )

• a final estimate (x, P )

126
 7 Multi-Sensor Fusion

We can rewrite them as follows:

x = (P1 + P2 ) (P1 + P2 )−1 · x1 + P1 (P1 + P2 )−1 (x2 − x1 )

x = P2 (P1 + P2 )−1 x1 + P1 (P1 + P2 )−1 x2 (7.4)

and
 
P = I − P1 (P1 + P2 )−1 P1
 
P = (P1 + P2 ) (P1 + P2 )−1 − P1 (P1 + P2 )−1 P1

P = P2 (P1 + P2 )−1 P1 (7.5)

In the scalar case, it is a well-known formula:

σ22 σ12
x= x 1 + x2
σ12 + σ22 σ12 + σ22
σ12 σ22
σ2 =
σ12 + σ22
In some books, the formulas that are often presented are as follows.
Consider the equation (7.5) and take its inverse1 :
 −1
P −1 = P2 (P1 + P2 )−1 P1 = P1−1 (P1 + P2 ) P2−1 (7.6)
 
= I + P1−1 P2 P2−1 = P2−1 + P1−1
The inverse of a covariance matrix, is an “information matrix”. So, when merging
two states, we sum their information to get the information of the result.
By multiplying with P −1 on the left, Eq. (7.4) can written as:

P −1 x = P −1 P2 (P1 + P2 )−1 x1 + P −1 P1 (P1 + P2 )−1 x2 (7.7)

By using Eq. (7.6) and by taking the transpose as the covariance matrices are
symmetric:

P −1 = P2−1 (P1 + P2 ) P1−1

By replacing these two terms in (7.7):

P −1 x = P1−1 (P1 + P2 ) P2−1 P2 (P1 + P2 )−1 x1 + P2−1 (P1 + P2 ) P1−1 P1 (P1 + P2 )−1 x2
= P1−1 x1 + P2−1 x2
P −1 x̂ is an information vector. So, the information vector of the result is the sum
of the two information vectors to be fused.
These are the two steps of the fusion process:

P −1= P1−1 + P2−1 

(7.8)
x = P P1−1 x1 + P2−1 x2
Notes :
1
If two square matrices A and B are invertible, then (AB)−1 = B −1 A−1

127
 7 Multi-Sensor Fusion

1. With formula (7.8), one can see directly that the fusion is commutative (by
reversing the role of each estimate in the fusion step, the result remains the
same).

2. In terms of computations, Eq. (7.8) is less efficient than using Eq. (7.4) and
Eq. (7.5) since its requires the inversion of 3 matrices instead of only one
which is (P1 + P2 )−1 .

3. In the scalar case, the formula becomes:

σ12 σ22
σ2 =
σ12 + σ22
1 1
!
x = σ̂ 2
x + 2 x2
2 1
σ1 σ2

7.2.2 General case of the fusion of n vectors with uncorrelated

errors
Consider now the problem of merging n vectors corresponding to estimates of the
same quantity. They can be merged two by two in cascade (because the Bayesian
data fusion is associative) but they can be merged directly in a single step:

P −1 = n −1P
P i=1 Pi  (7.9)
x = P. n −1
i=1 Pi xi

7.2.3 Merging two uncorrelated random vectors linked by a

linear relationship
Let x1 be a prediction and P1 the covariance matrix of the prediction error.
Let x2 with dim(x2 )=m ̸= n be an observation linked to x1 as follows :

x2 = Cx1 + β (7.10)
Here, P2 =var(β) = R.
If the measurement noise is uncorrelated with the prediction error, we have:

P −1 = P1−1 + C T P2−1 C
(7.11)
x = x1 + P C T P2−1 (x2 − Cx1 )

Let proof that the covariance matrix is P −1 = P1−1 + C T P2−1 C

 −1
P = P1−1 + C T P2−1 C
The Woodbury matrix identity states that:
 −1
(A + U CV )−1 = A−1 − A−1 U V A−1 U + C −1 V A−1
Let use this formula with A = P1−1 , U = C T , V = C and C = P2−1 :
 −1
P = P1 − P1 C T CP1 C T + P2 CP1
 −1
As K = P1 C T CP1 C T + P2 , we have:

128
 7 Multi-Sensor Fusion

P = (I − KC)P1
Which is the well-known Kalman expression of the update.

Let proof that the Kalman gain is K = P C T P2−1

 −1
We know that P = (I − KC)P1 and since K = P1 C T CP1 C T + P2 , we have:
  −1 
P = I − P1 C T
CP1 C + P2
T
C P1
  −1 
PC T
P2−1 = I − P1 C T
CP1 C + P2
T
C P1 C T P2−1
  −1 
= P1 C T
I − CP1 C + P2 T
CP1 C T
P2−1

We can write:
 −1  
I = CP1 C T + P2 CP1 C T + P2
 −1  −1
⇐⇒ I − CP1 C T + P2 CP1 C T = CP1 C T + P2 P2
Therefore:
 −1 
PC T
P2−1 = P1 C T
CP1 C + P2
T
P2 P2−1
 −1
= P1 C T CP1 C T + P2
=K
Please note that this Kalman gain expression is different from the ones we have
seen before in this course.

7.3 Dynamic Data Fusion

Consider a problem in which there are redundant measurements and unmeasured
variables to observe.
If we assume that disturbances are centered and characterized by their covariance,
then we can apply a statistical observer. The method is as follows:
1. Determine a state, establish evolution and observation models,
2. Study the observability of the state,
3. Implement an estimator/predictor observer.

7.3.1 Merging synchronous measurements

[Link] Method
When the measures yi are acquired synchronously with the period T e, a solution
consists in establishing a state space as follows:

xk+1 = f (xk ) + αk
(

yk = g (xk ) + βk
 
y1
where yk =  ... 


ym

129
 7 Multi-Sensor Fusion

Please notice that there is no input in this formalization.

The measurements may be correlated.

[Link] Example
Let consider a car equipped with a tachometer, a gyroscope and a GPS receiver.

GPS

Bus CAN

Figure 7.1: Sensors considered

M
k
vk
M k
yk

W xk x

Figure 7.2: Modeling of the vehicle and frames

We make a loose coupling between Dead-Reckoning (DR) and GPS where DR is

carried out with the set of sensors {tachometer, gyro}.

Modeling
ẋ = v · cos(ψ)



ẏ = v · sin(ψ)
ψ̇ = ω



where v is the linear speed of M and ω the rate.

Measures:

1. The GPS receiver provides (x,y)

2. The tachometer (v)

3. The gyro (w).

130
 7 Multi-Sensor Fusion

Discretization
xk+1 −xk
Equations are non-linear. We make an approximate discretization: ẋk = Te
We get
 xk+1 = xk + T [Link] · cos(ψk )


yk+1 = yk + T [Link] · sin(ψk )
ψk+1 = ψk + T e.ωk



Thereby errors are introduced.

State space
Propose a state vector X = [x, y, ψ, v, ω]T
An evolution model can be given by supposing the speeds have slow variation:

xk+1 = xk + T [Link] · cos(ψk ) + αx,k




yk+1 = yk + T [Link] · sin(ψk ) + αy,k





ψk+1 = ψk + T [Link] + αψ,k ⇔ Xk+1 =f(Xk )+αk
vk+1 = vk + αv,k




ωk+1 = ωk + αω,k




The model noises are particularly important here. They allow in particular the
evolution of {vk , ωk }.
An observation model is:
xgps,k = xk


gps,k = yk

 y



 vtachy,k = vk
ωgyro,k = ωk



xgps,k y1,k
   
 ygps,k   y2,k 
= = Yk
   
vtachy,k y3,k
  
   
ωgyro,k y4,k
We get:

1 0 0 0 0
 
 0 1 0 0 0 
Yk =   · Xk

+ βk = C · xk + βk
0 0 0 1 0

 
0 0 0 0 1
Finally, a nonlinear stochastic state representation is obtained with a non-linear
evolution model and a linear observation one.

Xk+1 = f (Xk ) + αk
(

Yk = C · Xk + βk

Observability study
In an observability study, the noise is neglected.
{x, y, v, ω} are directly observed. So, they are observable.
The only variable not directly observed is the heading ψ.
Let an equation of the model xk+1 = xk + T [Link] · cos(ψk )
Yet, ψk+1 = ψk + T e.ωk ⇒ ψk = ψk+1 − T e.ωk

⇒ xk+1 = xk + T [Link] · cos(ψk+1 − T e.ωk )

We make an index change: xk = xk−1 + T [Link]−1 · cos(ψk − T e.ωk−1 )

131
 7 Multi-Sensor Fusion

!
xk − xk−1
ψk = arccos + T e.ωk−1
T [Link]−1
So, ψk is a function of other parameters already observed. Therefore, it is observ-
able except if the speed is zero (degeneration).

Observer
We have to make an extended Kalman filter because of the non-linearity of the
evolution model. Therefore, we need to calculate the Jacobian of f for the calculation
of the covariance matrix in the prediction step.
1 0 −T e · vk · sin(ψk ) T e · cos(ψk ) 0
 

 0 1 T e · vk · cos(ψk ) T e · sin(ψk ) 0 
 
h i
= 0 0 1 0  to evaluate at X̂k/k
∂f  
∂X
Te 

 0 0 0 1 0 

0 0 0 0 1

7.3.2 Fusion of asynchronous measures

In practice, the asynchronicity can arise from:
1. Periodic measurements but with different periods,
2. Aperiodic measures (possibly sporadic in nature).
In the case where all measures yi are asynchronous, a solution consists to establish
a state space called "continuous state with discrete measurements"

ẋ(t) = f (x(t)) + α(t)



y1,k = g1 (x(tk )) + β1,k




 =
yp,i = gp (x(ti )) + βp,i



We discretize the evolution model in order to verify the Shannon condition and
so that the sampling times are multiples of T e (or rather a good approximation of
multiples of T e).

xk+1 = f (xk ) + αk


y1,j = g1 (x(t0 + j.T e) + β1,j




 =
yp,i = gp (x(t0 + i.T e) + βp,i



Remind that the f functions are different in the discrete and continuous state
spaces.
If we assume that the observation noises are independent (no correlation), then
in each period
1. We use available measures to make estimation steps,
2. We provided the observation of the state
3. We calculate the prediction with the model for the next step.
It may be noted that there may be no measure available at a given time. In this
case, the observer operates in "open loop" using only its evolution model.
Conversely, several measures can appear in the same period. Under assumption
of independence of noises, measurements can be processed in separate estimation
steps.

132
 7 Multi-Sensor Fusion

7.3.3 Fusion of Out Of Sequence measures

This is the most complex case.
The simplest method is to buffer the data from all sensors to be merged. Then,
one merges a measure only when one is certain that all other sensors have provided
their data. This way, we are sure to treat all measures in a good order. This method
has the disadvantage of delaying the result.
A more effective but more computational alternative is to keep timestamped data
in a buffer and go back in time to merge the measure that has just arrived at the
right moment. We go back to the present moment by chaining prediction steps.
These mechanisms are already integrated in some middlewares.

7.4 Consistency of an estimator

7.4.1 Problem
It is often important to check if the state observer is consistent, i.e. if it satisfies the
following two conditions:

1. Unbiased: E [e] = 0
The estimation error e of the estimate is effectively null.
h i
2. Px = E e · eT
The estimated covariance Px is effectively equal to the real covariance of the
error.
h i
In practice, it is observed that the observer is often too optimistic i.e. Px < E e · eT .
It may diverge. It is preferable that it be rather pessimistic. In addition, the
estimated matrix is often used to describe the uncertainty and is compared h to ia
threshold. If we want a conservative observer, rather we seek to have Px > E e · eT .
In practice, there are two ways to study the consistency:

1. Compared to a reference (a ground truth equipment or by simulation) verifying

that the observer is unbiased and using a test called “NEES” (Normalized
Estimation Error Squared)

2. By analyzing the innovation if there is no reference (Innovation test) verify-

ing that the observer is unbiased and using a test called “NIS” (Normalized
Innovation Squared)

Both tests are based on the Chi2 law.

7.4.2 Chi2 law

Let x1 , ... , xn be n independent random variables with the same standard normal
reduced distribution (Var(xi )=1).
By definition the variable x, such that
n
x= x2i
X

i=1

follows a χ2 law with n degrees of freedom.

133
 7 Multi-Sensor Fusion

The density probability of x is

k k
x≻0

1
e− 2 x 2 −1
2 2 Γ( k2 )
k

f (x) =

0 elsewhere

Where Γ is the Euler Gamma function Γ (x) =

R ∞ x−1 −t
0 t e dt.
We have E(x) = n and Var(x) = 2n.

Figure 7.3: χ2 probability density with respect to n

Consider now a Gaussian random vector x of dimension n.

The variable (x − mx )T P −1 (x − mx ) follows a χ2 law of degree n:

(x − mx )T P −1 (x − mx ) ∼ χ2 (n)
This results from the following property.
If x ∼ Nn (mx , P ) then we can find a matrix B such that x = mx + B · u where
u ∼ Nn (0, I) is centered and reduced. It is shown that B has full rank, and so we
can write u = B −1 . (x − mx ). In addition, we can note that B · B T = P .

(x − mx )T P −1 (x − mx )
 −1
= (x − mx )T B · B T (x − mx )
−1 T −1
= (x − mx ) (B ) (B) (x − mx )
T

= uT u = ni=1 u2i
P

7.4.3 NEES test with a reference

Now let us focus on consistency tests. Suppose we realized a long trial with a ground
truth (a reference system which is very accurate) denoted xref .

[Link] Principle
It is calculated for all samples:

1. Check if x̂k − xref,k is centered2 ,

2
Hypotheses of stationarity and ergodicity

134
 7 Multi-Sensor Fusion

2. Compute (x̂k − xref,k )T · Pk−1 · (x̂k − xref,k ) for all the samples and look at
the percentage of samples that exceed a calculated threshold (fractile) corre-
sponding to a degree n of a Chi2 law χ2 (n, 1 − α) (α=5% for instance). If
the percentage is not consistent with α then the filter is not consistent: either
pessimistic or components of the vector do not follow a normal distribution.

Example with Matlab for a pose estimate:

%For a 95% confidence domain

>‌> Threshold=chi2inv(0.95,3) %dof = 3 for a 2D pose
Threshold = 7.8147

[Link] Computation of the standard deviation along the 2D position error

vector
As we have seen, filter consistency is evaluated by considering the normalized estima-
tion error squared δ 2 which follows a Chi2 distribution under Gaussian assumption:

δ 2 = (x − x̂)T P −1 (x − x̂) (7.12)

Let us focus on the 2D position estimated by the filter. We consider now only the
2D position components of the state vector in the working frame.
The normalized horizontal positioning error (HPE) squared e2 is defined by :
!T !
ex ex
e =
2 −1
PHP E (7.13)
ey ey
!
σx2 σxy 2
where PHP E = and σx , σy and σxy are estimated by the filter.
σxy σy2
2

ex = x̂ − xref and ey = ŷ − yref , with (xref , yref ) the ground truth of the vehicle
horizontal position and (x̂, ŷ) the estimated position.
If the model assumptions and the tuning of the filter are correct, then equation
(7.13) follows a χ2 distribution with two degrees of freedom (denoted χ2 (2) in the
following).
In order to study the consistency of the HPE, one usually looks at the percentage
of samples exceeding a determined threshold given by a χ2 (2) distribution.
!T !
ex ex
−1
PHP E > k2 (7.14)
ey ey
where k 2 is a value
! linked to the chosen risk.
ex q
Let ue = / e2x + e2y be the unit vector supporting the HPE (see Figure
ey
7.4).
Equation (7.14) can be rewritten as:
 
e2x + e2y ueT PHP
−1
E ue > k
2

which is equivalent to (since k > 0):

1
s
q
e2x + e2y >k −1 (7.15)
ueT PHP E ue

135
 7 Multi-Sensor Fusion

Let us define σHP E as the standard deviation along the HPE vector:

1
s
σHP E = −1
ueT PHP E ue
Figure 7.4 illustrates the definition of kσHP E .
A common choice in robotics is to choose a consistency risk at 10−2 (1%). Ac-
cording to a χ2 (2) distribution, k 2 = 9.21. In this case, the corresponding bound
of the 2D estimated position is 3.035σHP E , with σHP E being estimated in real-time
by the filter. Therefore, a consistency failure happens when the real error is beyond
this 3.035σHP E bound, which means
q
e2x + e2y > 3.035σHP E
Calculate σHP E is useful for displaying integrity diagrams where the 2D position
error is plotted on the x-axis and 3.035σHP E on the y-axis for a consistency risk of
1%.

364.4

364.3

364.2

364.1

ex
North (m)

364
(x, y)
363.9

kσHPE ey
363.8

363.7
(xref, yref)
363.6

363.5

363.4
−436.8 −436.6 −436.4 −436.2 −436 −435.8 −435.6
East (m)

Figure 7.4: (x̂, ŷ) is the estimated position. (xref , yref ) is the ground truth of the
vehicle horizontal position. Here, the reference is located outside of the
confidence domain.

7.4.4 NIS test on the innovation

Often it is not possible to perform a test with a ground truth. In this case, we
consider the innovation test.
Same procedure is followed for the calculation of the mean with the samples
yk − C.x̂k .
The test on the variance is performed on the innovation
 −1
(yk − C.x̂k )T CP C T + R (yk − C.x̂k )

which has to follow a Chi2 law with m degrees of freedom (m is the size of the
observation vector).

136
 7 Multi-Sensor Fusion

The reasoning is the same with except that we evaluate the consistency of the
observations with the model.

7.5 Outliers rejection

In all data fusion problem, it is essential to test the consistency of the data before
merging them in order to avoid introducing incorrect information (outliers) in the
fusion process.

7.5.1 Mahalanobis distance

Concept introduced by Prasanta Chandra Mahalanobis in 1936.
Suppose a random vector follows a multivariate distribution with a mean mx and
a covariance matrix Px .
Let x be a vector.
The Mahalanobis distance of this vector x to the distribution (mx , Px ) is given
by:
q
D(x) = (x − mx )T Px−1 (x − mx ) (7.16)
It is a distance weighted by the variance.

Px
x2

mx

x1

Figure 7.5: Even if x1 is closer to mx its Mahalanobis distance is greater than that
of x2 . x2 has therefore a greater probability to follow the distribution
(mx , Px )

Given two points x and y in Rn , the Mahalanobis distance between them with
respect to the distribution with mean mx and covariance matrix Px is
q
D(x, y) = (x − y)T Px−1 (x − y) (7.17)
It is therefore also a measure of dissimilarity between two random vectors of the
same distribution.

137
 7 Multi-Sensor Fusion

7.5.2 Rejection
The Mahalanobis distance is often used to determine multivariate outliers.
A classical rejection mechanism is to evaluate the consistency of the predicted
measure ŷk|k−1 with the current one yk . In other words, we have to evaluate the
consistency of the innovation.
Consider the decision variable:
 T  −1  
D2 = ŷk|k−1 − yk · Ck Pk|k −1 CkT + R · ŷk|k−1 − yk (7.18)
where Ck Pk|k −1 CkT is the covariance of the predicted measure ŷk|k−1 .
In practice, the decision stage is simply a thresholding of the distance D. The
threshold can be chosen as a function of a false alarm probability given by the inverse
function of the Chi2 3 .
Let compute the distance D given by Eq. (7.18) and apply the following algorithm:

Algorithm 7.1 Typical Outlier rejection strategy

If D>Threshold then
The prediction and the measure are inconsistent.
If the filter is consistent,
Then the measure is a outlier
Usually it is excluded.
Else
The fusion (e.g. Kalman update) is possible.
End

7.6 Data association

To do the data association (or matching) between several sources of information, a
normalized distance is frequently used, in particular the Mahalanobis distance.

Tracking example
Let two observations (or detected objects) y1 and y2 with covariances R1 and R2
and one seeks the one that is the most consistent with a prediction ŷ = C x̂ with a
covariance P .
Assuming that the error probability distributions of ei and e are Gaussian, then
the innovation µi = yi − ŷ is too. We can write the likelihood of µi in the form:
1 1 T −1
p(yi |x) = q .e− 2 µi Si µi
(7.19)
(2π)n det(S i)

with
Si = Ri + P
The most likely association is the one that maximizes p(yi |x) since, from a statis-
tical point of view, it is the one that best matches in average.

3
Function "chi2inv" in Matlab

138
 7 Multi-Sensor Fusion

The natural logarithm function being a monotonically increasing function, maxi-

mizing p(yi |x) is equivalent to maximizing the logarithm lg(p(yi |x)).
We rewrite equation 7.19:
1 1 1
lg(p(yi |x)) = lg( q ) − lg(det(Si )) − µTi Si−1 µi
(2π)n 2 2

Since the first term is a constant, maximizing p(yi |x) is equivalent to minimizing
the distance Υi :

Υi = lg(det(Si )) + µTi Si−1 µi

Therefore, among the two observations the most likely is the one which minimizes
Υi , the sum of the Mahalanobis distance plus a penalty term due to the uncertainty
(the observation with the greatest uncertainty is penalized relative to the other).

Matching features for localization

Let us see how to match undiscernible features for localization.
When a robot localizes itself with a map containing features whose coordinates are
known and if those features are not discernible from each other, a data association
problem must be solved before updating the state.
In all the mapped features have the same uncertainty, the one that maximizes
the probability of association is the one that minimizes the Mahalanobis distance
with the predicted observation. In addition, a threshold is chosen over this distance
beyond which association is too unlikely.

139
8 Kalman Filter with Covariance
Intersection
8.1 Introduction
In many practical cases, noises and/or estimation errors are not uncorrelated. In
this case, if we assume that they are uncorrelated, the data fusion (obtained for
example with a Kalman filter) is not optimal. The estimates are affected by errors
and, even worse, the covariance matrices are often underestimated and the result is
not consistent.
We are going to study in this chapter a covariance intersection technique that can
be combined with Kalman filtering.

8.2 Covariance Intersection

8.2.1 Optimal implementation
We have seen in page 128 that merging n vectors of the same estimates can be done
like follows when the errors are uncorrelated:

P −1 = n −1
P
P i=1 Pi  (8.1)
x = P. n −1
i=1 Pi xi

The assumption that the estimation errors are uncorrelated can often not be met,
especially when performing data fusion in a distributed system where cycles can
we be led to re-merge an already integrated estimate in the fusion process. The
estimation errors are then correlated. Doing data fusion with KF updates is not
appropriate and leads to inconsistent estimates in practice.
One solution is to use the method called “Covariance Intersection” (CI) [13] which
calculates a covariance that remains consistent.
The estimate and its covariance are obtained by the CI update:

P −1 = ωP1−1 + (1 − ω) P2−1  with 0 < ω < 1 (8.2)

x = P. ωP1 x1 + (1 − ω) P2 x2
−1 −1

Since 0 < ω < 1, the resulting information matrix is obtained by a convex combi-
nation (i.e. a linear combination where all coefficients are non-negative and sum to
1).
In general, ω is computed to minimize the trace or the determinant of the resulting
covariance matrix P .

ωopt = argmin (det(P (ω)) with 0 < ω < 1 (8.3)

ω
 h i−1 
ωopt = argmin det ωP1−1 + (1 − ω) P2−1 with 0 < ω < 1 (8.4)
ω

140
 8 KF with Covariance Intersection

which is equivalent to compute without a matrix inversion:

 
ωopt = argmin 1/det(ωP1−1 +(1−ω)P2−1 ) with 0 < ω < 1 (8.5)
ω

It is therefore necessary to solve an optimization problem during the update.

The CI data fusion is conservative (see Fig. 8.1). The result can be graphically
interpreted as the smallest matrix that contains the intersection. This property is
due to the convex combination of the information matrices.

5
input 1
4 input 2
optimal CI
3 KF
CI with non optimal
2

-1

-2

-3

-4

-5
-4 -3 -2 -1 0 1 2 3 4

Figure 8.1: Illustration of the CI fusion of the blue estimate with the green one.
The two estimates are equal for illustration. Only, their covariances are
different. Results are displayed for different ω (in black) and ω = 0.50
for optimal value (in red). Kalman update in cyan.

2.5
input 1
2 input 2
optimal CI
1.5 KF
CI with non optimal
1

0.5

-0.5

-1

-1.5

-2

-2.5
-4 -3 -2 -1 0 1 2 3 4

Figure 8.2: Illustration of the CI fusion of the blue estimate with the green one.
Results are displayed for different ω (in black) and ω = 0.50 for optimal
value (in red) which is equal to the green one. Kalman update in cyan.

141
 8 KF with Covariance Intersection

8.2.2 Fast Covariance Intersection

In the optimal CI method, the weight ω is computed to minimize the determinant
(or the trace) of the resulting covariance matrix. It is therefore necessary to solve
an optimization problem during the update which can be computationally heavy.
There are many methods that can be used to estimate a suboptimal value of ω.
Let’s study one of them in the following.
In [20], a non iterative sub-optimal method was proposed. The idea is a follows.
The covariance obtained by the optimal CI update is given by:

P −1 = ωP1−1 + (1 − ω) P2−1 with 0 < ω1 < 1

This is equivalent to find ω1 and ω2 such that the uncertainty matrix P is minimal:

P −1 = ω1 P1−1 + ω2 P2−1 (8.6)

under the constraint:
ω1 + ω2 = 1 (8.7)
As the determinant of a covariance matrix is proportional to the volume of its
uncertainty ellipsoid, a second constraint is chosen as follows:

1. If the two uncertainty ellipsoids have the same volume (i.e. det(P1 ) = det(P2 )1 ),
then ω1 = ω2 ,

2. If det(P1 )/ det(P2 ) → 0, then ω2 → 0 (the uncertainty P2 in x2 compared to

P1 in x1 is so large that the result of the data fusion is (x1 , P1 ))

This second constraint can be written as:

det(P1 )ω1 = det(P2 )ω2 (8.8)

We can get analytical expressions of the weights ω1 and ω2 .

det(P1 )ω1 = det(P2 ) (1 − ω1 ) (8.9)

(det(P1 ) + det(P2 )) ω1 = det(P2 ) (8.10)

det(P2 )
ω1 = (8.11)
det(P1 ) + det(P2 )
det(P1 )
ω2 = (8.12)
det(P1 ) + det(P2 )
We can check that the weights respect the constraints 0 < ω1 < 1 and 0 < ω2 < 1.
Since P1 and P2 are covariance matrices (positive-definite), we have det(P1 ) > 0
and det(P2 ) > 0. Therefore, 0 < ω1 and 0 < ω2 .
Since det(P1 ) + det(P2 ) > det(P2 ) :

det(P2 )
ω1 = <1
det(P1 ) + det(P2 )
.

1
In his paper, Wolfgang Niehsen used the “trace” instead of the “determinant”

142
 8 KF with Covariance Intersection

5
input 1
4 input 2
optimal CI
3 Fast CI

-1

-2

-3

-4

-5
-4 -3 -2 -1 0 1 2 3 4

Figure 8.3: CI and fast CI applied to the blue and green estimates. The results are
very close.

8.3 Kalman update with covariance intersection

Let x1 be a prediction and P1 the covariance matrix of the prediction error.
Let x2 with dim(x2 )=m ̸= n be an observation linked to x1 as follows :

x2 = Cx1 + β
Here, P2 =var(β)(= R).
Please note that we now place ourselves in a very general case that varies between
few observations or many redundant observations.

8.3.1 First expression

We have seen before in page 128 that if the measurement noise is uncorrelated with
the prediction error, we have:

P −1 = P1−1 + C T P2−1 C
If the measurement noise has an unknown correlation with the prediction, we can
compute the covariance matrix P of the update with the optimal ω:

P −1 = ωP1−1 + (1 − ω) C T P2−1 C (8.13)

This is simply a direct extension of Eq. 7.11 for the CI
Let rewrite the expression of Eq. 8.13:

P −1 − (1 − ω) C T P2−1 C = ωP1−1
   
⇐⇒ P P −1 − (1 − ω) P C T P2−1 C = P ωP1−1

⇐⇒ P P −1 − (1 − ω) P C T P2−1 C = ωP P1−1

143
 8 KF with Covariance Intersection

⇐⇒ 1 − (1 − ω) P C T P2−1 C = ωP P1−1 (8.14)

The CI estimate is given by:
 
x = P ωP1−1 x1 + (1 − ω) C T P2−1 x2

⇐⇒ x = ωP P1−1 x1 + (1 − ω) P C T P2−1 x2
Using Eq. 8.14:
 
⇐⇒ x = 1 − (1 − ω) P C T P2−1 C x1 + (1 − ω) P C T P2−1 x2

⇐⇒ x = x1 + (1 − ω) P C T P2−1 (x2 − Cx1 )

We define Kω as :

Kω = (1 − ω) P C T P2−1
In short, the update phase can be written as, once ω has been computed for
instance by minimization of the first equation:

= ωP1−1 + (1 − ω) C T P2−1 C

−1
P


Kω = (1 − ω) P C T P2−1 (8.15)

x = x1 + Kω (x2 − Cx1 )



8.3.2 Second expression

The update we have seen in the previous section involves the inversion of three
matrices which is heavy in terms of calculations. It is possible to find an update
expression that involves only one matrix inversion [8].
The Woodbury matrix identity states that:
 −1
(A + U CV )−1 = A−1 − A−1 U V A−1 U + C −1 V A−1
Let use the Woodbury formula with A = ωP1−1 , U = C T , V = C and C =
(1 − ω)P2−1 :

 −1   −1 
P = ωP1−1 + (1 − ω) C T
P2−1 C = I− 1
P CT
ω 1
1
ω
CP1 C T + 1
P
1−ω 2
C 1
P
ω 1
(8.16)
From now on, let’s assume that ω ̸= 1 and ω ̸= 0.
We know that

Kω = (1 − ω) P C T P2−1
 −1
⇐⇒ Kω = (1 − ω) ωP1−1 + (1 − ω) C T P2−1 C C T P2−1
Take the transpose of P :
)T
1 1 1 1
("  −1 #
PT = I − P1 C T CP1 C T + P2 C P1
ω ω 1−ω ω

144
 8 KF with Covariance Intersection

1 1 1 1
"  −1 #
⇐⇒ P = P = P1 I − C T
T
CP1 C T + P2 C P1
ω ω 1−ω ω
Since covariance matrices are symmetric.
The gain becomes :

1 1 1 1
"  −1 #
Kω = (1 − ω) P1 I − C T CP1 C T + P2 C P1 C T P2−1
ω ω 1−ω ω

1 1 1 1
"  −1 #
⇐⇒ Kω = P1 C T I − CP1 C T + P2 C P1 C T (1 − ω) P2−1
ω ω 1−ω ω
 −1  
By changing I into ω1 CP1 C T + 1−ω
1
P2 . 1
ω
CP1 C T + 1
P
1−ω 2
, we get after sim-
plification a new expression of the gain:

1 1 1 −1
 
K ω = P1 C T CP1 C T + P2 (8.17)
ω ω 1−ω
The update of the covariance matrix can be directly computed once the gain is
known.
Eq. 8.16 can be rewritten:
  −1 
P = I− 1
P CT
ω 1
1
ω
CP1 C T + 1
P
1−ω 2
C 1
P
ω 1

We use this expression of the covariance matrix to compute ω by minimizing the

determinant for example. This expression is a little more complicated but with an
efficient optimizer, the calculation is done very well.
One can see that the gain appears explicitly and we get:

P = [I − Kω C] ω1 P1 (8.18)
In short, an update step can be written, once ω has been computed by minimiza-
tion:
  −1
K ω


 = ω1 P1 C T C ω1 P1 C T + 1
P
1−ω 2

 x = x1 + Kω (x2 − Cx1 ) with ω ̸= 1 and ω ̸= 0 (8.19)

P = [I

− Kω C] ω1 P1


The Joseph’s form can be computed for a more stable implementation [9]:
1 1
   
P = (I − Kω C) P1 (I − Kω C)T + Kω P2 KωT
ω 1−ω
We find the same expression as the classic Kalman filter but with and .
1
P1 =⇒ P1
ω
1
P2 =⇒ P2
1−ω

145
 8 KF with Covariance Intersection

Let proof the Joseph’s form

1 1
 
P = (I − Kω C) P1 (I − Kω C)T + Kω P2 KωT (8.20)
ω 1−ω
1 1 1
= (I − Kω C) P1 − (I − Kω C) P1 C T KωT + Kω P2 KωT
ω ω 1−ω
1 1 1 1
 
= (I − Kω C) P1 − P1 C Kω + Kω C P1 C +
T T T
P2 KωT
ω ω ω 1−ω
 
Let denote: PY,ω = C 1
P
ω 1
CT + 1
P
1−ω 2

1 1
P = (I − Kω C) P1 − P1 C T KωT + Kω PY,ω KωT
ω ω
1 1 1
= (I − Kω C) P1 − P1 H T KωT + P1 H T  −1
PY,ω
  T
P 
Y,ω Kω
ω ω ω
1
= (I − Kω C) P1
ω

146
 8 KF with Covariance Intersection

8.4 Split Covariance Intersection Filter

The Split Covariance Intersection Filter (SCIF) is a state estimation method which
combines Kalman filtering with covariance intersection. The covariance matrix P is
split into two components, one containing the estimation error that is conditionally
uncorrelated to the model and observation errors (it is denoted Pi like “indepen-
dent”) and one describing the covariance of the estimation error that might contain
correlation with the model and observation errors. It is denoted Pd like “depen-
dent”. Intuitively, the SCIF combines a KF and a CIF, where the KF handles the
independent part and the CIF handles the dependent one.
The estimation error is split into two parts (see Figure 8.4):

e = ei + ed (8.21)

P = Pi + Pd (8.22)

6
Pi
Pd
4 P=Pi+Pd

-2

-4

-6
-5 0 5

Figure 8.4: Split covariance matrix.

The distinction of a dependent and independent part is also done for the model
and observation covariance matrices Q and R.

8.4.1 Update stage

The covariance matrix of the observation error R is split into two known parts:

R = P2,d + P2,i
Suppose we know the covariance matrices P1,d and P1,i of the prediction errors.
The update is given by:

147
 8 KF with Covariance Intersection

P1 = ω1 P1,d + P1,i % cov of the prediction





P2 = 1−ω1
P2,d + P2,i % cov of the observation





  −1
= + P2 % gain

T
K P C CP1 C T




 1

 x = x1 + K(x2 − Cx1 ) %
new estimate with ω ̸= 1 and ω =
̸ 0
P = [I − KC] P1 %
new cov of the estimate






Pi = (I − KC) P1,i (I − KC)T + KP2,i K %
cov of the independ error



 T


Pd = P − Pi %
cov of the depend error



(8.23)
The proof is given in [16] but intuitively by replacing P1,d =⇒ ω P1,d and P2,d =⇒
1
1
P , we get the result.
1−ω 2,d

ω is computed with

P = [I − KC] P1 (8.24)
  −1 
P = I − P1 C T CP1 C T + P2 C P1 (8.25)

1 1 1 1
"      −1 # 
P (ω) = I − P1,d + P1,i C T C P1,d + P1,i C T + P2,d + P2,i C P1,d + P1,i
ω ω 1−ω ω
(8.26)

ωopt = argmin (det (P (ω))) with 0 < ω < 1 (8.27)

ω
Then the update is applied with Eq. 8.23.

8.4.2 Prediction stage

Q = Qd + Qi
= f (x)

 x1




P1,d = APd AT + Qd
P1,i = APi AT + Qi



8.4.3 The SCIF: a general filtering method

It can be easily shown that SCIF is a generalization of Kalman filtering and covari-
ance intersection. In the case where the estimation errors are not correlated with
the other uncertainties, it works like a Kalman filter. In the opposite case (where
the independent estimation errors are zero), it works as a covariance intersection.

8.4.4 Practical implementation

In order to implement a SCIF, we have now four terms to tune: P2,d , P2,i , Qd and
Qi .
Often, P2,d = 0 if the hypothesis that the observation errors are uncorrelated
holds.
The tuning of the model error can be done starting with Qd small and then
progressively increase its value to make the filter consistent.

148
Bibliography
[1] Martin A., Fischler, and Robert C. Bolles. Random sample consensus: A
paradigm for model fitting with applications to image analysis and automated
cartography. In Proceedings of the Comm. ACM., pages 381–395, June 1981.

[2] Y Bar-Shalom, XR Li, and T Kirubarajan. Estimation with application to

tracking and navigation. Wiley. 2001.

[3] C. G. Broyden. The Convergence of a Class of Double-rank Minimization Al-

gorithms 1. General Considerations. IMA Journal of Applied Mathematics,
6(1):76–90, 1970.

[4] Zhe Chen. Bayesian filtering: From kalman filters to particle filters, and beyond.
Technical report, McMaster University, 2003.

[5] Christophe Coué, Cédric Pradalier, Christian Laugier, Thierry Fraichard, and
Pierre Bessiere. Bayesian Occupancy Filtering for Multitarget Tracking: an
Automotive Application. International Journal of Robotics Research, 25(1):19–
30, January 2006.

[6] Hugh Durrant-Whyte. Multi Sensor Data Fusion. The University of Sydney
NSW 2006. 2001.

[7] Fredrik Gustafsson, Fredrik Gunnarsson, Niclas Bergman, Urban Forssell,

Jonas Jansson, Rickard Karlsson, and P.-J. Nordlund. Particle filters for po-
sitioning, navigation, and tracking. IEEE Transactions on Signal Processing,
50(2):425–437, 2002.

[8] Elwan Héry. Localisation coopérative de véhicules autonomes communicants.

Phd thesis, Université de Technologie de Compiègne, November 2019.

[9] Elwan Héry, Philippe Xu, and Philippe Bonnifait. Consistent decentralized
cooperative localization for autonomous vehicles using LiDAR, GNSS, and HD
maps. Journal of Field Robotics, 38(4):552–571, 2021.

[10] Huber Peter J. Robust Statistics (2nd ed). Handbook of Multisensor Data
Fusion: Theory and Practice. John Wiley and Sons Inc, 2009.

[11] Luc Jaulin. Représentation d’état pour la modélisation et la commande des

systèmes. Hermes science, 2005.

[12] A. H. Jazwinski. Stochastic Processes and Filtering Theory. Academic Press,

1970.

[13] S. Julier and J. K. Uhlmann. General decentralized data fusion with covariance
intersection, Chapter 14. Handbook of Multisensor Data Fusion: Theory and
Practice. CRC Press, Taylor Francis, Boca Raton FL USA, 2001.

149
 Bibliography

[14] Simon J. Julier and Jeffrey K. Uhlmann. A new extension of the kalman filter
to nonlinear systems. In In Int. Symp. Aerospace/Defense Sensing, Simul. and
Controls, pages 182–193, 1997.

[15] Hao Li. Cooperative Perception : Application in the Context of Outdoor Intel-
ligent Vehicle Systems. PhD thesis, Ecole Nationale Supérieure des Mines de
Paris, 2012.

[16] Hao Li, Fawzi Nashashibi, and Ming Yang. Split covariance intersection fil-
ter: Theory and its application to vehicle localization. IEEE Transactions on
Intelligent Transportation Systems, 14(4):1860–1871, 2013.

[17] Martin E. Liggins and Kuo-Chu Chang. Introduction to Particle Filtering: the
next stage in tracking, chap 9. Handbook of Multisensor Data Fusion: Theory
and Practice. CRC Press, Taylor Francis, Boca Raton FL USA, 2001.

[18] D. Marquardt. An algorithm for least-squares estimation of nonlinear parame-

ters. SIAM Journal Applied Mathematics, 11:431–441, 1963.

[19] Peter S. Maybeck. Stochastic models, estimation, and control, volume 141 of
Mathematics in Science and Engineering. 1979.

[20] Wolfgang Niehsen. Information fusion based on fast covariance intersection

filtering. In International Conference on Information Fusion, 2002.

[21] B. Ristic, S. Arulampalam, and N. Gordon. Beyond the Kalman Filter. Particle
Filters for Tracking Applications. Artech House. 2004.

[22] Nando de Freitas Rudolph van der Merwe, Arnaud Doucet and Eric Wan. The
unscented particle filter, cambridge university department of engineering,. In
Technical report CUED/F-INFENG/TR 380, 2000.

[23] R. Smith, M. Self, and P. Cheeseman. Estimating Uncertain Spatial Relation-

ships in Robotics. In I. J. Cox and G. T. Wilfong, editors, Autonomous Robot
Vehicles, pages 167–193. Springer New York, New York, NY, 1990.

[24] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics.
The MIT Press, 2005.

[25] M Tossaint, J Samson, F Toran, J Ventura-Traveset, M Hernández-Pajares,

JM Juan, J Sanz, and P Ramos-Bosch. The stanford–esa integrity diagram: A
new tool for the user domain sbas integrity assessment. Navigation, 54(2):153–
162, 2007.

[26] COST Action TU1302. Sappart handbook: Assessment of positioning perfor-

mance in its applications, 2017.

[27] J. K. Uhlmann. Dynamic Map Building and Localization: New Theoretical

Foundations. PhD thesis, University of Oxford, 1995.

150