This action might not be possible to undo. Are you sure you want to continue?
Paul Michael Newman
October 2003, Version 1.00
Contents
1 Introduction and Motivation 6
2 Introduction to Path Planning and Obstacle Avoidance 8
2.1 Holonomicity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Conﬁguration Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3 The MinkowskiSum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.4 Voronoi Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.5 Bug Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.6 Potential Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3 Estimation  A Quick Revision 19
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2 What is Estimation? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2.1 Deﬁning the problem . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.3 Maximum Likelihood Estimation . . . . . . . . . . . . . . . . . . . . . . . . 21
3.4 Maximum APosteriori  Estimation . . . . . . . . . . . . . . . . . . . . . . . 22
2
3
3.5 Minimum Mean Squared Error Estimation . . . . . . . . . . . . . . . . . . . 24
3.6 Recursive Bayesian Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4 Least Squares Estimation 28
4.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.1.1 A Geometric Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4.1.2 LSQ Via Minimisation . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4.2 Weighted Least Squares . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.2.1 Nonlinear Least Squares . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.2.2 Long Baseline Navigation  an Example . . . . . . . . . . . . . . . . . 31
5 Kalman Filtering Theory, Motivation and Application 35
5.1 The Linear Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
5.1.1 Incorporating Plant Models  Prediction . . . . . . . . . . . . . . . . 39
5.1.2 Joining Prediction to Updates . . . . . . . . . . . . . . . . . . . . . . 42
5.1.3 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.2 Using Estimation Theory in Mobile Robotics . . . . . . . . . . . . . . . . . . 46
5.2.1 A Linear Navigation Problem  “Mars Lander” . . . . . . . . . . . . . 46
5.2.2 Simulation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
5.3 Incorporating NonLinear Models  The Extended Kalman Filter . . . . . . . 52
5.3.1 Nonlinear Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
5.3.2 Nonlinear Observation Model . . . . . . . . . . . . . . . . . . . . . . 54
3
4
5.3.3 The Extended Kalman Filter Equations . . . . . . . . . . . . . . . . 56
6 Vehicle Models and Odometry 59
6.1 Velocity Steer Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
6.2 Evolution of Uncertainty . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
6.3 Using DeadReckoned Odometry Measurements . . . . . . . . . . . . . . . . 63
6.3.1 Composition of Transformations . . . . . . . . . . . . . . . . . . . . . 65
7 Feature Based Mapping and Localisation 69
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
7.2 Features and Maps . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.3 Observations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.4 A Probabilistic Framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
7.4.1 Probabilistic Localisation . . . . . . . . . . . . . . . . . . . . . . . . . 71
7.4.2 Probabilistic Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . 72
7.5 Feature Based Estimation for Mapping and Localising . . . . . . . . . . . . . 73
7.5.1 Feature Based Localisation . . . . . . . . . . . . . . . . . . . . . . . . 73
7.5.2 Feature Based Mapping . . . . . . . . . . . . . . . . . . . . . . . . . 74
7.6 Simultaneous Localisation and Mapping  SLAM . . . . . . . . . . . . . . . . 78
7.6.1 The role of Correlations . . . . . . . . . . . . . . . . . . . . . . . . . 81
8 Multimodal and other Methods 83
8.1 Montecarlo Methods  Particle Filters . . . . . . . . . . . . . . . . . . . . . . 83
4
5
8.2 Grid Based Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
9 In Conclusion 89
10 Miscellaneous Matters 90
10.1 Drawing Covariance Ellipses . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
10.2 Drawing High Dimensional Gaussians . . . . . . . . . . . . . . . . . . . . . . 93
11 Example Code 94
11.1 Matlab Code For Mars Lander Example . . . . . . . . . . . . . . . . . . . . 94
11.2 Matlab Code For Ackerman Model Example . . . . . . . . . . . . . . . . . . 98
11.3 Matlab Code For EKF Localisation Example . . . . . . . . . . . . . . . . . . 100
11.4 Matlab Code For EKF Mapping Example . . . . . . . . . . . . . . . . . . . 103
11.5 Matlab Code For EKF SLAM Example . . . . . . . . . . . . . . . . . . . . . 107
11.6 Matlab Code For Particle Filter Example . . . . . . . . . . . . . . . . . . . . 111
5
Topic 1
Introduction and Motivation
This set of lectures is about navigating mobile platforms or robots. This is a huge topic and
in eight lectures we can only hope to undertake a brief survey. The course is an extension
of the B4 estimation course covering topics such as linear and nonlinear Kalman Filtering.
The estimation part of the lectures is applicable to many areas of engineering not just mobile
robotics. However I hope that couching the material in a robotics scenario will make the
material compelling and interesting to you.
Lets begin by dispelling some myths. For the most parts when we talk about “mobile
robots” we are not talking about gold coloured humanshaped walking machines
1
. Instead
we are considering somekind of platform or vehicle that moves through its environment
carrying some kind of payload. Almost without exception it is the payload that is of interest
 not the platform. However the vast majority of payloads require the host vehicle to navigate
—to be able to parameterize its position and surroundings and plan and execute trajectories
through it. Consider some typical autonomous vehicle and payloads:
• Humans in a airplane on autopilot (or car in the nearish future. CMU NavLab project)
• Scientiﬁc equipment on a Mars lander
• Mine detectors on an autonomous underwater vehicle
• Cargo containers in a port transport vehicle
• Pathology media in a hospital deliver system
1
although the Honda P5 humanoid is a good approximation
6
7
• Verbal descriptions in a museum tour guide
• A semisubmersible drill ship (oil recovery)
• Cameras on an aerial survey drone
• Obvious military uses (tend to be single mission only....)
All of the above require navigation. This course will hopefully give you some insight into
how this can be achieved.
It is worth enumerating in general terms what makes autonomous navigation so hard. The
primary reason is that the majority of mobile robots are required to work in unengineered
environments. Compare the workspaces of a welding robot in automotive plant to one that
delivers blood samples between labs in a hospital. The former operates in a highly controlled,
known, time invariant (apart from the thing being built) scene. If computer vision is used
as a sensor then the workspace can be lit arbitrarily well to mitigate against shadows and
color ambiguity. Many industrial robots work in such well known engineered environments
that very little external sensing is needed — they can do their job simply by controlling
their own internal joint angles. Hospitals are a diﬀerent ballpark altogether. The corridors
are dynamic — ﬁlling and emptying (eventually) with people on stretchers. Even if the
robot is endowed with a map of the hospital and ﬁtted with an upward looking camera to
navigate oﬀ markers on the ceiling it still has to avoid fast moving obstacles (humans) while
moving purposely towards it’s goal destination. The more generic case involves coping with
substantial scene changes (accidental or malicious ) — for example doors closing in corridors
or furniture being moved. The thing then, that makes mobile robotics so challenging is
uncertainty. Uncertainty is pervasive in this area and we must embrace it to make progress....
7
Topic 2
Introduction to Path Planning and
Obstacle Avoidance
A basic requirement of a mobile autonomous vehicle is path planning. With the vehicle in an
arbitrary initial position A we wish to issue a desired goal position B (including orientation)
and have the vehicle execute a trajectory to reach B . This sounds pretty simple and we can
think of several ways in which we could combine simple control laws that will get us from A
to B
1
Unfortunately the waters quickly become muddied when we start talking about our
other concerns:
• while executing its trajectory the vehicle must not smash into objects in the environ
ment (especially true regarding squishy humans).
• we cannot guarantee that the vehicle in question can turnonthespot and would like
to be able to operate a vehicle of arbitrary shape. These are called “kinematic” con
straints.
• we expect only uncertain estimates of the location of the robot and objects in the
environment.
The combination of pathplanning, obstacle avoidance, kinematic constraints and uncer
tainty makes for a very hard problem indeed — one which is still an active area of research.
However we can do some interesting things if we decouple some of the issues and make some
1
for example drive in a straight line from A to B until B (x,y) is reached then rotate to align with B
(theta).
8
Holonomicity 9
Figure 2.1: Two holonomic vehicles. The underwater vehicle (ODIN, University of Hawaii)
can move in any direction irrespective of pose and the complex wheel robot PPRK (CMU)
driven by a “Palm Pilot” uses wheels that allow slip parallel to their axis of rotation. A sum
of translation and slip combine to achieve any motion irrespective of pose.
simplifying assumptions. We shall begin by discussing vehicle properties and categorizing
them into two classes — holonomic and nonholonomic.
2.1 Holonomicity
Holonomicity is the term used to describe the locomotive properties of a vehicle with respect
to its workspace. We will introduce a mathematical deﬁnition of the term shortly but we
will begin by stating, in words, a deﬁnition:
A vehicle is holonomic if the number of local degrees of freedom of
movement equals the number of global degrees of freedom.
We can make this a little more clear with a few examples:
• a car is nonholonomic : the global degrees of freedom are motion in x,y and heading
however locally, a car can only move forward or turn. It cannot slide sideways.(Even
the turning is coupled to motion).
9
Holonomicity 10
Figure 2.2: A commercial nonholonomic robot vehicle (Roombavac from iRobot coorpora
tion. This vehicle can be purchased for about 100 USD  but is it’s utility great enough to
warrant the prices? Discuss....
• The “spherical” underwater robot (Odin) and the rolling wheel vehicle in Figure 2.1
are holonomic they can turn on the spot and translate instantaneously in any direction
without having to rotate ﬁrst.
• A train is holonomic: it can move forwards or backwards along the track which is
parameterised by a single global degree of freedom — the distance along the track.
• The robot “Roombavac” (iRobot corporation) vacuum cleaner in Figure 2.1 is also
nonholonomic. It can rotate in place but cannot slide in any direction — it needs to
use a turntranslateturn or turnwhiledrive (like a car) paradigm to move.
It should be obvious to you that motion control for a holonomic vehicle is much easier
than for a nonholonomic vehicle. If this isn’t obvious consider the relative complexity of
parking a car in a tight space compared to driving a vehicle that can simply slide into the
space sideways (a hovercraft).
Unfortunately for us automation engineers, the vast majority of vehicles in use today
(i.e. used by humans) are nonholonomic. In fact intrinsically holonomic vehicles are so rare
and complex (or so simple
2
) that we shall not discuss them further.
We can now place some formalism on our notion of holonomicity. We say a vehicle whose
state is parameterised by a vector x is nonholonomic if there exists a constraint Q such that
Q(x, ˙ x, ¨ x, · · · ) = 0 (2.1)
where the state derivatives cannot be integrated out. To illustrate such a constraint we will
take the case of a front wheel steered vehicle as shown in ﬁgure 2.1
2
path planning for a train is quite uninteresting
10
Conﬁguration Space 11
x=[x,y,θ]
T
θ
x'
Figure 2.3: A steered nonholonomic vehicle
Immediately we can write a constraint expressing that the vehicle cannot slip sideways
that is a function of x and its ﬁrst derivative:
˙ x
˙ y
˙
θ
¸
¸
.
−sin θ
cos θ
0
¸
¸
= 0 (2.2)
Q(x, ˙ x) = 0 (2.3)
The state and its derivatives are inseparable and by our deﬁnition the vehicle is non
holonomic.
2.2 Conﬁguration Space
We are describing the robot by its state x = [x
1
, x
2
, · · · x
n
]
T
( for a 2D plane vehicle commonly
x
1
= x x
2
= y x
3
= θ )which is a nstate parameterisation. We call the space within which
x resides the conﬁguration space C −space of the vehicle:
C =
¸
∀x
1
,x
2
···x
n
x (2.4)
The conﬁguration space ( or C −space ) is the set of all allowable conﬁgurations of the robot.
For a simple vehicle moving on a plane the conﬁguration space has the same dimension as
the work space but for more complicated robots the dimension of the conﬁguration space is
much higher. Consider the case of the bomb disposal vehicle in ﬁgure 2.2. The conﬁguration
space for such a vehicle would be 11 — 3 for the base and another 8 for the pose of the arm
and gripper. We can view obstacles as deﬁning regions of C −space that are forbidden. We
11
Conﬁguration Space 12
Figure 2.4: A vehicle with a high dimensional conﬁguration space  a commercial bomb
disposal platform (picture courtesy of Roboprobe Ltd. The conﬁguration space for a human
is immensely high dimensional —around 230.
can label this space as C
⊗
and the remaining accessible/permissable space as C
such that
C = C
⊗
∪ C
. It is often possible to deﬁne and describe the boundaries of C
⊗
(and hence the
boundaries of C
in which the vehicle must move) as a constraint equation. For example if
the workspace of a point robot in 2D x = [xy]
T
is bounded by a wall ax + by + c = 0 then
C
=
¸
{x  ax + by + c > 0}
. .. .
union of all x for which ax+by+c > 0
(2.5)
If each of the constraints imposed on C −spaceby each obstacle k is represented by an
equation of the form C
k
(x) = 0 then the open free space C
admitted by n obstacles can be
written as the following intersection:
C
=
k=n
¸
k=1
¸
{x  C
k
(x) = 0}
(2.6)
Equation 2.6 simple states what conﬁgurations are open to the vehicle —nothing more. Any
path planning algorithm must guide the vehicle along a trajectory within this space while
satisfying any nonholonomic vehicle constraints and ﬁnally deliver the vehicle to the goal
pose. This clearly is a hard problem. Two poses that may be adjacent to each other in state
space may require an arbitrarily long path to be executed to transition between them. Take,
for example, the seemingly simple task of turning a vehicle through 180
o
when it is near
a wall. One solution trajectory to the problem is shown in ﬁgure 2.2. The nonholonomic
vehicle constraints conspire with the holonomic constraint imposed by the wall to require a
complicated solution trajectory.
12
The MinkowskiSum 13
3
2
3 4
1
Figure 2.5: A simple task  turning through 180
o
quickly becomes complicated by
C −spaceconstraints.
2.3 The MinkowskiSum
Real robots have arbitrary shapes and these shapes make for complicated interactions with
obstacles which we would like to simplify. One way to do this is to transform the problem
to one in which the robot can be considered as a pointobject and a technique called the
“MinkowskiSum” does just this. The basic idea is to artiﬁcially inﬂate the extent of obstacles
to accommodate the worstcase pose of the robot in close proximity. This is easiest to
understand with a diagram shown in Figure 2.3. The idea is to replace each object with a
virtual object that is the union of all poses of the vehicle that touch the obstacle. Figure 2.3
has taken a conservative approach and “replaced” a triangular vehicle with a surrounding
circle. The minimal MinkowskiSum would be the union of the obstacle and all vehicle poses
with the vehicle nose just touching it boundary. With the obstacles suitably inﬂated the
vehicle can be thought of a pointobject and we have a guarantee that as long as it keeps to
the new, shrunken, free space it cannot hit an object
3
. Note it is usual to ﬁt a polygonal
hull around the results of the MinkowskiSum calculation to make ensuing path planning
calculations easier.
So now we have a method by which we can calculate C
. The next big question is how
exactly do we plan a path through it? How do we get from an initial pose to a goal pose?
We will consider three methods : Voronoi, “Bug” and Potential methods.
3
This doesn’t mean that awkward things won’t happen — the situation shown in ﬁgure 2.2 is still
possible. However progress can be made by planning motion such that at object boundaries the vehicle is
always capable of moving tangentially to the boundaries
13
Voronoi Methods 14
Figure 2.6: The MinkowskiSum transforms a arbitrarily shaped vehicle to a point while
inﬂating the obstacle. The result is guaranteed free space outside the inﬂated object bound
ary.
2.4 Voronoi Methods
−20 0 20 40 60 80 100 120 140 160
−20
0
20
40
60
80
100
120
Figure 2.7: A Voronoi diagram for obstacle avoidance in the presence of point objects.
Voronoi diagrams are elegant geometric constructions
4
that ﬁnd applications throughout
computer science — one is shown in ﬁgure 2.4. Points on a 2DVoronoi diagram are equi
distant from the nearest two objects in the real world. So the Voronoi diagram of two points
a, b is the line bisecting them — all points on that line are equidistant from a, b. The eﬃcient
computation of Voronoi diagrams can be quite a complex matter but what is important
here is that algorithms exist that when given a set of polygonal objects can generate the
appropriate equidistant loci. So how does this help us with path planning? Well, if we follow
the paths deﬁned by a Voronoi diagram we are guaranteed to stay maximally far away from
4
http://www.voronoi.com/
14
Bug Methods 15
nearby objects. We can search the set of points on the diagram to ﬁnd points that are closest
to and visible from the start and goal positions. We initially drive to the “highway entry”
point, follow the “Voronoi  highways” until we reach the “exit point” where we leave the
highway and drive directly towards the goal point.
2.5 Bug Methods
The generation of a global Voronoi diagram requires upfront knowledge of the environment.
In many cases this is unrealistic. Also Voronoi planners by deﬁnition keep the vehicle as far
away from objects as possible  this can have two side eﬀects. Firstly the robot may be using
the objects to localise and moving away from them makes them harder to sense. Secondly
the paths generated from a Voronoi planner can be extremely long and far from the shortest
path (try playing with the Matlab Voronoi function). An alternative family of approaches
go under the name of “ bug algorithms”. The basic algorithm is simple:
1. starting from A and given the coordinates of a goal pose B draw a line AB (it may
pass through obstacles that are known or as yet unknown)
2. move along this line until either the goal is reached or an obstacle is hit.
3. on hitting an obstacle circumnavigate its perimeter until AB is met
4. goto 2
In contrast to the Voronoi approach this method keeps the vehicle as close to the obstacles
as possible (but we won’t hit them as the obstacles have been modiﬁed by the Minkowski
sum!). However the path length could be stupidly long. A smarter modiﬁcation would be
to replace step 3 in the original algorithm with “on hitting and obstacle circumnavigate it’s
perimeter until AB is met or the line AB becomes visible in which case head for a point on
AB closer to B ” Figure 2.5 shows the kind of trajectory this modiﬁed “VisBug” algorithm
would execute. Clearly the hugging the object boundary is not always a good plan but
interestingly it is guaranteed to get the robot to the goal location if it is indeed reachable.
2.6 Potential Methods
A third very popular method of path planning is a so called “Potential Method”. Once again
the idea is very simple. We view the goal pose as a point of low potential energy and the
15
Potential Methods 16
start
goal
Figure 2.8: The visbug algorithm executing a goalseek trajectory around Southern Italy.
obstacles as areas of high potential energy. If we think of the vehicle as a ballbearing free
to roll around on a “potential terrain“ then it will naturally roll around obstacles and down
towards the goal point. The local curvature and magnitude of the potential ﬁeld can be used
to deduce a locally preferred motion. We now ﬁrm up this intuitive description with some
mathematics.
At any point x we can write the total potential U
Σ
as a sum of the potential induced U
o
by k obstacles and the potential induced by the goal U
g
:
U
Σ
(x) =
¸
i=1:k
U
o,i
(x) +U
g
(x) (2.7)
Now we know that the force F(x) exerted on a particle in a potential ﬁeld U
Σ
(x) can be
written as :
F(x) = −∇U
Σ
(x) (2.8)
= −
¸
i=1:k
∇U
o,i
(x) −∇U
g
(x) (2.9)
where ∇is the grad operator ∇V = i
∂V
∂x
+j
∂V
∂y
+k
∂V
∂x
5
This is a powerful tool  we can simply
move the vehicle in the manner in which a particle would move in a locationdependent force
ﬁeld. Equation 2.8 tells us the direction and magnitude of the force for any vehicle position
x .
5
don’t get confused here with the ∇ notation used for jacobians in the material covering nonlinear
estimation in coming pages!
16
Potential Methods 17
The next question is what exactly do the potential functions look like. Well, there is
no single answer to that — you can “roll your own”. A good choice would be to make the
potential of an obstacle be an inverse square function of the distance between vehicle and
obstacle. We may also choose an everywhereconvex potential for the goal so that where ever
we start, in the absence of obstacles, we will “fall” towards the goal point. Figure 2.6 shows
two useful potential candidates (forgive the pun). Deﬁning ρ(x) as the shortest distance
point obstacle potential
goal potential
Figure 2.9: Two typical potential functions  inverse quadratic for obstacle and quadratic
for the goal.
between the obstacle and the vehicle (at x ) and x
g
as the goal point, the algebraic functions
for these potentials are:
U
o,i
(x) = η
1
2
1
ρ(x)
−
1
ρ
0
2
∀ ρ(x) ≤ ρ
0
0 otherwise
(2.10)
U
g
(x) =
1
2
(x −x
g
)
2
(2.11)
The term ρ
0
places a limit on the region of space aﬀected by the potential ﬁeld — the virtual
vehicle is only aﬀected when it comes with ρ
0
of an obstacle. η is just a scale factor. Simple
diﬀerentiation allows us to write the force vector exerted on a virtual point vehicle as:
F
o,i
=
η
1
ρ(x)
−
1
ρ
0
1
ρ(x)
2
∂ρ(x)
∂x
∀ρ(x) ≤ ρ
0
0 otherwise
(2.12)
where
∂ρ(x)
∂x
is the vector of derivatives of the distance function ρ(x) with respect to x, y, z.
We proceed similarly for the goal potential. So to ﬁgure out the force acting on a virtual
vehicle and hence the direction the real vehicle should move in, we take the sum of all obstacle
17
Potential Methods 18
forces and the goal force – a very simple algorithm. The course website has example code
for a potential path planner written in Matlab. Download it and play.
Although elegant, the potential ﬁeld method has one serious drawback — it only acts
locally. There is no global path planning at play here — the vehicle simply reacts to local
obstacles, always moving in a direction of decreasing potential. This policy can lead to the
vehicle getting trapped in a local minimum as shown in ﬁgure 2.6. Here the vehicle will
descend into a local minima in front of the two features and will stop. Any other motion will
increase its potential. If you run the code you will see the vehicle getting stuck between two
features from time to time
6
. The potential method is someway between the bug method
and Voronoi method in terms of distance to obstacles. The bug algorithm sticks close to
them, the Voronoi as far away as possible. The potential method simply directs the vehicle
in a straight line to a goal unless it moves into the vicinity of of an obstacle in which case it
is deﬂected.
0
0.5
1
1.5
2
2.5
3
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
100
200
300
400
500
0 0.5 1 1.5 2 2.5 3
−2
−1
0
1
2
TRAP
GOAL
TRAP GOAL
Figure 2.10: A pathological potential terrain and goal combination. The vehicle can get
stuck in a localminima or “trap”
6
one way out of this is to detect a trap and temporarily move the goal point, the problem is, to where?
18
Topic 3
Estimation  A Quick Revision
3.1 Introduction
This lecture will begin to cover a topic central to mobile robotics — Estimation. There
is a vast literature on Estimation Theory encompassing a rich variation of techniques and
ideas. We shall focus on some of the most commonly used techniques which will provide the
tools to undertake a surprisingly diverse set of problems. We shall bring them to bear on
real life mobilerobot problems. Although we shall discuss and illustrate these techniques
in a domain speciﬁc fashion, you will do well to keep in your mind that these are general
techniques and can be applied to loads of other nonrobotic problems
1
.
3.2 What is Estimation?
To make sure we are all on the same page it is worth stating (in a wordy fashion) what we
mean by “Estimation”:
“Estimation is the process by which we infer the value of a quantity of interest,
x, by processing data that is in some way dependent on x .”
1
For this reason some of this material was previously taught in the B4Estimation class
19
What is Estimation? 20
There is nothing surprising there —it ﬁts intuitively with our everyday meaning of the
word. However, we may have our interest piqued when we impose the following additional
characteristics to an estimator.
• We expect the measurements to be noise corrupted and would expect to see this un
certainty in input transformed to uncertainty in inference.
• We do not expect the measurements to always be of x directly. For example a GPS
receiver processes measurements of time (4 or more satellites transmit “timeofday
atsource”) and yet it infers Euclidean position.
• We might like to incorporate prior information in our estimation. For example we may
know the depth and altitude of a submarine but not its latitude.
• We might like to employ a model that tells us how we expect a system to evolve over
time. For example given a speed and heading we could reasonably oﬀer a model on
how a ship might move between two time epochs — openloop.
• We expect these models to be uncertain. Reasonably we expect this modelling error
to be handled in the estimation process and manifest itself in the uncertainty of the
estimated state.
Pretty quickly we see that if we can build an estimator capable of handling the above
conditions and requirements we are equipping ourselves with a powerful and immensely
useful inference mechanism.
Uncertainty is central to the problem of estimation (and robotics). After all, if it weren’t
for uncertainty many problems would have a simple algebraic solution —“give me the dis
tances to three known points and the algebraicallydetermined intersection of three circles
will deﬁne my location”. We will be trying to ﬁnd answers to more realistic questions like
“I have three measurements (all with diﬀerent uncertainty) to three surveyed points (known
roughly) — what is your best estimate of my location?”. Clearly this is a much harder
and more sophisticated question. But equipped with basic probability theory and a little
calculus the next sections will derive techniques capable of answering these questions using
a few modest assumptions.
3.2.1 Deﬁning the problem
Probability theory and randomvariables are the obvious way to mathematically manipulate
and model uncertainties and as such much of this lecture will rely on your basic knowledge
20
Maximum Likelihood Estimation 21
of these subjects. We however undertake a quick review.
We want to obtain our best estimate ˆ x for a parameter x given a set of k measurements
Z
k
= {z
1
, z
2
· · · z
k
}. We will use a “hat” to denote an estimated quantity and allow the
absence of a hat to indicate the true (and unknowable) state of the variable.
We will start by considering four illuminating estimation problems  Maximum Likeli
hood, Maximum aposterior, Least Squares and Minimum Mean Squared Error.
3.3 Maximum Likelihood Estimation
It is sane to suppose that a measurement z that we are given is in some way related to
the state we wish to estimate
2
. We also suppose that measurements (also referred to as
observations in these notes) are not precise and are noise corrupted. We can encapsulate
both the relational and uncertain aspects by deﬁning a likelihood function:
L p(zx) (3.1)
The distribution p(zx) is the conditional probability of the measurement z given a particular
value of x. Figure 3.3 is just such a distribution — a Gaussian in this case( C is just a
normalising constant):
p(zx) =
1
C
e
−
1
2
(z−x)
T
P
−1
(z−x)
(3.2)
Notice that equation 3.2 is a function of both x and z . Crucially we interpret L as func
tion of x and not z as you might initially think. Imagine we have been given an observation
z and an associated pdf L for which we have a functional form of (Equation 3.2). We form
our maximum likelihood estimate ˆ x
m.l
by varying x until we ﬁnd the maximum likelihood.
Given an observation z and a likelihood function p(zx), the maximum like
lihood estimator  ML ﬁnds the value of x which maximises the likelihood
function L p(zx).
ˆ x
m.l
= arg max
x
p(zx) (3.3)
2
clearly, as if they were independent Z
k
would contain no information about x and the sensor providing
the measurements wouldn’t be much good
21
Maximum APosteriori  Estimation 22
0 5 10 15 20 25 30 35
0
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
0.09
0.1
x
Λ
z=15.000000
Figure 3.1: A Gaussian Likelihood function for a scalar observation z
3.4 Maximum APosteriori  Estimation
In many cases we will already have some prior knowledge on x . Imagine x is a random
variable which we have good reason to suppose is distributed as p(x ). For example, perhaps
we know the intended (by design) rotational velocity µ
p
of a CDROM drive  we might model
our prior knowledge of this as a gaussian ∼ N(µ
p
, σ
2
p
). If we have a sensor that can produce
an observation z of actual rotation speed with a behaviour modelled as p(zx) we can use
Bayes rule to come up with a posterior pdf p(xz) that incorporates not only the information
from the sensor but also our assumed prior knowledge on x :
p(xz) =
p(zx)p(x)
p(z)
= C ×p(zx)p(x)
The maximum aposteriori  MAP ﬁnds the value of x which maximises p(zx)p(x) (the
normalising constant is not a function of x ).
Given an observation z ,a likelihood function p(zx) and a prior distribution on
x , p(x), the maximum a posteriori estimator  MAP ﬁnds the value of x
which maximises the posterior distribution p(xz)
ˆ x
map
= arg max
x
p(zx)p(x) (3.4)
22
Maximum APosteriori  Estimation 23
Lets quickly ﬁnish the CD example, assume our sensor also has a gaussian likelihood
function, centered on the observation z but with a variance σ
2
z
:
p(x) = C
1
exp{−
(x −µ
p
)
2
2σ
2
p
}
p(zx) = C
2
exp{−
(z −x)
2
2σ
2
z
}
p(zx) =
p(zx)p(x)
p(z)
= C(z) ×p(zx) ×p(x)
= C(z) exp{−
(x −µ
p
)
2
2σ
2
p
−
(z −x)
2
2σ
2
z
}
p(zx) is maximum when the exponent is zero. A simple way to ﬁnd what value of x achieves
this is to rewrite the exponent of p(zx) as
(x −α)
2
β
2
= −
(x −µ
p
)
2
2σ
2
p
−
(z −x)
2
2σ
2
z
(3.5)
Expanding the R.H.S and comparing coeﬃcients swiftly leads to:
α =
σ
2
z
µ
p
+ σ
2
p
z
σ
2
z
+ σ
2
p
β
2
=
σ
2
z
σ
2
p
σ
2
z
+ σ
2
p
Therefore the MAP estimate ˆ x is
σ
2
z
µ
p
+σ
2
p
z
σ
2
z
+σ
2
p
and it has variance
σ
2
z
σ
2
p
σ
2
z
+σ
2
p
. It is interesting and
informative to note that as we increase the prior uncertainty in the CD speed by increasing
σ
p
towards inﬁnity then ˆ x tends towards z —the ML estimate. In other words when we have
an uninformative prior the MAP estimate is the same as the ML estimate. This makes sense
as in this case the only thing providing information is the sensor (via its likelihood function).
There several other things that you should be sure you appreciate before progressing:
Decreasing posterior variance Calling the posterior variance σ
2
+
( which is β
2
above),
we note it is smaller than that of the prior. In the simple example above this can
be seen as
1
σ
2
+
=
1
β
2
=
1
σ
2
p
+
1
σ
2
z
. This is not surprising as the measurement is adding
information
3
3
Indeed a useful metric of information is closely related to the inverse of variance — note the addition of
inverses...
23
Minimum Mean Squared Error Estimation 24
Update to prior In the Gaussian example above we can also write ˆ x
map
as an adjustment
to the prior mean:
ˆ x
map
= µ
p
+
σ
2
p
σ
2
p
+ σ
2
z
×(z −µ
p
) (3.6)
clearly if the observation matches the mode of the prior (expected) distribution then
ˆ x
map
is unchanged from the prior (but it’s uncertainty still decreases). Also note that
the correction z−µ
p
is scaled by the relative uncertainty in both prior and observation
 if the observation variance σ
2
z
is huge compared to σ
2
p
(i.e. the sensor is pretty
terrible) then irrespective of the magnitude of z −µ
p
, ˆ x
map
is still µ
p
and the decrease
in uncertainty is small. This too makes sense — if a sensor is very noisy we should
not pay too much attention to disparities between expected (the prior) and measured
values. This is a concept that we will meet again soon when we discuss Kalman
Filtering.
3.5 Minimum Mean Squared Error Estimation
Another key technique for estimating the value of a random variable x is that of minimum
mean squared error estimation. Here we assume we have been furnished with a set of
observations Z
k
. We deﬁne the following cost function which we try to minimise as a
function of ˆ x :
ˆ x
mmse
= arg min
ˆ x
E{(ˆ x −x)
T
(ˆ x −x)Z
k
} (3.7)
The motivation is clear, we want to ﬁnd an estimate of x that given all the measurement
minimises the expected value of sum of the squared errors between the truth and estimate.
Note also that we are trying to minimise a scalar quantity.
Recall from probability theory that
E{g(x)y} =
∞
−∞
g(x)p(xy)dx (3.8)
the cost function is then:
J(ˆ x, x) =
∞
−∞
(ˆ x −x)
T
(ˆ x −x)p(xZ
k
)dx (3.9)
Diﬀerentiating the cost function and setting to zero:
∂J(ˆ x, x)
∂ˆ x
= 2
∞
−∞
(ˆ x −x)p(xZ
k
)dx = 0 (3.10)
24
Recursive Bayesian Estimation 25
Splitting apart the integral, noting that ˆ x is a constant and rearranging:
∞
−∞
ˆ xp(xZ
k
)dx =
∞
−∞
xp(xZ
k
)dx (3.11)
ˆ x
∞
−∞
p(xZ
k
)dx =
∞
−∞
xp(xZ
k
)dx (3.12)
ˆ x =
∞
−∞
xp(xZ
k
)dx (3.13)
ˆ x
mmse
= E{xZ
k
} (3.14)
Why is this important?  it tells us that the mmse estimate of a random variable
given a whole bunch of measurements is just the mean of that variable conditioned on the
measurements. We shall use this result time and time again in coming derivations.
Why is it diﬀerent from the Least Squares Estimator? They are related (the
LSQ estimator can be derived from Bayes rule) but here x is a random variable where in
the LSQ case x was a constant unknown. Note we haven’t discussed the LSQ estimator yet
 but by the time you come to revise from these notes you will have.
3.6 Recursive Bayesian Estimation
The idea of MAP estimation leads naturally to that of recursive Bayesian estimation. In
MAP estimation we fused both apriori beliefs and current measurements to come up with
an estimate, ˆ x , of the underlying world state x . If we then took another measurement
we could use our previous ˆ x as the prior, incorporate the new measurement and come up
with a fresh posterior based now, on two observations. The appeal of this approach to a
robotics application is obvious. A sensor (laser radar etc) produces a timestamped sequence
of observations. At each time step k we would like to obtain an estimate for it’s state given
all observations up to that time ( the set Z
k
). We shall now use Bayes rule to frame this
more precisely:
p(x, Z
k
) = p(xZ
k
)p(Z
k
) (3.15)
and also
p(x, Z
k
) = p(Z
k
x)p(x) (3.16)
25
Recursive Bayesian Estimation 26
so
p(xZ
k
)p(Z
k
) = p(Z
k
x)p(x) (3.17)
if we assume (reasonably) that given the underlying state, observations are conditionally
independent we can write
p(Z
k
x) = p(Z
k−1
x)p(z
k
x) (3.18)
where z
k
is a single observation arriving at time k. Substituting into 3.17 we get
p(xZ
k
)p(Z
k
) = p(Z
k−1
x)p(z
k
x)p(x) (3.19)
now we invoke Bayes rule to reexpress the p(Z
k−1
x) term
p(Z
k−1
x) =
p(xZ
k−1
)p(Z
k−1
)
p(x)
(3.20)
and substituting to reach
p(xZ
k
)p(Z
k
) = p(z
k
x)
p(xZ
k−1
)p(Z
k−1
)
p(x)
p(x) (3.21)
= p(z
k
x)p(xZ
k−1
)p(Z
k−1
) (3.22)
so
p(xZ
k
) =
p(z
k
x)p(xZ
k−1
)p(Z
k−1
)
p(Z
k
)
(3.23)
note that
p(z
k
Z
k−1
) =
p(z
k
, Z
k−1
)
p(Z
k−1
)
(3.24)
=
p(Z
k
)
p(Z
k−1
)
(3.25)
so
p(Z
k−1
)
p(Z
k
)
=
1
p(z
k
Z
k−1
)
(3.26)
26
Recursive Bayesian Estimation 27
ﬁnally then we arrive at our desired result:
p(xZ
k
) =
p(z
k
x)p(xZ
k−1
)
p(z
k
Z
k−1
)
where the denominator is just a normaliser
(3.27)
So what does this tell us? Well, we recognise p(xZ
k
) as our goal  the pdf of x conditioned
on all observations we have received up to and including time k. The p(z
k
x) term is just the
likelihood of the k
th
measurement. Finally p(xZ
k−1
) is a prior — it is our last best estimate
of x at time k−1 which at that time was conditioned on all the k−1 measurements that had
been made up until that time. The recursive Bayesian estimator is a powerful mechanism
allowing new information to be added simply by multiplying a prior by a (current) likelihood.
Note that nothing special has been said about the form of the distributions manipulated
in the above derivation. The relationships are true whatever the form of the pdfs. However
we can arrive at a very useful result if we consider the case where we assume Gaussian priors
and likelihoods... the linear Kalman Filter.
27
Topic 4
Least Squares Estimation
4.1 Motivation
Imagine we have been given a vector of measurements z which we believe is related to a
state vector of interest x by the linear equation
z = Hx (4.1)
We wish to take this data and solve this equation to ﬁnd x in terms of z . Initially you
may naively think that a valid solution is
x = H
−1
z (4.2)
which is only a valid solution if H is a square matrix with  H = 0 — H must be invertible.
We can get around this problem by seeking a solution ˆ x that is closest to the ideal
1
The
metric of “closest” we choose is the following:
ˆ x = arg min
x
 Hx −z 
2
(4.3)
ˆ x = arg min
x
¸
(Hx −z)
T
(Hx −z)
¸
(4.4)
Equation 4.4 can be seen to be a “least squares” criterion. There are several ways to solve
this problem we will describe two of them  one appealing to geometry and one to a little
calculus.
1
For this section we will assume that we have more observations than required ie dim(z) > dim(x) which
assures that there is a unique “best” solution
28
Motivation 29
4.1.1 A Geometric Solution
Recall from basic linear algebra that the vector Hx is a linear sum of the columns of H. In
other words Hx ranges over the column space of H. We seek a vector x such that Hx is
closest to the data vector z . This is achieved when the error vector e = Hx−z is orthogonal
to the space in which Hx is embedded. Thus e must be orthogonal to every column of H:
H
T
(Hx −z) = 0 (4.5)
which can be rearranged as
H
T
Hx = H
T
z (4.6)
x = (H
T
H)
−1
H
T
z (4.7)
This is the least squares solution for x . The matrix (H
T
H)
−1
H
T
is called the pseudoinverse
of H.
4.1.2 LSQ Via Minimisation
We can expand and set the derivative of Equation 4.4 to zero :
 Hx −z 
2
= x
T
H
T
Hx −x
T
H
T
z −zHx +z
T
z (4.8)
∂  Hx −z 
2
∂x
= 2H
T
Hx −2H
T
z (4.9)
= 0
⇒x = (H
T
H)
−1
H
T
z (4.10)
The least squares solution for the linear system Ax = b with A
m,n
m > n is
ˆ x = (A
T
A)
−1
A
T
b (4.11)
29
Weighted Least Squares 30
4.2 Weighted Least Squares
Imagine now that we have some information regarding how reliable each of the elements in
z is. We might express this information as a diagonal measurement covariance matrix R :
R =
σ
2
z1
0 0
0 σ
2
z2
· · ·
.
.
.
.
.
.
.
.
.
¸
¸
¸
(4.12)
It would be natural to weight each element of the error vector e according to our uncertainty
in each element of the measurement vector z  ie by R
−1
. The new minimisation becomes:
ˆ x = arg min
x
 R
−1
(Hx −z) 
2
(4.13)
Carrying this through the same analysis yields the weighted linear least squares esti
mate:
ˆ x = (H
T
R
−1
H)
−1
HR
−1
z (4.14)
4.2.1 Nonlinear Least Squares
The previous section allows us to derive a least squares estimate under a linear observation
model. However most interesting problems will involve nonlinear models  measuring the
Euclidean distance between two points for example. We now have a new minimisation task:
ˆ x = arg min
x
 h(x) −z 
2
(4.15)
We begin by assuming we have some initial guess of a solution x
0
. We seek a vector δ
such that x
1
= x
0
+ δ and  h(x
1
) − z 
2
is minimised. To proceed we use a Taylor series
expansion:
h(x
0
+ δ) = h(x
0
) +∇H
x
0
δ (4.16)
 h(x
1
) −z 
2
= h(x
0
) +∇H
x
0
δ −z 
2
(4.17)
= ∇H
x
0
. .. .
A
δ −(z −h(x
0
))
. .. .
b

2
(4.18)
where
∇H
x
0
=
∂h
∂x
=
∂h
1
∂x
1
· · ·
∂h
1
∂x
m
.
.
.
.
.
.
∂h
n
∂x
1
· · ·
∂h
n
∂x
m
¸
¸
¸
. .. .
evaluated at x
0
(4.19)
30
Weighted Least Squares 31
Equation 4.18 can be seen to be a linear least square problem  something we have already
solved and stated in equation 4.11. By inspection then we can write an expression for δ that
minimises the right hand side of 4.18:
δ =
∇H
x
0
T
∇H
x
0
−1
∇H
x
0
T
[z −h(x
0
)] (4.20)
We now set x
1
= x
0
+δ and iterate again until the norm falls below a tolerance value. Like the
linear case, there is a natural extension to the weighted non linear case. For a measurement
z with variance R the weighted nonlinear least squares algorithm is as follows:
1. Begin with an initial guess ˆ x
2. Evaluate
δ =
∇H
ˆ x
T
R
−1
∇H
ˆ x
−1
∇H
ˆ x
T
R
−1
[z −h(ˆ x)]
3. Set ˆ x = ˆ x + δ
4. If  h(ˆ x) −z 
2
> goto 1 else stop.
4.2.2 Long Baseline Navigation  an Example
So why is the nonlinear LSQ problem interesting? Well, nonlinear least squares
allows us to solve some interesting and realistic navigation problems. For example, consider
the case of an autonomous underwater vehicle (AUV) moving within a network of acoustic
beacons. The AUV shown in ﬁgure 4.2.2 is about to be launched on a mine detection mission
in the Mediterranean. Within the hull (which ﬂoods) is a transceiver which emits a “call”
pulse into the water column. Beacons deployed at known locations detect this pulse and
reply with “acknowledge” pulses which are detected by the transceiver. The diﬀerence in
time between “call” and “acknowledge” is proportional to the distance between vehicle and
each beacon. Figure 4.2.2 shows a diagram of this kind of navigation (which is very similar
to how GPS works).
Imagine the vehicle is operating within a network of 4 beacons. We wish to ﬁnd an
LSQ estimate of the vehicle’s position x
v
= [x, y, z]
T
. Each beacon i is at known position
x
bi
= [x
bi
, y
bi
, z
bi
]
T
. We shall assume that the observations become available simultaneously
31
Weighted Least Squares 32
Figure 4.1: A Long Baseline Acoustic Network for an Autonomous Underwater Vehicle
(AUV)
so we can stack them into a single vector
2
. The model of the longbaseline transceiver
operating in water with speed of sound c can be written as follows:
z =
t
1
t
2
t
3
t
4
T
= h(x
v
) (4.21)
t
1
t
2
t
3
¸
¸
=
2
c
 x
b1
−x
v

 x
b2
−x
v

 x
b3
−x
v

 x
b4
−x
v

¸
¸
¸
¸
(4.22)
so
∇H
x
v
= −
2
rc
∆
x1
∆
y1
∆
y1
∆
x2
∆
y2
∆
y2
∆
x3
∆
y3
∆
y3
∆
x4
∆
y4
∆
y4
¸
¸
¸
¸
(4.23)
where
∆
xi
= x
bi
−x
∆
yi
= y
bi
−y
∆
zi
= z
bi
−z
r =
(x
bi
−x)
2
) + (y
bi
−y)
2
) + (z
bi
−z)
2
)
2
In practice of course the signal that travels the furthest comes in last  the measurements are staggered.
This is only a problem if the vehicle is moving
32
Weighted Least Squares 33
Figure 4.2: An autonomous underwater vehicle about to be launched. The large fork on the
front is a sonar designed to detect buried mines. Within the hull is a transceiver which emits
a “call” pulse into the water column. Beacons deployed at known locations detect this pulse
and reply with a “acknowledge” pulses which are detected by the transceiver. The diﬀerence
in time between “call” and “acknowledge” is proportional to the distance between vehicle
and each beacon. (photo courtesy of MIT) see ﬁgure 4.2.2
With the jacobian calculated and given a set of four measurements to the beacons we
can iteratively apply the nonlinear least squares algorithm until the change in x
v
between
iterations becomes small enough to disregard. You might be surprised that many of the
oil rigs ﬂoating in the Gulf of Mexico basically use this method to calculate their position
relative to the wellhead thousands of meters below them. Oil rigs are perhaps one of the
largest mobilerobots you are likely to encounter.
One issue with the Least Squares solution is that enough data has to be accumulated to
make the system observable  H
T
H must be invertible. For example, in our subsea example,
acousticrefractive properties of the water column may mean that replies from a particular
beacon are never detected. Alternatively acoustic noise may obliterate the true signal leading
to false detections. Figure 4.2.2 shows some real LBL data from vehicle shown in Figure
4.2.2. Most of the noise is due to the enormously powerful forklike sonar on its nose.
The Kalman ﬁlter which we shall discuss, derive and use shortly is one way to overcome
this problem of instantaneous unobservability.
33
Weighted Least Squares 34
0 500 1000 1500 2000 2500 3000
0
1000
2000
3000
Raw Data
T
O
F
x
C
(
s
)
0 500 1000 1500 2000 2500 3000
0
500
1000
1500
Data Sketch
T
O
F
x
C
(
m
)
0 500 1000 1500 2000 2500 3000
0
500
1000
1500
Recovered Data
T
O
F
x
C
(
m
)
Time (s)
SAS turns on
Figure 4.3: Some real data from the ocean. The synthetic aperture adds a lot of noise to the
LBL sensing scheme. However it is possible to separate true signal from noise as shown in
the lower graphs. The y axes are time of ﬂight × c giving eﬀective distance between vehicle
and beacons.
34
Topic 5
Kalman Filtering Theory, Motivation
and Application
Keep in mind
The following algebra is simple but a little on the laborious side and was provided here
for completeness. The following section will involve a linear progression of simple algebraic
steps. I hope you will derive some degree of intellectual satisfaction in seeing the equations
being derived using only Bayes rule. Of course, like many things in engineering, derivations
need not be done at every invocation of a smart idea but it is important to understand the
underlying principles. The exams will not ask for a complete derivation of the Kalman ﬁlter
but may ask you to explain the underlying concepts with reference to the key equations.
5.1 The Linear Kalman Filter
We will now assume that the likelihood p(zx) and a prior p(x) on x are Gaussian. Further
more, initially, we are going to model our sensor as something that produces a observation
z which is a noise corrupted linear function of the state x :
z = Hx +w (5.1)
Here w is a gaussian noise with zero mean and covariance R so that
p(w) =
1
(2π)
n/2
 R 
1/2
exp{−
1
2
w
T
R
−1
w}. (5.2)
35
The Linear Kalman Filter 36
Note that up until now the examples have often been dealing with scalar 1D distributions.
Here we generalise to the multidimensional case but things should still look familiar to you.
We shall let the state have n
x
dimensions and the observation vector have n
z
dimensions.
This allows us to write down a likelihood function:
p(zx) =
1
(2π)
nz/2
 R 
1/2
exp{−
1
2
(z −Hx)
T
R
−1
(z −Hx)} (5.3)
.
We can also use a multidimensional Gaussian with mean x
and covariance P
to describe
our prior belief in x :
p(x) =
1
(2π)
nx/2
 P

1/2
exp{−
1
2
(x −x
)
T
P
−1
(x −x
)} (5.4)
.
Now we can use Bayes rule to ﬁgure out an expression for the posterior p(xz).:
p(xz) =
p(zx)p(x)
p(z)
(5.5)
=
p(zx)p(x)
∞
−∞
p(zx)p(x)dx
(5.6)
=
1
(2π)
nz/2
R
1/2
exp{−
1
2
(z −Hx)
T
R
−1
(z −Hx)}
1
(2π)
nx/2
P

1/2
exp{−
1
2
(x −x
)
T
P
−1
(x −x
)}
C(z)
(5.7)
This looks pretty formidable but we do know that we will end up with a gaussian (gaussian
× gaussian = gaussian) and scale factors are not important therefore we can disregard the
denominator and focus on the product:
exp{−
1
2
(z −Hx)
T
R
−1
(z −Hx)} exp{−
1
2
(x −x
)
T
P
−1
(x −x
)} (5.8)
or equivalently:
exp{−1/2
(z −Hx)
T
R
−1
(z −Hx) + (x −x
)P
−1
(x −x
)
T
} (5.9)
We know (because gaussian × gaussian = gaussian) that we can ﬁnd a way to express the
above exponent in a quadratic way:
(x −x
⊕
)
T
P
−1
⊕
(x −x
⊕
) (5.10)
36
The Linear Kalman Filter 37
.
We can ﬁgure out the new mean x
⊕
and covariance P
⊕
by expanding expression 5.9 and
comparing terms with expression 5.10. Remember we want to ﬁnd the new mean because
Equation 3.14 tells us this will be the MMSE estimate. So expanding 5.9 we have:
x
T
P
−1
x−x
T
P
−1
x
−x
T
P
−1
x
+x
T
P
−1
x
+x
T
H
T
R
−1
Hx−x
T
H
T
R
−1
z−zR
−1
Hx+z
T
R
−1
z
(5.11)
Now collecting terms this becomes:
x
T
(P
−1
+H
T
R
−1
H)x−x
T
(P
−1
x
+H
T
R
−1
z)−(x
T
P
−1
+zR
−1
H)x+(x
T
P
−1
x
+z
T
R
−1
z)
(5.12)
Expanding 5.10:
x
T
P
−1
⊕
x −x
T
P
−1
⊕
x
⊕
−x
T
⊕
P
−1
⊕
x +x
T
⊕
P
−1
⊕
x
⊕
. (5.13)
Comparing ﬁrst terms in 5.12 and 5.13 we immediately see that
P
⊕
= (P
−1
+H
T
R
−1
H)
−1
. (5.14)
Comparing the second terms we see that:
P
−1
⊕
x
⊕
= P
−1
x
+H
T
R
−1
z. (5.15)
Therefore we can write the MMSE estimate, x
⊕
as
x
⊕
= (P
−1
+H
T
R
−1
H)
−1
(P
−1
x
+H
T
R
−1
z). (5.16)
We can combine this result with our understanding of the recursive Bayesian ﬁlter we
covered in section 3.6. Every time a new measurement becomes available we update our
estimate and its covariance using the above two equations.
There is something about the above two equations 5.14 and 5.16 that may make them
inconvenient — we have to keep inverting our prior covariance matrix which may be com
putationally expensive if the statespace is large
1
. Fortunately we can do some algebra to
come up with equivalent equations that do not involve an inverse.
We begin by stating a block matrix identity. Given matrices A , B and C the following
is true (for nonsingular A ):
(A+BCB
T
)
−1
= A
−1
−A
−1
B(C
−1
+B
T
A
−1
B)
−1
B
T
A
−1
(5.17)
1
in some navigation applications the dimension of x can approach the high hundreds
37
The Linear Kalman Filter 38
We can immediately apply this to 5.14 to get:
P
⊕
= P
−P
H
T
(R+HP
H
T
)
−1
HP
(5.18)
= P
−WSW
T
(5.19)
(5.20)
or
= (I −WH)P
(5.21)
where
S = HP
H
T
+R (5.22)
W = P
H
T
S
−1
(5.23)
Now look at the form of the update equation 5.16 it is a linear combination of x and z .
Combining 5.20 with 5.16 we have:
x
⊕
= (P
−1
+H
T
R
−1
H)
−1
(P
−1
x
+H
T
R
−1
z) (5.24)
= (P
−1
+H
T
R
−1
H)
−1
(H
T
R
−1
z) + (I −WH)P
(P
−1
x
) (5.25)
= (P
−1
+H
T
R
−1
H)
−1
(H
T
R
−1
z) +x
+W(−Hx
) (5.26)
= Cz +x
+W(−Hx
) (5.27)
where
C = (P
−1
+H
T
R
−1
H)
−1
H
T
R
−1
(5.28)
Taking a step aside we note that both
H
T
R
−1
(HP
H
T
+R) = H
T
R
−1
HP
H
T
+H
T
(5.29)
and also
(P
−1
+H
T
R
−1
H)P
H
T
= H
T
+H
T
R
−1
HP
H
T
(5.30)
so
(P
−1
+H
T
R
−1
H)
−1
H
T
R
−1
= P
H
T
(HP
H
T
+R)
−1
(5.31)
therefore
C = P
H
T
S
−1
(5.32)
= Wfrom 5.23 (5.33)
38
The Linear Kalman Filter 39
Finally then we have
x
⊕
= x
+W(z −Hx
) (5.34)
We can now summarise the update equations for the Linear Kalman Filter:
The update equations are as follows. Given an observation z
with uncertainty (covariance) R and a prior estimate x
with
covariance P
the new estimate and covariance are calculated
as:
x
⊕
= x
+Wν
P
⊕
= P
−WSW
T
where the “Innovation” ν is
ν = z −Hx
the “Innovation Covariance” S is given by
S = HP
H
T
+R
and the “Kalman Gain” W is given by
W = P
H
T
S
−1
5.1.1 Incorporating Plant Models  Prediction
The previous section showed how an observation vector (z ) related in some linear way to
a state we wish to estimate (x ) can be used to update in a optimal way a prior belief
/estimate. This analysis lead us to the so called Kalman update equations. However there
is another case which we would like to analyse: given a prior at time k − 1, a (imperfect)
model of a system that models the transition of state from time k −1 to k, what is the new
estimate at time k?
39
The Linear Kalman Filter 40
Mathematically we are being told that the true state behaves in the following fashion:
x(k) = Fx(k −1) +Bu(k) +Gv(k) (5.35)
Here we are modelling our uncertainty in our model by adding a linear transformation
of a “white noise”
2
term v with strength (covariance) Q. The term u(k) is the control
vector at time k. It models actions taken on the plant that are independent of the state
vector itself. For example, it may model an acceleration on a model that assumes constant
velocity. (A good example of u is the steer angle on a car input into the system by a driver
(machine or human))
The ij notation
Our general approach will be based on an inductive argument. However ﬁrst we shall intro
duce a little more notation. We have already shown (and used) the fact that given a set Z
k
of k observations the MMSE estimate of x is
ˆ x
mmse
= E{xZ
k
} (5.36)
We now drop the “mmse” subscript and start to use two parenthesised time indexes i and
j. We use ˆ x(ij) to mean the “MMSE estimate of x at time i given observations up until
time j”. Incorporating this into Equation 5.36 we have
ˆ x(ij) = E{x(i)Z
j
}. (5.37)
We also use this notation to deﬁne a covariance matrix P(ij) as
P(ij) = E{(x(i) − ˆ x(ij))(x(i) − ˆ x(ij))
T
Z
j
} (5.38)
which is the mean square error of the estimate ˆ x(ij)
3
.
2
a random variable distributed according to a zeromean gaussian pdf
3
If this sounds confusing focus on the fact that we are maintaining a probability distribution for x . Our
estimate at time i using measurements up until time j (ˆ x(ij)) is simply the mean of this distribution and
it has variance P(ij). If our distribution is in fact a Gaussian then these are the only statistics we need to
fully characterize the pdf. This is exactly what a Kalman ﬁlter does — it maintains the statistics of a pdf
that “best” represents x given a set of measurements and control inputs
40
The Linear Kalman Filter 41
ˆ x (i—j) is the estimate of x at time i given measurements up until time j. Com
monly you will see the following combinations:
• ˆ x(kk) estimate at time k given all available measurements. Often simply
called the estimate
• ˆ x(kk −1) estimate at time k given ﬁrst k −1 measurements. This is often
called the prediction
Now back to our question about incorporating a plant model F and a control signal u.
Imagine at time k we have been provided with MMSE estimate of x (k1). Using our new
notation we write this estimate and its covariance as ˆ x(k − 1k − 1) and P(k − 1k − 1).
We are also provided with a control signal u(k) we want to know how to ﬁgure out a new
estimate ˆ x(kk −1) at time k. Note that we are still only using measurements up until time
(k − 1) ( i > j in Equation 5.37) however we are talking about an estimate at time k. By
convention we call this kind of estimate a “prediction”. You can think of predicting as an
“open loop” step as no measurements of state are used to correct the calculation.
We can use our “conditional expectation result” to progress:
ˆ x(kk −1) = E{x(kZ
k−1
} (5.39)
= E{Fx(k −1) +Bu(k) +Gv(k)Z
k−1
} (5.40)
= FE{x(k −1)Z
k−1
} +Bu(k) +GE{v(k)Z
k−1
} (5.41)
= Fˆ x(k −1k −1) +Bu(k) +0 (5.42)
This is a both simple and intuitive result. It simply says that the best estimate at time k
given measurements up until time k − 1 is simply the projection of the last best estimate
ˆ x(k − 1k − 1) through the plant model. Now we turn our attention to the propagation of
the covariance matrix through the prediction step.
P(kk −1) = E{(x(k) − ˆ x(kk −1))(x(k) − ˆ x(kk −1))
T
Z
k−1
} (5.43)
perfoming the subtraction ﬁrst
x(k) − ˆ x(kk −1) = (Fx(k −1) +Bu(k) +Gv(k)) −(Fˆ x(k −1k −1) +Bu(k)) (5.44)
= F(x(k −1) − ˆ x(k −1k −1)) +Gv(k) (5.45)
41
The Linear Kalman Filter 42
so
P(kk −1) = E{(F(x(k −1) − ˆ x(k −1k −1)) +Gv(k))× (5.46)
(F(x(k −1) − ˆ x(k −1k −1)) +Gv(k))
T
Z
k−1
} (5.47)
Now we are going to make a moderate assumption: that the previous estimate ˆ x(k −1k −1)
and the noise vector (modelling inaccuracies in either model or control) are uncorrelated (i.e.
E{(x(k −1) − ˆ x(k −1k −1))v(k)
T
} is the zero matrix). This means that when we expand
Equation 5.47 all cross terms between ˆ x(k −1k −1) and v(k) disappear. So:
P(kk −1) = FE{(x(k −1) − ˆ x(k −1k −1))(x(k −1) − ˆ x(k −1k −1))
T
Z
k−1
}F
T
+
(5.48)
GE{v(k)v(k)
T
Z
k−1
}G
T
(5.49)
P(kk −1) = FP(k −1k −1)F
T
+GQG
T
(5.50)
This result should also seem familiar to you ( remember that if x ∼ N(µ, Σ) and y = Mx
then y ∼ N(Mµ, MΣM
T
) ?).
5.1.2 Joining Prediction to Updates
We are now almost in a position to put all the pieces together. To start with we insert our
now temporalconditional notation into the Kalman update equations. We use ˆ x(kk −1) as
the prior x
and process an observation z at time k. The posterior x
⊕
becomes ˆ x(kk)
ˆ x(kk) = ˆ x(kk −1) +W(k)ν(k)
P(kk) = P(kk −1) −W(k)SW(k)
T
ν(k) = z(k) −Hx(kk −1)
S = HP(kk −1)H
T
+R
W(k) = P(kk −1)H
T
S
−1
So now we are in a position to write down the “standard Linear Kalman Filter Equations”.
If the previous pages of maths have started to haze your concentration wake up now as you
will need to know and appreciate the following:
42
The Linear Kalman Filter 43
Linear Kalman Filter Equations
prediction:
ˆ x(kk −1) = Fˆ x(k −1k −1) +Bu(k) (5.51)
P(kk −1) = FP(k −1k −1)F
T
+GQG
T
(5.52)
update:
ˆ x(kk) = ˆ x(kk −1) +W(k)ν(k) (5.53)
P(kk) = P(kk −1) −W(k)SW(k)
T
(5.54)
where
ν(k) = z(k) −Hˆ x(kk −1) (5.55)
S = HP(kk −1)H
T
+R (5.56)
W(k) = P(kk −1)H
T
S
−1
(5.57)
5.1.3 Discussion
There are several characteristics of the Kalman Filter which you should be familiar with
and understand well. A ﬁrm grasp of these will make your task of implementing a KF for a
robotics problem much easier.
Recursion The Kalman Filter is recursive. The output of one iteration becomes the input
to the next.
Initialising Initially you will have to provide P(00) and ˆ x(00). These are initial guesses
(hopefully derived with some good judgement)
Structure The Kalman ﬁlter has a predictorcorrector architecture. The prediction step
is corrected by fusion of a measurement. Note that the innovation ν is a diﬀerence
between the actual observation and the predicted observation (Hˆ x(kk − 1)). If these
43
The Linear Kalman Filter 44
two are identical then there is no change to the prediction in the update step —both
observation and prediction are in perfect agreement. Fusion happens in data space.
Asynchronisity The update step need not happen at every iteration of the ﬁlter. If at a
given time step no observations are available then the best estimate at time k is simply
the prediction ˆ x(kk −1).
Prediction Covariance Inﬂation Each predict step inﬂates the covariance matrix — you
can see this by the addition of GQG
T
. This makes sense sice we are only using a
preconceived idea of how x will evolve. By our own admission — the presence of a
noise vector in the model — the model is inaccurate and so we would expect certainty
to dilate (uncertainty increase) with each prediction.
Update Covariance Deﬂation Each update step generally
4
deﬂates the covariance ma
trix. The subtraction of WSW
T 5
during each update illustrates this. This too makes
sense. Each observation fused contains some information and this is “added” to the
state estimate at each update. A metric of information is related to the inverse of
covariance — note how in equation 5.16 H
T
R
−1
H is added to the inverse of P, this
might suggest to you that information is additive. Indeed, equation 5.16 is the so called
“information form” of the Kalman Filter.
Observability The measurement z need not fully determine the state x (ie in general H
is not invertible). This is crucial and useful. In a least squares problem at every
update there have to be enough measurements to solve for the state x , However the
Kalman ﬁlter can perform updates with only partial measurements. However to get
useful results over time the system needs to be observable otherwise the uncertainty in
unobserved state components will grow without bound. Two factors make this possible
: the fact that the priors presumably contain some information about the unobserved
states (they were observed in some previous epoch) and the role of correlations.
Correlations The covariance matrix P is highly informative. The diagonals are the princi
pal uncertainties in each of the state vector elements. The oﬀ diagonals tell us about
the relationships between elements of our estimated vector ˆ x — how they are corre
lated. The Kalman ﬁlter implicitly uses these correlations to update states that are not
observed directly by the measurement model! Let’s take a real life example. Imagine
we have a model [F, BQ] of a plane ﬂying. The model will explain how the plane
moves between epochs as a function of both state and control. For example at 100m/s,
nose down 10 deg, after 100ms the plane will have travelled 10m forward (y direction)
and perhaps 1.5 m down (in z direction). Clearly the changes and uncertainties in y
and z are correlated — we do not expect massive changes in height for little change
4
technically it never increases it
5
which will always be positive semi deﬁnite
44
The Linear Kalman Filter 45
New control u(k)
available ?
x(kk1) = Fx(k1k1) +Bu(k)
P(kk1) = F P(k1k1) F
T
+GQG
T
x(kk1) = x(k1k1)
P(kk1) = P(k1k1)
x(kk) = x(kk1)
P(kk) = P(kk1)
k=k+1
Delay
Prepare For Update
Prediction
Update
Input to plant enters here
e.g steering wheel angle
yes no
yes no
prediction is last
estimate
estimate is last
prediction
S = H P(kk1) H
T
+R
W = P(kk1) H
T
S
1
v(k) = z(k)  H x(kk1)
x(kk) = x(kk1)+ Wv(k)
P(kk) = P(kk1)  W S W
T
New observation z(k)
available ?
Data from sensors enter
algorithm here e.g compass
measurement
Figure 5.1: Flow chart for the Kalman ﬁlter. Note the recursive structure and how simple it
is to implement once the model matrices F and H have been speciﬁed along with the model
uncertainty matrices R and Q .
in distanceoverground in normal ﬂight conditions. Now comes the clever part. The
plane is equipped with an altimeter radar which measures height above the ground  a
direct measurement of z. Fusing this measurement in the Kalman ﬁlter will result in a
change in estimated height and also a change in yposition. The reason being that the
correlations between height and yposition maintained in the covariance matrix mean
that changes in estimated height should imply changes in estimated yposition because
the two states are codependent. The exercises associated with these lectures should
illustrate this fact to you further.
45
Using Estimation Theory in Mobile Robotics 46
5.2 Using Estimation Theory in Mobile Robotics
Previous pages have contained a fair amount of maths taking you through the derivation of
various estimators in a steady fashion. Remember estimation theory is at the core of many
problems in mobile robotics — sensors are uncertain, models are always incomplete. Tools
like the Kalman ﬁlter
6
give a powerful framework in which we can progress.
We will now apply these freshly derived techniques to problems in mobile robotics fo
cussing particularly on navigation. The following sections are not just a standaloneexample.
Within the analysis, implementation and discussion a few new ideas are introduced which
will be used later in the course.
5.2.1 A Linear Navigation Problem  “Mars Lander”
A Lander is at an altitude x above the planet and uses a time of ﬂight radar to detect
altitude—see Figure 5.2.1. The onboard ﬂight controller needs estimates of both height
and velocity to actuate control surfaces and time rocket burns. The task is to estimate both
altitude and descent velocity using only the radar. We begin modelling by assuming that the
vehicle has reached its terminal velocity (but this velocity may change with height slowly).
A simple model would be:
x(k) =
¸
1 δT
0 1
. .. .
F
x(k −1) +
¸
δT
2
2
δT
. .. .
G
v(k) (5.58)
where δT is the time between epochs and the state vector x is composed of two elements
altitude and rate of altitude change (velocity)
x(k) =
¸
h
˙
h
. (5.59)
The process noise vector is a scalar, gaussian process with covariance Q. It represents noise
in acceleration (hence the quadratic time dependence when adding to a positionstate).
Now we need to write down the observation equations. Think of the observation model
as “explaining the observations as a function of the thing you want to estimate”.
6
it is not the only method though
46
Using Estimation Theory in Mobile Robotics 47
x
z
Figure 5.2: A simple linear navigation problem. A Lander is at an altitude x above the
planet and uses a time of ﬂight radar to detect altitude. The onboard ﬂight controller needs
estimates of both height and velocity to actuate control surfaces and time rocket burns. The
task is to estimate both altitude and descent velocity using only the radar.
So we can write
z(k) = Hx(k) +w(k) (5.60)
z(k) =
2
c
0
¸
h
˙
h
+w(k) (5.61)
These are the “truth models” of the system (note there are no “hats” or (k—k) notations
involved). We will never known the actual value of the noises at any epoch but we model the
imperfections in the sensor and motions models that they represent by using their covariance
matrices (R and Q respectively) in the ﬁlter equations.
We are now in a position to implement this example in Matlab. Section 11.1 is a print
out of the entire source code for a solution  it can also be downloaded from the course
website ¡http://www.robots.ox.ac.uk/∼pnewman : teaching¿. The exact source created the
following graphs and so all parameters can be read from the listing. It’s important that
you can reconcile all the estimation equations we have derived with the source and also that
you understand the structure of the algorithm —the interaction of simulator, controller and
estimator.
47
Using Estimation Theory in Mobile Robotics 48
100 200 300 400 500
2000
4000
6000
8000
10000
True and Estimated Trajectories
A
l
t
i
t
u
d
e
(
m
)
100 200 300 400 500
−150
−100
−50
0
V
e
l
o
c
i
t
y
(
k
m
/
h
)
time (s)
Chute @ 178.2, Rockets @ 402.1, Lands @ 580.9
0 200 400 600
−10
−5
0
5
10
Trajectories Error (XEst−XTrue)
100 200 300 400 500
−1
−0.5
0
0.5
1
time (s)
5.2.2 Simulation Model
The simulation model is nonlinear. This of course is realistic — we may model the world
within our ﬁlter as a well behaved linear system
7
but the physics of the real world can be
relied upon to conspire to yield something far more complicated. However, the details of
the models employed in the simulation are not important. One point of this example is to
illustrate that sometimes we can get away with simple approximations to complex systems.
We only examine the vertical descent velocity (i.e. we ignore the coriolis acceleration)—
it’s as though we were dropping a weight vertically into a layer of atmosphere. The drag
exerted on the vehicle by the atmosphere is a function of both vehicle formfactor, velocity
and atmospheric density as a function of height (modelled as a saturating exponential).
7
we shall shortly introduce a way to use nonlinear models but the idea that the world is always more
complicated than we care to imagine is still valid
48
Using Estimation Theory in Mobile Robotics 49
50 100 150 200 250 300 350 400 450 500 550
0
1
2
3
4
5
6
x 10
−5
Observations
time
M
e
a
s
u
r
e
m
e
n
t
(
t
i
m
e
o
f
f
l
i
g
h
t
s
e
c
o
n
d
s
)
0 100 200 300 400 500 600
−5
0
5
x 10
−7
Innovation Sequence
time
I
n
n
o
v
a
t
i
o
n
(
t
i
m
e
o
f
f
l
i
g
h
t
s
e
c
o
n
d
s
)
Figure 5.3:
Vehicle Controller
The controller implemented does two things. Firstly when the vehicle descends below a
predetermined height it deploys a parachute which increases its eﬀective aerodynamic drag.
Secondly, it ﬁres retroburners when it drops below a second altitude threshold. A simple
control law based on altitude and velocity is used to control the vehicle to touch down with
a low velocity. At the point when rockets are ﬁred, the parachute is also jettisoned.
The important point here is that the controller operates on estimated quantities. If the
estimated quantities are in error it is quite easy to envision bad things happening. This is a
point common to all robotic systems—actions (involving substantial energies) are frequently
executed on the basis estimates. The motivation to understand estimation process and its
failure modes is clear!
Analysis of Mars Lander Simulation
Flight Pattern Figure 5.2.1 shows the simulated(true) and estimated states using the code
listed in Section 11.1. Initially the vehicle is high in thin atmosphere which produces
little drag. The vehicle accelerates through the high levels of the atmosphere. Soon the
density increases and the vehicle brakes under the eﬀect of drag to reach a quasisteady
49
Using Estimation Theory in Mobile Robotics 50
state terminal velocity. When the parachute opens the instantaneous increase drag
decelerates the vehicle rapidly until another steady state terminal velocity is achieved.
Finally shortly before landing the retroburners are ﬁred to achieve a smooth landing
 essentially decelerating the vehicle smoothly until touch down.
Fitness of Model The ﬁlter vehicle model < F, G > is one of constant velocity (noise
in acceleration). Oﬀtheblocks then we should expect good estimation performance
during periods of constant velocity. Examining the graphs we see this is true. When
the velocity is constant both position and velocity are tracked well. However during
periods of rapid acceleration we see poor estimates of velocity emerging. Note that
during these times the innovation sequence and truthestimated graphs ‘spike’....
Derivation of Velocity Note that the observation model H does not involve the velocity
state and yet the ﬁlter is clearly estimating and tracking velocity pretty well. At no
point in the code can you ﬁnd statements like vel = (xnew − xold)/∆T. The ﬁlter
is not explicitly diﬀerentiating position to derive velocity — instead it is inferring it
through the models provided. The mechanism behind this has already been discussed
in section 5.1.3. The ﬁlter is using correlations (oﬀ diagonals) in the 2 × 2 matrix P
between position and velocity states. The covariance matrix starts oﬀ being diagonal
but during the ﬁrst prediction step it becomes fully populated.
8
. Errors in position
are correlated through the vehicle model to errors in velocity. This is easy to spot in
the plant model as predicted position is a function of current position estimate and
velocity estimate. Here the KF is working as an observer of a hidden state — an
immensely useful characteristic. However there is no free lunch. Note how during
times of acceleration the velocity estimate lags behind the true velocity. This makes
sense
9
the velocity state is being dragged (and hence lags) through state space by the
correlations to directly observed position.
Filter Tuning An obvious question to ask is how can the ﬁlter be made “tighter”? How can
we produce a more agile tracker of velocity? The answer lies in part with the process
noise strength Q . The addition of GQG
T
at each time step dilutes the interstate
correlations. By making Q smaller we maintain stronger correlations and track inferred
velocity. But we cannot reduce Q too far— it has to model the uncertainty in our
model. If we reduce it too much we will have too much conﬁdence in our predictions
and the update stage will have little corrective eﬀect. The process of choosing a suitable
Q (and R ) is called tuning. It is an important part of KF deployment and can be hard
to do in practice. Fortunately there are a few concepts that can help in this process.
Their derivation is more suited for a course in stochastic estimation rather than mobile
8
You should download the code and check this out for yourself. Try forcing the oﬀ diagonals to zero
and see the eﬀect
9
expand the KF update equations with a constant velocity model and full P matrix. Note how the
change in velocity W(2, 1) is a function of the oﬀ diagonals
50
Using Estimation Theory in Mobile Robotics 51
robotics and so some of them are stated here as a set of rules (valid for linear Gaussian
problems);
Innovation Sequence The innovation sequence should be a white (utterly random),zero
mean process. It is helpful to think heuristically of the innovation as the exhaust
from the ﬁlter. If the ﬁlter is optimal all information will have been extracted
from previous observations and the diﬀerence between actual and predicted ob
servations will be noise
10
.
SBound The innovation covariance matrix S provides a 1σ
2
statistical bound on
the innovations sequence. The ith element innovation sequence should lie with
+/ −
√
S
ii
. Figure 5.2.2 shows these bounds plotted. Note how during periods
of constant velocity the innovation sequence (a scalar sequence in this case) is
bounded around 66 percent of the time.
Normalised Innovation Squared The scalar quantity ν(k)
T
S
−1
ν(k) is distributed
according to chisquared distribution with degree of freedom equal to the dimen
sion of the innovation vector ν(k)
Estimate Error In a deployed system the only information available to the engi
neer is the innovation sequence (see above). However if a simulation is available
comparisons between estimated and nominal true states can be made. The ﬁlter
should be unbiased and so the average error should be zero.
Normalised Estimate Error Squared If we denote the error at epoch k between
true and estimated states as ˜ x(k) then the quantity E{˜ x(k)
T
P(kk)
−1
˜ x(k)} is
distributed according to chisquared distribution with degrees of freedom equal
to the state dimension.
There is some skill involved in choosing values of R and Q such that the above crite
ria are met, especially when the ﬁlter models are a poor representation of the truth.
The correct thing to do here is implement a better model. If however, other engi
neering issues impede this course of action, the ﬁlter must be detuned (increase noise
strengths) in the hope of ‘lumping’ unmodelled characteristics into the noise vector.
This of course means that the ﬁlter looses any claim to optimality.
10
There is a powerful geometric interpretation of the Kalman ﬁlter that ﬁts closely to this analogy using
ideas of orthogonality
51
Incorporating NonLinear Models  The Extended Kalman Filter 52
5.3 Incorporating NonLinear Models  The Extended
Kalman Filter
Up until now we have only considered linear models (although working in nonlinear simu
lated environments). It shouldn’t come as a surprise to you that the majority of real world
applications require the use of nonlinear models. Think about an everyday example  a re
ally simple GPS receiver sitting at x
r
= (x, y, z)
T
and measuring time of ﬂight (equivalent)
to a satellite sitting at x
s
= (x, y, z)
T
s
. Clearly the time of ﬂight measurement is “explained”
by the norm of the diﬀerence vector  x
r
− x
s
. This then requires the use of a nonlinear
measurement model. Fortunately we shall see that nonlinear models can easily be incorpo
rated into the Kalman Filter equations (yielding a ﬁlter called the Extended Kalman Filter
or EKF) you are already familiar with. The derivations are given here for completeness and
the results are stated in Section 5.3.3.
5.3.1 Nonlinear Prediction
We begin by writing a general form for a nonlinear plant truth model:
x(k) = f (x(k −1), u(k), k) +v(k) (5.62)
z(k) = h(x(k), u(k), k) +w(k). (5.63)
The trick behind the EKF is to linearize the nonlinear models around the “best” current
estimate (best meaning prediction (kk − 1) or last estimate (k − 1k − 1)). This is done
using a Taylor series expansion. Assume we have an estimate ˆ x(k −1k −1) then
x(k) = f (ˆ x(k −1k −1), u(k), k) +∇F
x
[x(k −1) − ˆ x(k −1k −1)] +· · · (5.64)
The term ∇F
x
is understood to be the jacobian of (f ) with respect to x evaluated at an
elsewhere speciﬁed point:
∇F
x
=
∂f
∂x
=
∂f
1
∂x
1
· · ·
∂f
1
∂x
m
.
.
.
.
.
.
∂f
n
∂x
1
· · ·
∂f
n
∂x
m
¸
¸
¸
52
Incorporating NonLinear Models  The Extended Kalman Filter 53
Now we know that ˆ x(kk −1) = E{x(k)Z
k−1
} and ˆ x(k −1k −1) = E{x(k −1)Z
k−1
}so
ˆ x(kk −1) = E{f (ˆ x(k −1k −1), u(k), k) +∇F
x
[x(k −1) − ˆ x(k −1k −1)] +· · · +v(k)Z
k−1
}
(5.65)
= E{f (ˆ x(k −1k −1), u(k), k)Z
k−1
} +∇F
x
[ˆ x(k −1k −1) − ˆ x(k −1k −1)]
(5.66)
= f (ˆ x(k −1k −1), u(k), k) (5.67)
Which is an obvious conclusion — simply pass the last estimate through the nonlinear model
to come up with a prediction based on a control signal u (k). To ﬁgure out how to propagate
the covariance (using only terms from the previous time step) we look at the behaviour of
˜ x(kk − 1) = x(k) − ˆ x(kk − 1) because P(kk − 1) = E{˜ x(kk − 1)˜ x(kk − 1)
T
Z
k−1
}.
Understanding that the jacobians of f are evaluated at ˆ x(k −1k −1) we can write
˜ x(kk −1) = x(k) − ˆ x(kk −1) (5.68)
≈ f (ˆ x(k −1k −1), u(k), k) +∇F
x
[x(k −1) − ˆ x(k −1k −1)] +v(k)− (5.69)
f (ˆ x(k −1k −1), u(k), k) (5.70)
= ∇F
x
[x(k −1) − ˆ x(k −1k −1)] +v(k) (5.71)
= ∇F
x
[˜ x(k −1k −1)] +v(k) (5.72)
(5.73)
So
P(kk −1) = E{˜ x(kk −1)˜ x(kk −1)
T
Z
k−1
} (5.74)
≈ E{(∇F
x
˜ x(k −1k −1) +v(k))(∇F
x
˜ x(k −1k −1) +v(k))
T
Z
k−1
} (5.75)
= E{∇F
x
˜ x(k −1k −1)˜ x(k −1k −1)
T
∇F
x
T
Z
k−1
} +E{v(k)v(k)
T
Z
k−1
}
(5.76)
= ∇F
x
P(k −1k −1)∇F
x
T
+Q (5.77)
We now have the predict equations in the case of nonlinear plant models. Note that fre
quently the model will be in the form
x(k) = f (x(k −1), u(k), v(k), k) (5.78)
where the noise v (k) is not simply additive. In this case one would proceed with a multivari
ate Taylor
11
series which swiftly becomes notationally complex and algebraically tiresome.
However the end result is intuitive. The state prediction remains unchanged but the predic
tion equation becomes:
P(kk −1) = ∇F
x
P(k −1k −1)∇F
x
T
+∇G
v
Q∇G
v
T
(5.79)
11
alternatively stack x and v in a new vector y and diﬀerentiate with respect to y  the same result follows.
53
Incorporating NonLinear Models  The Extended Kalman Filter 54
where ∇F
u
is the jacobian of f w.r.t the noise vector. (Don’t worry many examples to follow
— also look at the source code provided ) It is a common practice to make the substitution
u(k) = u
n
(k) +v(k) where u
n
(k) is a nominal control which is then corrupted by noise. In
this case ∇G
v
= ∇G
u
. You will see this again soon. In the meantime, look at the example
source code provided.
5.3.2 Nonlinear Observation Model
Now we turn to the case of a nonlinear observation model (for example a range and bearing
sensor) of the general form
z(k) = h(x(k)) +w(k) (5.80)
By using the same technique used for the nonlinear prediction step we can show that the
predicted observation z(kk −1) (Hˆ x(kk −1) in the linear case) is simply the projection of
ˆ x(kk −1) through the measurement model:
z(kk −1) = E{z(k)Z
k−1
} (5.81)
z(kk −1) = h(ˆ x(kk −1)). (5.82)
Now we wish to derive an expression for S, the innovation covariance. We begin by expressing
the observation and the estimated observation error ˜ z(kk −1) using a Taylor series:
z(k) ≈ h(ˆ x(kk −1)) +∇H
x
(ˆ x(kk −1) −x(k)) +· · · +w(k) (5.83)
˜ z(kk −1) = z(k) −z(kk −1) (5.84)
= h(ˆ x(kk −1)) +∇H
x
(ˆ x(kk −1) −x(k)) +· · · +w(k) −h(ˆ x(kk −1))
(5.85)
= ∇H
x
(ˆ x(kk −1) −x(k)) +w(k) (5.86)
= ∇H
x
(˜ x(kk −1)) +w(k) (5.87)
So the covariance of the diﬀerence between actual and predicted observations (the innovation)
can be written as:
S = E{˜ z(kk −1)˜ z(kk −1)
T
Z
k−1
} (5.88)
= E{(∇H
x
(˜ x(kk −1)) +w(k))(∇H
x
(˜ x(kk −1)) +w(k))
T
Z
k−1
} (5.89)
= ∇H
x
E{˜ x(kk −1)˜ x(kk −1)
T
Z
k−1
}∇H
x
T
+E{w(k)w(k)
T
Z
k−1
} (5.90)
= ∇H
x
P(kk −1)∇H
x
T
+R (5.91)
You may be wondering where the E{˜ x(kk −1)w(k)
T
Z
k−1
} terms have gone. They evaluate
to zero as (reasonably) we do not expect the noise in observations to be correlated to the
error in our prediction.
54
Incorporating NonLinear Models  The Extended Kalman Filter 55
We now have one last thing to ﬁgure out — how does a nonlinear observation model
change the update equations resulting in ˆ x(kk) and P(kk)? The procedure should now be
becoming familiar to you: ﬁgure out an expression using a series expansion for ˜ x(kk) and
take squared conditional expectation to evaluate P(kk).
Assume that “somehow” we have a gain W (we’ll derive this in a minute) then we can
immediately write:
ˆ x(kk) = ˆ x(kk −1)
. .. .
prediction
+ W
....
gain
( z(k)
....
observation
− h(ˆ x(kk −1))
. .. .
predictedobservation
. .. .
innovation
(5.92)
We can now use this expression to make progress in ﬁguring out P(kk).
P(kk) = E{˜ x(kk)˜ x(kk)
T
Z
k
} (5.93)
˜ x(kk) = ˆ x(kk) −x(k) (5.94)
= ˆ x(kk −1) +W(z(k) −h(ˆ x(kk −1))) −x(k) (5.95)
= ˆ x(kk −1) +W˜ z(kk −1) −x(k) (5.96)
and substituting equation 5.87
= ˆ x(kk −1) +W(∇H
x
(˜ x(kk −1)) +w(k)) −x(k) (5.97)
= ˜ x(kk −1) +W∇H
x
˜ x(kk −1) +Ww(k) (5.98)
= [I −W∇H
x
]˜ x(kk −1) +Ww(k) (5.99)
An expression for P(kk) follows swiftly as
P(kk) = E{˜ x(kk −1)˜ x(kk −1)
T
Z
k
} (5.100)
= E{([I −W∇H
x
]˜ x(kk −1) +Ww(k))([I −W∇H
x
]˜ x(kk −1) +Ww(k))
T
Z
k
}
(5.101)
= [I −W∇H
x
]E{˜ x(kk −1)˜ x(kk −1)
T
Z
k
}[I −W∇H
x
]
T
+WE{w(k)w(k)
T
Z
k
}W
T
(5.102)
= [I −W∇H
x
]P(kk −1)[I −W∇H
x
]
T
+WRW
T
(5.103)
Above, we have used the fact that the expectation of ˜ x is zero and so the crossterms of the
expansion evaluate to zero. We now just need to ﬁnd an expression for W . Here we derive
the gain using a little calculus. Recall that behind all of this is a quest to minimise the mean
square estimation error (which is scalar):
J(ˆ x) = E{˜ x(kk)
T
˜ x(kk)kZk} (5.104)
= Tr(P(kk)) (5.105)
55
Incorporating NonLinear Models  The Extended Kalman Filter 56
Fortunately there is a closed form to the diﬀerential of such a function. If B is symmetric
for any A then
∂Tr(ABA
T
)
∂A
= 2AB
∂AC
∂A
= C
T
(5.106)
Applying this (and the chain rule) to Equation 5.103 and setting the derivative equal to zero
we get:
∂J(ˆ x)
∂W
= −2[I −W∇H
x
]P(kk −1)∇H
x
T
+ 2WR = 0 (5.107)
W = P(kk −1)∇H
x
T
(∇H
x
P(kk −1)∇H
x
T
+R)
−1
= P(kk −1)∇H
x
T
S
−1
(5.108)
where
S = (∇H
x
P(kk −1)∇H
x
T
+R) (5.109)
Note that substituting P(kk −1)∇H
x
T
= WS into Equation 5.103 leads to the form of
the covariance update we are already familiar with:
P(kk) = P(kk −1) −WSW
T
(5.110)
5.3.3 The Extended Kalman Filter Equations
We can now state in a “good to remember this box” a rule for converting the linear Kalman
ﬁlter equations to the nonlinear form:
To convert the linear Kalman Filter to the Extended Kalman Filter simply replace
F with ∇F
x
and H with ∇H
x
in the covariance and gain calculations only.
The jacobians are always evaluated at the best available estimate (ˆ x(k −1k −1)
for ∇F
x
and ˆ x(kk −1) for ∇H
x
.
For completeness here are the EKF equations. (You’ll need these for the classwork). If
you don’t feel you are on top of the previous maths  its not the end of the world. Make
56
Incorporating NonLinear Models  The Extended Kalman Filter 57
sure however you are familiar with the general form of the equations. (exam useful things
are in boxes in these notes (but not exclusively))
Prediction:
ˆ x(kk −1)
. .. .
predicted state
=
plant model
. .. .
f (ˆ x(k −1k −1)
. .. .
old state est
, u(k)
....
control
, k) (5.111)
P(kk −1)
. .. .
predicted covariance
= ∇F
x
P(k −1k −1)
. .. .
old est covariance
∇F
x
T
+∇G
v
Q∇G
v
T
. .. .
process noise
(5.112)
z(kk −1)
. .. .
predicted obs
=
observation model
. .. .
h(ˆ x(kk −1)) (5.113)
Update:
ˆ x(kk)
. .. .
new state estimate
=
prediction and correction
. .. .
ˆ x(kk −1) +W ν(k)
....
innovation
(5.114)
P(kk)
. .. .
new covariance estimate
= P(kk −1) −WSW
T
. .. .
update decreases uncertainty
(5.115)
where
ν(k) =
measurement
....
z(k) −z(kk −1) (5.116)
W = P(kk −1)∇H
x
T
S
−1
. .. .
kalman gain
(5.117)
S = ∇H
x
P(kk −1)∇H
x
T
+R
. .. .
Innovation Covariance
(5.118)
∇F
x
=
∂f
∂x
=
∂f
1
∂x
1
· · ·
∂f
1
∂x
m
.
.
.
.
.
.
∂f
n
∂x
1
· · ·
∂f
n
∂x
m
¸
¸
¸
. .. .
evaluated at ˆ x(k −1k −1)
∇H
x
=
∂h
∂x
=
∂f
1
∂x
1
· · ·
∂h
1
∂x
m
.
.
.
.
.
.
∂h
n
∂x
1
· · ·
∂h
n
∂x
m
¸
¸
¸
. .. .
evaluated at ˆ x(kk −1)
(5.119)
57
Incorporating NonLinear Models  The Extended Kalman Filter 58
We are now in a great position, possessing some very powerful tools which we shall now
apply to some key autonomous navigation problems.
58
Topic 6
Vehicle Models and Odometry
6.1 Velocity Steer Model
This is a good point to write down a simple motion model for a mobile robotic vehicle. We
allow the vehicle to move on a 2D surface ( a ﬂoor) and point in arbitrary directions. We
parameterise the vehicle pose x
v
(the joint of position and orientation) as
x
v
=
x
v
y
v
θ
v
¸
¸
. (6.1)
Figure 6.1 is a diagram of a nonholonomic (local degrees of freedom less than global
degree of freedom) vehicle with “Ackerman” steering. The angle of the steering wheels is
given by φ and the instantaneous forward velocity (sometimes called throttle) is V . With
reference to Figure 6.1, we immediately see that
˙ x
v
= V δT cos(θ
v
) (6.2)
˙ y
v
= V δT sin(θ
v
) (6.3)
(6.4)
Using the instantaneous center of rotation we can calculate the rate of change of orien
59
Velocity Steer Model 60
(x,y)
Global Reference Frame
L
θ
φ
Instantaneous
Center of
Rotation
V
φ
a
Figure 6.1: A nonholonomic vehicle with Ackerman steering
tation as a function of steer angle:
L
a
= tan(φ) (6.5)
a
˙
θ
v
= V (6.6)
˙
θ
v
=
V
L
tan(φ) (6.7)
We can now discretise this model by inspection:
x
v
(k + 1) = f (x
v
(k), u(k)); u(k) =
¸
V (k)
φ(k)
(6.8)
x
v
(k + 1)
y
v
(k + 1)
θ
v
(k + 1)
¸
¸
=
x
v
(k + 1) + δTV (k) cos(θ
v
(k))
y
v
(k + 1) + δTV (k) sin(θ
v
(k))
θ
v
(k) +
δTV (k) tan(φ(k))
L
¸
¸
(6.9)
Note that we have started to lump the throttle and steer into a control vector — this makes
sense if you think about the controlling actions of a human driver. Equation 6.9 is a model for
a perfect, noiseless vehicle. Clearly this is a little unrealistic — we need to model uncertainty.
One popular way to do this is to insert noise terms into the control signal u such that
u(k) = u
n
(k) +v(k) (6.10)
where u
n
(k) is a nominal (intended) control signal and v(k) is a zero mean gaussian dis
60
Evolution of Uncertainty 61
tributed noise vector:
v(k) ∼ N(0,
¸
σ
2
V
0
0 σ
2
φ
) (6.11)
u(k) ∼ N(u
n
(k),
¸
σ
2
V
0
0 σ
2
φ
) (6.12)
This completes a simple probabilistic model of a vehicle. We shall now see how propa
gation of this model aﬀects uncertainty in vehicle pose over time.
6.2 Evolution of Uncertainty
In this section we will examine how an initial uncertainty in vehicle pose increases over time
as the vehicle moves when only the control signal u is available. Hopefully you will realise
that one way to approach this is repetitive application of the prediction step of a Kalman
ﬁlter discussed in Section 5.1.1. The model derived in the previous section is nonlinear and
so we will have to use the nonlinear form of the prediction step.
Assume at time k we have been given a previous best estimate of the vehicle pose ˆ x
v
(k −
1k −1) and an associated covariance P
v
(k −1k −1). Equations 5.67 and 5.79 have that:
x
v
(kk −1) = f (ˆ x(k −1k −1), u(k), k) (6.13)
P
v
(kk −1) = ∇F
x
P
v
(k −1k −1)∇F
x
T
+∇F
v
Q∇F
v
T
(6.14)
In this case
Q =
¸
σ
2
V
0
0 σ
2
φ
(6.15)
We need to evaluate the Jacobians with respect to state and control noise at ˆ x(k −1k −1).
We do this by diﬀerentiating each row of f by each state and each control respectively:
∇F
x
=
1 0 −δTV sin(θ
v
)
0 1 δTV cos(θ
v
)
0 0 1
¸
¸
(6.16)
∇F
u
=
δT cos(θ
v
) 0
δT sin(θ
v
) 0
tan(φ)
L
δTV sec
2
(φ)
L
¸
¸
(6.17)
Figure 6.2 shows the results of iterating equations 6.13 and 6.14 using the matlab code
printed in Section 11.2. Things are pretty much as we might expect. The uncertainty
61
Evolution of Uncertainty 62
−20 −15 −10 −5 0 5 10 15 20
−5
0
5
10
15
20
25
x
y
uncertainty bounds for Ackermann model
Figure 6.2: Evolution of uncertainty evolution of open loop the Ackermann Model. The
circles are the true location of the vehicle whereas the crosses mark the deadreckoned lo
cations. The orientation of the vehicle is made clear by the orientation of the triangles.
Note the divergence between true and deadreckoned locations. This is typical of all dead
reckoning methods — the only thing that can be changed is the rate of divergence.
injected into the system via the noisy control makes the estimated covariance of the vehicles
grow without bound.
There is an important point to make here that you must understand. In actual real life
the real robot is integrating the noisy control signal. The true trajectory will therefore always
drift away from the trajectory estimated by the algorithms running inside the robot. This is
exactly the same as closing your eyes and trying to walk across University Parks. Your inner
ears and legs give you u which you pass through your own kinematic model of your body
in your head. Of course, one would expect a gross accumulation of error as the time spent
walking “open loop” increases. The point is that all measurements such as velocity and
rate of turn are measured in the vehicle frame and must be integrated, along with the noise
on the measurements. This always leads to what is called “dead reckoning drift”. Figure
6.3 shows the eﬀect of integrating odometry on a real robot called “B21” shown in ﬁgure
6.3(right). The main cause of this divergence on land vehicles is wheel slip. Typically robot
wheels are ﬁtted with encoders that measure the rotation of each wheel. Position is then
an integralfunction of these “wheel counts”. The problem is a wheel or radius r may have
turned through θ but due to slip/skid the distance travelled over the ground is only (1−η)rθ
where η is an unobservable slip parameter.
62
Using DeadReckoned Odometry Measurements 63
6.3 Using DeadReckoned Odometry Measurements
The model in Section 6.1 used velocity and steer angles as control input into the model. It is
common to ﬁnd that this low level knowledge is not easy to obtain or that the relationship
between control, prior and prediction is not readily discernable. The architecture in ﬁgure
6.3 is a typical example.
DR Model
Encoder Counts
Hidden
Control
Nav
Physics
Other Measurements
Supplied Onboard
System
Navigation (to be installed by us)
Physics (to be estimated)
Dead Reckoned output
(drifts badly)
required (corrected)
navigation estimates
integration
Figure 6.3: Sometimes a navigation system will be given a dead reckoned position as input
without recourse to the control signals that were involved. Nevertheless the deadreckoned
position can be converted into a control input (a stream of small motions) for use in the core
navigation system.
It would clearly be a bad plan to simply use a deadreckoned odometry estimate as a
direct measurement of state in something like a Kalman Filter. Consider Figure 6.3 which
is the dead reckoned position of a B21 mobile robot (shown on right of ﬁgure) moving
around some corridors. Clearly by the end of the experiment we cannot reasonably interpret
deadreckoned position as an unbiased measurement of position!
The low level controller on the vehicle reads encoders on the vehicle’s wheels and outputs
an estimate (with no metric of uncertainty) of its location. We can make a guess at the
kind of model it uses
1
. Assume it has two wheels (left and right), radius r mounted either
side of its center of mass which in one time interval turn an amount δθ
l
, δθ
r
— as shown in
Figure 6.3. We align a bodycentered coordinate frame on the vehicle as shown. We want
1
This for illustration only  the real b21 vehicle is actually a synchronous drive machine in which all four
wheels change direction http://www.irobot.com
63
Using DeadReckoned Odometry Measurements 64
Figure 6.4: Dead Reckoned position from a B21 mobile robot. The start and end locations
are actually the same place! See how you could roll the trajectory back over itself. This is
typical of dead reckoned trajectories — small angular errors integrate to give massive long
term errors
to express the change of position of the center of the vehicle as a function of δθ
l
, δθ
r
:
rδθ
r
= (c −L/2)α (6.18)
rδθ
l
= (c + L/2)α (6.19)
⇒c =
L
2
δθ
l
+ δθ
r
δθ
l
−δθ
r
(6.20)
⇒α =
2r
L
(δθ
l
−δθ
r
) (6.21)
Immediately then we have
dx
dy
dθ
¸
¸
=
(1 −cos α)c
c sin α
−α
¸
¸
(6.22)
Which for small α becomes:
dx
dy
dθ
¸
¸
=
0
r(δθ
l
+ δθ
r
)/2
−2r(δθ
l
−δθ
r
)
L
¸
¸
(6.23)
The deadreckoning system in the vehicle simply compounds these small changes in position
64
Using DeadReckoned Odometry Measurements 65
y'
x'
L
r
y
x
[
d
x
,
d
y
]
α
c
dθ
Figure 6.5: Geometric Construction for a two wheel drive vehicle
and orientation to obtain a global position estimate. Starting from an initial nominal frame
at each iteration of its sensing loop it deduces a small change in position and orientation,
and then “adds” this to its last deadreckoned position. Of course the “addition” is slightly
more complex than simple adding (otherwise the x coordinate would always be zero!). What
actually happens is that the vehicle composes successive coordinate transformation. This
is an important concept (which you will probably have met in other courses but perhaps
with a diﬀerent notation) and will be discussed in the next section.
6.3.1 Composition of Transformations
Figure 6.3.1 shows three relationships between three coordinate frames. We can express any
coordinate j frame with respect to another frame i as a threevector x
i,j
= [xyθ]. Here x and
y are translations in frame i to a point p and θ is anticlockwise rotation around p. We deﬁne
two operators ⊕ and to allow us to compose (chain together) multiple transformations:
x
i,k
= x
i,j
⊕x
j,k
(6.24)
x
j,i
= x
i,j
(6.25)
65
Using DeadReckoned Odometry Measurements 66
2
3
1
X
1,2
X
2,3
X
1,3
Figure 6.6: Three transformations between coordinate frames
With reference to ﬁgure 6.3.1 we see that x
1,3
= x
1,2
⊕ x
2,3
. But what exactly are these
operators? Well, they are just a short hand for a function of one or two transformations:
x
1
⊕x
2
=
¸
x
1
+ x
2
cos θ
1
−y
2
sin θ
1
y
1
+ x
2
sin θ
1
+ y
2
cos θ
1
(6.26)
x
1
=
−x
1
cos θ
1
−y
1
sin θ
1
x
1
sin θ
1
−y
1
cos θ
1
−θ
1
¸
¸
(6.27)
Be sure you understand exactly what these equations allow. They allow you to express
something (perhaps a point or vehicle) described in one frame, in another alternative frame.
We can use this notation to explain the compounding of odometry measurements. Figure
6.3.1 shows a vehicle with a prior pose x
o
(k −1). The processing of wheel rotations between
successive readings (via Equation 6.23) has indicated a vehiclerelative transformation (i.e.
in the frame of the vehicle) u . The task of combining this new motion u(k) with the old
deadreckoned estimate x
o
to arrive at a new deadreckoned pose x
o
is trivial. It is simply:
x
o
(k) = x
o
(k −1) ⊕u(k) (6.28)
We have now explained a way in which measurements of wheel rotations can be used to esti
mate deadreckoned pose. However we set out to ﬁgure out a way in which a deadreckoned
pose could be used to form a control input or measurement into a navigation system. In
66
Using DeadReckoned Odometry Measurements 67
x
o
(k1)
u(k)
x
o
(k)
Global Frame
Figure 6.7: Using transformation compositions to compound a local odometry measurement
with a prior deadreckoned estimate to deduce a new deadreckoned estimate.
other words we are given from the lowlevel vehicle software a sequence x
o
(1), x
o
(2) · · · x
o
(k)
etc and we want to ﬁgure out u (k). This is now simple— we can invert equation 6.28 to get
u(k) = x
o
(k −1) ⊕x
o
(k) (6.29)
Looking at the Figure 6.3.1 we can see that the transformation u(k) is equivalent to going
back along x
o
(k − 1) and forward along x
o
(k). This gives us a small control vector u (k)
derived from two successive deadreckoned poses that is suitable for use in another hopefully
less error prone navigation algorithm. Eﬀectively equation 6.29 subtracts out the common
deadreckoned gross error  locally odometry is good  globally it is poor.
We are now in a position to write down a plant model for a vehicle using a deadreckoned
position as a control input:
x
v
(k + 1) = f (x
v
(k), u(k)) (6.30)
= x
v
(k) ⊕(x
o
(k −1) ⊕x
o
(k)
. .. .
dr−control
) (6.31)
= x
v
(k) ⊕u
o
(k) (6.32)
It is reasonable to ask how an initial uncertainty in vehicle pose P
v
propagates over time.
We know that one way to address this question is to propagate the second order statistics
(covariance) of a pdf for x
v
through f using equation 5.79. To do this we need to ﬁgure out the
jacobians of equation 6.32 with respect to x
v
and u. This is one area where the compositional
representation we have adopted simpliﬁes matters. We can deﬁne and calculate the following
jacobians:
67
Using DeadReckoned Odometry Measurements 68
J
1
(x
1
, x
2
)
∂(x
1
⊕x
2
)
∂x1
(6.33)
=
1 0 −x
2
sin θ
1
−y
2
cos θ
1
0 1 x
2
cos θ
1
−y
2
sin θ
1
0 0 1
¸
¸
(6.34)
J
2
(x
1
, x
2
)
∂(x
1
⊕x
2
)
∂x2
(6.35)
=
cos θ
1
−sin θ
1
0
sin θ
1
cos θ
1
0
0 0 1
¸
¸
(6.36)
This allows us to write (substituting into equation 5.79) :
P(kk −1) = ∇F
x
P
v
(k −1k −1)∇F
x
T
+∇F
v
Q∇F
v
T
(6.37)
= J
1
(x
v
, u
o
)P
v
(k −1k −1)J
1
(x
v
, u
o
)
T
+J
2
(x
v
, u
o
)U
o
J
2
(x
v
, u
o
)
T
(6.38)
Here the matrix U
o
describes the strength of noise in the small shifts in pose represented by
u
o
derived from two sequential deadreckoned poses. A simple form of this matrix would be
purely diagonal
2
:
U
o
=
σ
2
ox
0 0
0 σ
2
oy
0
0 0 σ
2
oθ
¸
¸
(6.39)
where the diagonals are variances in odometry noise. For example if the odometry loop ran
at 20Hz and the vehicle is moving at 1m/s the magnitude of translation in u would be 5cm.
If we say slip accounts for perhaps one percent of distance travelled we might “try” a value
of σ
2
ox
= σ
2
oy
= (0.05/100)
2
. Allowing a maximum rotation of w perhaps a good starting
guess for σ
2
oθ
would be (w/100)
2
. These numbers will give sensible answers while the vehicle
is moving but not when it is stopped. Even when u
o
= 0 the covariance P
v
will continue
to inﬂate. This motivates the use of a time varying U
o
which is a function of u
o
(k). An
exercise you should think about.....
2
implying we expect errors in x y and orientations to be uncorrelated which is probably not true in reality
but we will live with this approximation for now
68
Topic 7
Feature Based Mapping and
Localisation
7.1 Introduction
We will now apply the estimation techniques we have learnt to two very important mobile
robotics tasks  Mapping and Localisation. Theses two tasks are fundamental to successful
deployment of autonomous vehicles and systems for example:
Mapping Managing autonomous opencast
mining
GPS, Radar, Inertial
Battleﬁeld Surveillance Cameras, GPS, Inertial
Fracture detection xray acoustic
Subsea Oil ﬁeld detection on an
AUV
Acoustic Beacons, Inertial
Localisation GPS Satellite and time of ﬂight
Museum Guides Sonar, Scanning Laser, Cameras
Hospital Delivery System Radio tags, laser cameras
A common way to approach these problems is to parameterise both the robot pose and
aspects of the environment’s geometry into one or more state vectors. For this course we
69
Features and Maps 70
will work mostly in 2D but the deﬁnitions that follow are, of course, valid for the full 3D
case.
7.2 Features and Maps
We suppose that the world is populated by a set of discrete landmarks or features whose
location / orientation and geometry (with respect to a deﬁned coordinate frame) can be
described by a set of parameters which we lump into a feature vector x
f
.
We call a collection of n features a Map such that M = {x
f ,1
, x
f ,2
, x
f ,3
· · · x
f ,n
}. To keep
notation simple sometimes we will use M to denote the map vector which is simply the
concatenation of all the features:
M =
x
f ,1
x
f ,2
.
.
.
x
f ,n
¸
¸
¸
¸
¸
(7.1)
In this course we will constrain ourselves to using the simplest feature possible  a point
feature such that for the i
th
feature:
x
f ,i
=
¸
x
i
y
i
(7.2)
where x and y are the coordinates of the point in a global frame of reference. Point features
are not as uncommon as one might initially think. Points occur at the intersection of lines,
corners of rectangles, edges of objects (regions of maximal curvature)
1
.
7.3 Observations
We deﬁne two distinct types of observations all denoted as z — vehicle relative and absolute.
Absolute Absolute observations are made with the help of some external device and usually
involve a direct measurement of some aspect of the vehicle’s pose. The best examples
1
Sure, our own vision system operates at a higher level recognising things like gorillas and sailboats as
complete entities but lowerlevel geometricprimitive detection is buried in there as well.
70
A Probabilistic Framework 71
are a GPS and a compass. They depend on external infrastructure (taken as known)
and are nothing to do with the map.
Vehicle Relative This kind of observation involves sensing the relationship between the
vehicle and its immediate surroundings –especially the map, see ﬁgure 7.3. A great
example is the measurement of the angle and distance to a point feature with respect to
the robot’s own frame of reference. We shall also class odometry (integration of wheel
movement) as a vehiclerelative measurement because it is not a direct measurement
of the vehicle’s state.
x
v
x
fi
Features and Landmarks
Global Reference Frame
Mobile Vehicle
VehicleFeature Relative
Observation
Figure 7.1: Feature Based Navigation and Mapping
7.4 A Probabilistic Framework
It is informative to describe the localisation and mapping tasks in terms of likelihoods (ob
servation pdf) and priors.
7.4.1 Probabilistic Localisation
For the localisation task we assume we have been given a map and receive a sequence of
vehiclerelative observations described by a likelihood p(Z
k
M, x
v
). We wish to ﬁgure a pdf
71
A Probabilistic Framework 72
for the vehicle pose given map and observations:
p(x
v
M, Z
k
) =
p(Z
k
M, x
v
)p(M, x
v
)
p(M, Z
k
)
(7.3)
=
p(Z
k
M, x
v
)p(x
v
M)p(M)
p(M, Z
k
)
(7.4)
=
p(Z
k
M, x
v
)p(x
v
M)p(M)
∞
−∞
p(Z
k
M, x)p(x
v
M)p(M)dx
v
(7.5)
If we are given a nominally perfect map (i.e. told to take it as absolute truth) then p(M) = 1
this simpliﬁes the above:
p(x
v
M, Z
k
) =
p(Z
k
M, x
v
)p(x
v
M)p(M)
∞
−∞
p(Z
k
M, x)p(x
v
M)p(M)dx
v
(7.6)
=
p(Z
k
x
v
)p(x
v
)
∞
−∞
p(Z
k
x)p(x
v
)dx
v
(7.7)
=
p(Z
k
x
v
)p(x
v
)
C(Z
k
)
(7.8)
7.4.2 Probabilistic Mapping
For the mapping side of things we are given the vehicle location (probably derived from
absolute observation) and an sequence of vehicle relative observations. We wish to ﬁnd the
distribution of M conditioned on Z
k
and x
v
.
p(Mx
v
, Z
k
) =
p(Z
k
M, x
v
)p(M, x
v
)
p(x
v
, z)
(7.9)
=
p(Z
k
M, x
v
)p(Mx
v
)p(x
v
)
p(x
v
, z)
(7.10)
=
p(Z
k
M, x
v
)p(Mx
v
)p(x
v
)
∞
−∞
p(Z
k
M, x
v
)p(Mx
v
)p(x
v
)dM
(7.11)
(7.12)
If we assume perfect knowledge of the vehicle location then p(x
v
) = 1 and so
72
Feature Based Estimation for Mapping and Localising 73
p(Mx
v
, Z
k
) =
p(Z
k
M, x
v
)p(Mx
v
)p(x
v
)
∞
−∞
p(Z
k
M, x
v
)p(Mx
v
)p(x
v
)dM
(7.13)
=
p(Z
k
M)p(M)
∞
−∞
p(Z
k
M)p(M)dM
(7.14)
=
p(Z
k
M)p(M)
C(Z
k
)
(7.15)
Equations 7.15 and 7.8 should have a familiar form — we met them when discussing
Maximum a priori estimators, recursive bayesian estimation which collectively lead us to
discuss and explore the Kalman ﬁlter. The Kalman ﬁlter would appear to be an excellent
way in which to implement these equations. If we parameterise the random vectors x
v
and
M with ﬁrst and second order statistics (mean and variance) then the Kalman Filter will
calculate the MMSE estimate of the posterior. The ﬁrst derivation of the Kalman ﬁlter
presented proceeded by assuming Gaussian distributions. In this case the Kalman ﬁlter is
the optimal Bayesian estimator. The Kalman ﬁlter provides a real time way to perform state
estimation on board a vehicle.
7.5 Feature Based Estimation for Mapping and Local
ising
7.5.1 Feature Based Localisation
This is the simplest task. We are given a map M containing a set of features and a stream
of observations of measurements between the vehicle and these features (see ﬁgure 7.3). We
assume to begin with that an oracle is telling us the associations between measurements and
observed features. We assume the vehicle we are navigating is equipped with a rangebearing
sensor which returns the range and bearing to point like objects (the features). We will base
the ensuing simulation on a real vehicle  the B21 we have already met  this means that
we will have an additional sequence of deadreckoned positions as input into the prediction
stage. We denote these as x
o
We have already have the prediction equations from previous discussions ( Equation
7.16):
73
Feature Based Estimation for Mapping and Localising 74
ˆ x(kk −1) = ˆ x(k −1k −1) ⊕(x
o
(k −1) ⊕x
o
(k)) (7.16)
ˆ x(kk −1) = ˆ x(k −1k −1) ⊕u
o
(k) (7.17)
P(kk −1) = ∇F
x
P(k −1k −1)∇F
x
T
+∇F
v
Q∇F
v
T
(7.18)
= J
1
(x
v
, u
o
)P
v
(k −1k −1)J
1
(x
v
, u
o
)
T
+J
2
(x
v
, u
o
)U
o
J
2
(x
v
, u
o
)
T
(7.19)
Now we come to the observation equation which is simply a range r
i
and bearing θ
i
to
the i
th
feature:
z(k)
¸
r
θ
(7.20)
= h(x(k), w(k)) (7.21)
=
¸
(x
i
−x
v
(k))
2
+ (y
i
−y
v
(k))
2
atan2(
y
i
−y
v
(k)
x
i
−x
v
(k)
) −θ
v
¸
(7.22)
We diﬀerentiate w.r.t x
v
to arrive at the observation model jacobian:
∇H
x
∂h
∂x
(7.23)
=
¸
x
i
−x
v
(k)
r
y
i
−y
v
(k)
r
0
y
i
−y
v
(k)
r
2
−
x
i
−x
v
(k)
r
2
−1
¸
(7.24)
We assume independent errors on range and bearing measurements and use a diagonal
observation covariance matrix R :
R =
¸
σ
2
r
0
0 σ
2
θ
(7.25)
Section 11.3 is a print out of an implementation of feature based localisation using the
models we have just discussed. You can also download the code from the course website
¡http://www.robots.ox.ac.uk/∼pnewman : teaching¿. Figure 7.5.1 shows the trajectory of the
vehicle as it moves through a ﬁeld of random point features. Note that the code simulates
a sensor failure for the middle twenty percent of the mission. During this time the vehicle
becomes more and more lost. When the sensor comes back on line there is a jump in
estimated vehicle location back to one close to the true position (see ﬁgure 7.5.1).
7.5.2 Feature Based Mapping
Now we will consider the dual of Localisation  Mapping. In this case the vehicle knows
where it is but not what is in the environment. Perhaps the vehicle is ﬁtted with a GPS
74
Feature Based Estimation for Mapping and Localising 75
−80 −60 −40 −20 0 20 40 60 80
−60
−40
−20
0
20
40
60
Figure 7.2: Feature Based Localisation
0 1000 2000 3000 4000 5000 6000
−10
−5
0
5
10
Innovation
r
a
n
g
e
0 1000 2000 3000 4000 5000 6000
−30
−20
−10
0
10
20
30
B
e
a
r
in
g
(
d
e
g
)
time
0 1000 2000 3000 4000 5000 6000
−40
−20
0
20
40
Covariance and Error
x
0 1000 2000 3000 4000 5000 6000
−20
−10
0
10
20
y
0 1000 2000 3000 4000 5000 6000
−200
−100
0
100
200
θ
time
Figure 7.3: Feature Based Localisation innovation and error/covariance plots. Notice how
when no measurements are made the vehicle uncertainty grows rapidly but reduces when
features are reobserved. The covariance bounds are 3sigma and 1sigmabound on state
error and innovation plots respectively.
or some other localisation system using an apriori map. To avoid initial confusion we’ll
imagine the vehicle has a ’supergps’ on board telling it where it is at all times.
The state vector for this problem is now much larger it will be the Map itself and
75
Feature Based Estimation for Mapping and Localising 76
the concatenation of all point features. The observation equation is the same as for the
localisation case only now the feature coordinates are the free variables.
The prediction model for the state is trivial. We assume that features don’t move and
so x(k + 1k)
map
= x(kk)
map
.
vehicle trajectory
decreasing feature uncertainty
Figure 7.4: Evolution of a map over time. The vertical axis is time (k). Covariance ellipses
for each feature are plotted in the z = k planes. Note how the ﬁlter converges to a perfect
map because no process noise is added in the prediction step
Initially the map is empty and so we need some method to add “newly discovered’ features
to it. To this end we introduce a feature initialisation function Y that take as arguments
the old state vector and an observation to a landmark and returns a new, longer state vector
with the new feature at its end. For the case of a range bearing measurement we would have
the following:
x(kk)
∗
= y(x(kk), z(k), x
v
(kk)) (7.26)
=
¸
x(kk)
g(x(k), z(k), x
v
(kk))
(7.27)
=
x(kk)
x
v
+ r cos(θ + θ
v
)
y
v
+ r sin(θ + θ
v
)
¸
¸
(7.28)
(7.29)
76
Feature Based Estimation for Mapping and Localising 77
where the coordinates of the new feature are given by the function g:
x
fnew
= g(x(k), z(k), x
v
(kk)) (7.30)
= (
¸
x
v
+ r cos(θ + θ
v
)
y
v
+ r sin(θ + θ
v
)
(7.31)
We also need to ﬁgure out how to transform the covariance matrix P when adding a new
feature. Of course we can use the jacobian of the transformation:
P(kk)
∗
= ∇Y
x,z
¸
P(kk) 0
0 R
∇Y
x,z
T
(7.32)
where
∇Y
x,z
=
¸
I
n×n
0
n×2
∇G
x
∇G
z
(7.33)
Now for the mapping case ∇G
x
= 0 as the new feature is not dependent on any element
in the state vector (which only contains features). Therefore:
P(kk)
∗
=
¸
P(kk) 0
0 ∇G
z
R∇G
z
T
(7.34)
One thing to realise is that the observation Jacobian is now “long and thin”. When
observing feature i it is only nonzero at the “location” (indexes) of the feature in the state
vector:
∇H
x
=
¸
· · · 0· · ·
. .. .
otherfeatures
∇H
x
ﬁ
. .. .
observedfeature
· · · 0· · ·
. .. .
otherfeatures
(7.35)
Figure 7.5.2 shows the evolution of the map over time. Note that as no process noise is
ever added to the system the uncertainty in feature locations after initialisation is always
decreasing. In the limit the map will be known perfectly. The code used to generate this
simulation for feature based mapping can be downloaded from the course website. You
might think it odd that in the limit the map becomes perfectly known. You might think
that intuitively there should always be some residual uncertainty. The point is that the
77
Simultaneous Localisation and Mapping  SLAM 78
vehicle is continually being told where (via the superGPS) it is and is not changing its
position estimate as a function of landmark observations. As a consequence all features
are independent (check this by examining the P matrix –it is block diagonal) and each
observation of them simply adds information and therefore reduces their uncertainty  again
and again and again.... This is in contrast to the next section where landmark observations
will be allowed to adjust map and vehicle estimates simultaneously!
7.6 Simultaneous Localisation and Mapping  SLAM
SLAM is the generalised navigation problem. It asks if it is possible for a robot, starting
with no prior information, to move through its environment and build a consistent map of
the entire environment. Additionally the vehicle must be able to use the map to navigate
(localise) and hence plan and control its trajectory during the mapping process. The appli
cations of a robot capable of navigating , with no prior map, are diverse indeed. Domains
in which ’man in the loop’ systems are impractical or diﬃcult such as Martian exploration,
subsea surveys, and disaster zones are obvious candidates. Beyond these, the sheer increase
in autonomy that would result from reliable, robust navigation in large dynamic environ
ments is simply enormous. Autonomous navigation has been an active area of research for
many years.
In SLAM no use is made of prior maps or external infrastructure such as GPS. A SLAM
algorithm builds a consistent estimate of both environment and vehicle trajectory using only
noisy proprioceptive (e.g., inertial, odometric) and vehiclecentric (e.g., radar, camera and
laser) sensors. Importantly, even though the relative observations are of the local environ
ment, they are fused to create an estimate of the complete workspace.
The SLAM problem is of fundamental importance in the quest for autonomous mobile
machines. It binds aspects of machine learning, uncertainty management, perception, sensing
and control to enable a machine to discover and understand its surroundings with no prior
knowledge or external assistance.
This section will introduce a simple feature based approach to the SLAM problem. It
doesn’t work in real life for deployments in large areas because it involves running a Kalman
ﬁlter to estimate the entire map and vehicle state. The update of the covariance matrix
is therefore at best proportional to the square of the number of features. Given enough
features the system will grind to a halt. Figure 7.6 shows some of the kind of area that can
be mapped and localised in using this technique.
78
Simultaneous Localisation and Mapping  SLAM 79
Figure 7.5: SLAM in a real environment. These ﬁgures are screen shots from an exper
iment using a B21 robot in a hallway at MIT. Having built the map the vehicle used
it to navigate home to within a few cm of its start position. Videos can be found at
¡http://www.robots.ox.ac.uk/∼pnewman : teaching¿.
You’ll be pleased to know we have pretty much done all the work required to implement
a full SLAM algorithm. We will still employ the oracle that tells us which feature is being
seen with each observation. All we do now is change our state vector to include both vehicle
and map:
x
x
v
x
f ,1
.
.
.x
f ,n
¸
¸
¸
(7.36)
Of course to start with n = 0 but as new features are seen the state vector is grown
79
Simultaneous Localisation and Mapping  SLAM 80
just as in the pure mapping case. The diﬀerence now is that the observation and feature
initialisation jacobians have two nonzero blocks. one with respect to the vehicle and one
with respect to the observed feature.
∇H
x
=
∇H
x
v
· · · ∇H
x
f,1
(7.37)
∇Y
x,z
=
¸
I
n×n
0
n×2
∇G
x
∇G
z
(7.38)
=
¸
I
n×n
0
n×2
∇G
x
v
· · · 0· · ·
∇G
z
(7.39)
Also note that the jacobian of the prediction models have changed in an obvious way:
∇F
x
=
¸
∇F
x
v
0
0 I
2n×2n
(7.40)
=
¸
J1(x
v
, u) 0
0 I
2n×2n
(7.41)
∇F
u
=
¸
J2(x
v
, u) 0
0 0
2n×2n
(7.42)
(7.43)
because the prediction model is now:
x
v
(k + 1)
x
f ,1
(k)
.
.
.
x
f ,n
(k)
¸
¸
¸
¸
¸
=
x
v
(k) ⊕u(k)
x
f ,1
(k)
.
.
.
x
f ,n
(k)
¸
¸
¸
¸
¸
(7.44)
Note that the features are not expected to move between time steps (pretty much what you
would hope for when using a map!) and they are noiseless. Only the vehicle has process
noise injected into its covariance matrix during a prediction step.
The matlab code for an EKF, feature based SLAM algorithm can be found on the course
website. You should be able to recognise it as a union of previous localisation and mapping
examples. Figure 7.6 shows a few snap shots of the algorithm running.
80
Simultaneous Localisation and Mapping  SLAM 81
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
−100 −50 0 50 100
−100
−80
−60
−40
−20
0
20
40
60
80
100
7.6.1 The role of Correlations
Note that the covariance matrix now has some structure to it you can partition map P
mm
and vehicle P
vv
blocks.
P =
¸
P
vv
P
vm
P
T
vm
P
mm
(7.45)
Of diagonals P
vm
are the correlations between map and vehicle. Its pretty clear that there
should be correlations between map and vehicle as they are so interrelated. Consider the
following sequence of events. From the moment of initialisation the feature’s location is a
function of vehicle location and so errors in the vehicle location will also appear as errors
in feature location. Now recall the discussion regarding correlations in section 5.1.3 —
correlations cause adjustments in one state to ripple into adjustments in other states. This
is of course also true in this kind of approach to the SLAM problem. Remarkably every
observation of a feature aﬀects the estimate of every other feature in the map. It’s as though
they are all tied up together with elastic bands  pulling at one will pull at the others in
turn.
81
Simultaneous Localisation and Mapping  SLAM 82
There are some further characteristics of the SLAM problem that transcend the estima
tion method being used. You should be able to check that they are true by running the
example code:
• The feature uncertainties never drop below the initial uncertainty of the vehicle. This
makes sense, if you start with say 10m of uncertainty relative to Carfax tower mapping
and reobserving things within the Thom building is never going to reduce your overall
uncertainty. The best you can hope for is to reduce all features to the lower bound 
your initial uncertainty. Compare this to the ever decreasing uncertainty of features
in the mapping only case.
• The feature uncertainties never increase but the vehicle uncertainty can. The prediction
model is the identity matrix for the map  we don’t expect it to move. Furthermore the
map has a noiseless model and so the prediction step does not inﬂate the covariance
of the map.
The SLAM problem is a very topical problem and is attracting interest from the AI,
robotics and machine learning communities. If you want to ﬁnd out more just ask....
82
Topic 8
Multimodal and other Methods
The previous discussion on Estimation, Mapping and Localisation has focused almost exclu
sively on estimating unimodel pdfs. Much use has been made of the Kalman ﬁlter which
estimates the mean and covariance of a pdf which under gaussian noise assumptions is the
MMSE estimator. The Kalman ﬁlter is an important tool (and has great value outside of
this course) but it is not the whole story.
The single EKF approach we have talked about so far does not really behave well in
large environments  the big problem is that linearisation errors (that came from the Taylor
series approximations in deriving the EKF) compound and the linearisation starts to occur
around points that are far from the true state. There is neither the space nor time to cover in
detail how this can be overcome. Broadly though, one of two schemes are adopted. The ﬁrst
partitions the world into local maps and uses unimodal methods (EKF) within each map.
The maps are then carefully glued back together to form a uniﬁed global map. Secondly
unimodal methods are abandoned altogether. Instead, montecarlo methods are used.
8.1 Montecarlo Methods  Particle Filters
The basic idea is simple. We maintain lots of diﬀerent versions of the state vector — all
slightly diﬀerent from each other. When a measurement comes in we score how well each
version explains the data. We then make copies of the best ﬁtting vectors and randomly
perturb them (the equivalent of adding Q in the EKF) to form a new generation of candidate
states. Collectively these thousands of possible states and their scores deﬁne the pdf we are
seeking to estimate. We never have to make the assumption of Gaussian noise or perform
83
Montecarlo Methods  Particle Filters 84
a linearisation. The big problem with this elegant approach is the number of samplestates
(versions) we need to try out increase geometrically with the number of states being esti
mated. For example to estimate a 1D vector we may keep 100 samples. For a 2D vector
we would require something like 100
2
, for 3D 100
3
and so on. The trick often played is to
be smart about selecting which samples should be copied between timesteps. This is a very
current area of research.
The PFLocalisation algorithm proceeds as follows:
1. Initialise n particles in a map. Each particle is a 3 by 1 state vector of the vehicle
2. Apply the plant model to each particle. In contrast to the prediction step in the
Kalman approach we actually inject the process noise as well — i.e we add a random
vector to the control vector u. This is in some ways analogous to adding GUG
T
to
the predicted covariance in the Kalman approach because it increases the variance of
the estimate (in this case the particle set).
3. For each particle predict the observation. Compare this to the measured value. Use a
likelihood function to evaluate the likelihood of the observation given the state repre
sented by each particle. This will be a scalar L which is associated with each particle.
This scalar is referred to as a “weight” or “importance”.
4. Select the particles that best explain the observation. One way to do this would be to
simply sort and select the top candidates based on L but this would reduce the number
of particles. One solution to this would be to copy from this “winning set” until we
have n particles again. This has a hidden consequence though: it would artiﬁcially
reduce the diversity (spread) of the particle set. Instead we randomly sample from the
samples biased by the importance values. This means that there is a ﬁnite chance that
particles that really didn’t explain the observation well, will be reselected. This may
turn out to be a good plan because they may do a better job for the next observation.
5. Goto 2
Figure 8.1 shows this process graphically for the localisation problem. A common ques
tion is “what is the estimate?”. Well technically it is the distribution of particles themselves
which represents a pdf. If you want a single vector estimate, the mode, mean and median are
all viable options. However you need to think carefully. If you run the code yourself initially
you will see distinct clouds of particles representing diﬀering modes of the pdf  the mean
is somewhere between them all in a location with no support
1
! As with all the algorithms
1
however soon the clouds should coverge into one large cloud as the motion of the vehicle disambiguates
its location
84
Grid Based Mapping 85
in the course you are strongly advised to down load the code (MCL.m) and peruse it to
understand the translation from concept to implementation. In summary, the particle ﬁlter
approach is very elegant and very simple to implement (see the code on the website). A
crucial point is that it does not require any linearisation assumptions (there are no jacobians
involved) and there are no Gaussian assumptions. It is particularly well suited for problems
with small state spaces  the example code has a state dimension of three.
8.2 Grid Based Mapping
Up until now we have adopted a feature based approach to navigation. We assumed that the
environment contains features possessing some geometry that can be extracted by suitable
processing of sensor data  edges, points, lines etc. We then store and estimate (using further
measurements) this sparse parameterisation (often by stacking all the feature parameters in
to a long state vector and calling it x ).
In contrast grid based approaches tend not to reason about the geometry of features in
the environment but take a more sensorcentric view. A mesh of cells (a grid) is laid over the
(initially unknown) world. Each cell c(i, j) has a number associated with it corresponding
to an occupancy probability  p
o
(i, j) and a probability of being empty p
e
. So for example
if p
o
(40, 2) = 1 we are sure that the cell at (40,20) is occupied. We make no assertions as to
what is occupying it just that something or part of something is at the coordinates covered
by cell(40,20).
We use two functions to model the behaviour of the sensor  one to express the likelihood
of a cells within the observed region being empty and one to express the likelihood of them
being occupied. Figure 8.2 shows one possible set of functions for a widebeam inair sonar.
For each cell under the beam a Bayes rule update is used to ﬁgure out the new probability
of a cell being empty or being occupied. As the sensor (mounted on a vehicle) moves around
the environment and if the location of the sensor is known a grid based map can be built.
Figure 8.2 shows the occupancy and empty maps for a map built with the CMU Terragator
vehicle in ﬁgure 8.2.
One problem with grid based methods is memory usage  it becomes expensive for 3D
maps. However cunning data structures can overcome this. A second issue is that of grid
alignment  it is arbitrary and as such cells are likely to cover areas that are awkwardly
partially full as such is not possible to capture the sharp discontinuities at the edges of
objects. Perhaps the biggest problem of grid based methods stems from the diﬃculties in
localising with respect to a grid. It requires trying to match a small sensor centric map with
85
Grid Based Mapping 86
3
2
1
4
5
3
7
2
1
4
5
3
7
6
6
2
1
4
5
3
7
6
Feature
Predicted Observations
p(zx
i
)
zh(x
i
) obspred
L
i
= f(zh(x
i
))
h(x
i
)
1 2 3 4 5
6
7
1 1 3 1 1 0 0
particle
#copies
2
1
4
5
3
3
Actual Observation
7 4 1 2 3 6 5
x
i
= f(x
i
,u(k),v(k))
1. Plant Model
2. Obs Prediction
3. Fitness Evaluation
4. Selection
5. Regeneration
updated sample set
starting sample set
motion +
perturbation
x3
uniformly
choosen
random
numbers
cumulative importance ( ΣL
i
)
Figure 8.1: Graphical interpretation of a particle ﬁlter. The method is general but this
diagram can be easily understood in terms of a rangetobeacon sensor (like the long baseline
acoustic sensor mentioned in the Least Squares section). The best particles are those that
best explain the actual range measurement. These are preferentially choosen to form the
next generation.
86
Grid Based Mapping 87
Figure 8.2: From the early days of mobile robotics. The CMU terragator vehicle used grid
based mapping in various settings.
Figure 8.3: The shape of two functions used to describe the probability P
e
(r) of empty cells
at a range r from the sensor and the probability of them being occupied p
o
(r). The actual
object is at distance R from the sensor. The sensor modelled here is a wide beam sonar that
sends out a cone of sound and measures the time between transmit and echo reception.
87
Grid Based Mapping 88
Figure 8.4: The two typical occupancy maps built by a grid based approach. The left map
show the probability of regions being occupied and the left the probability of regions being
empty. Each map is maintained using a diﬀerent likelihood function
a large global map. The vehicle can be localised by rotating and translating the local map
(what is seen by the sensor) until the best match is found.
88
Topic 9
In Conclusion
Hopefully you have found this introductory course interesting and have picked up a sense
that the tools we have been using are applicable to many domains. If this were a 10 lecture
course we would have spent more time considering how to appraise in realtime the validity
of sensor data. This is a crucial point — we need to decide whether the data is even worth
processing. In may situations sensors return grossly erroneous data (outliers). However if
we don’t know what the world looks like how can we be sure it is bad data and not faithful
sensing of an unexpected world state? I also hope you have found the discussions on mobile
robotics thought provoking. Perhaps in a decade we will have cars that avoid crashes, goods
trucks that drive themselves, an alwaysdeployed ﬂeet of ocean vehicles monitoring ElNino
currents, smart machines on Mars and robots that massively increase the independence and
mobility of our old and inﬁrm. We don’t need the “fullmonty” of AI to be able to make a
big diﬀerence....(but it would be useful).
P.M.N October 2003.
89
Topic 10
Miscellaneous Matters
This section is meant to help you with the class work and covers things that you may need
to know but that don’t really ﬁt in the main body of the notes
10.1 Drawing Covariance Ellipses
The ndimensional multivariatenormal distribution has form
p(x) =
1
(2π)
n/2
 P 
1/2
exp{−
1
2
(x −µ
x
)
T
P
−1
(x −µ
x
)} (10.1)
The exponent (x − µ
x
)
T
P
−1
(x − µ
x
) deﬁnes contours on the bell curve. A particularly
useful contour is the “1 −σ bound” where (x −µ
x
)
T
P
−1
(x −µ
x
) = 1. It is really useful to
be able to plot this contour to illustrate uncertainties in 2D. We can disregard the vector µ
x
as it simply shifts the distribution. So we have
x
T
P
−1
x = 1 (10.2)
we can use eigenvalue decomposition to factorise P as VDV
T
where V is some rotation
matrix and D is a diagonal matrix of principal values
x
T
(VDV
T
)
−1
x = 1 (10.3)
90
Drawing Covariance Ellipses 91
X
p(x,y)
Y
Figure 10.1: A bivariate normal distribution. The thick contour is the 1 −σ bound where
(x −µ
x
)
T
P
−1
(x −µ
x
) = 1
.
using the fact that V is orthonormal:
x
T
VD
−1
V
T
x = 1 (10.4)
x
T
VD
−1/2
D
−1/2
V
T
x = 1 (10.5)
x
T
KK
T
x = 1 (10.6)
where
K = VD
−1/2
. (10.7)
now for any point y = [x, y]
T
which is on the unit circle, y
T
y = 1, so
x
T
KK
T
x = y
T
y (10.8)
K
T
x = y (10.9)
⇒x = VD
1/2
y (10.10)
So to draw an ellipse described by a 2 × 2 covariance matrix P we take a whole bunch of
points on the unit circle and multiply them by VD
1/2
and plot the resulting points. This
makes sense, intuitively the variance is in “units squared”— D
1/2
is a diagonal matrix of
standard deviations  the semimajor and semiminor axes of the ellipse. The ﬁst thing we
do is scale the unit circle by these factors. Then we rotate the ellipse by V — recall that
the correlations between x and y (oﬀ diagonals in P ) induce a rotation in the ellipse?
91
Drawing Covariance Ellipses 92
2σ
xx
2σ
yy
α
Figure 10.2: The relationship between geometry of the 1 − σ bound for a bivariate normal
distribution and its covariance matrix.
Figure 10.1 shows the relation ship between P and the plotted form for a bivariate normal
distribution.
P =
¸
σ
2
xx
σ
2
xy
σ
2
xy
σ
2
yy
(10.11)
tan 2α =
2σ
2
xy
σ
2
xx
−σ
2
yy
(10.12)
92
Drawing High Dimensional Gaussians 93
10.2 Drawing High Dimensional Gaussians
Obviously it is hard to draw a high dimensional gaussian on a screen or paper. For example
we may have a covariance matrix that corresponds to a large state vector:
x =
a
b
c
¸
¸
(10.13)
P =
¸
σ
a
11
2
σ
a
12
2
σ
a
21
2
σ
a
22
2
P
aa
· · · · · ·
.
.
.
marginal of component b
. .. .
¸
σ
b
11
2
σ
b
12
2
σ
b
21
2
σ
b
22
2
P
bb
· · ·
.
.
.
.
.
.
¸
σ
c
11
2
σ
c
12
2
σ
c
21
2
σ
c
22
2
P
cc
¸
¸
¸
¸
¸
¸
¸
¸
¸
¸
(10.14)
So to plot a 2D representation of the i
th
entity in a state vector simply plot the ellipse for
the i
th
2 by 2 block in P .
93
Topic 11
Example Code
11.1 Matlab Code For Mars Lander Example
%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
% A MARS LANDER −−− EXAMPLE CODE C4
% P. Newman 2003
%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
%−−−−−−−−− TOP LEVEL LOOP −−−−−−−−−−−−−−−−−−−−−%
function SimulateMarsLander
close all ; clear all ;
global Params;global XTrue;global VehicleStatus;global Store;
%set up parameters
DoInitialise ;
%initial conditions of estimator
XEst = [Params.X0+Params.InitialHeightError;Params.V0+Params.InitialVelocityError];
PEst = diag([Params.InitialHeightStdˆ2, Params.InitialVelocityStdˆ2]);
%store initial conditions:
DoStore(1,XEst,PEst ,[0],[0], NaN);
k = 2;
while(˜VehicleStatus.Landed & k <Params.StopTime/Params.dT)
% simulate the world
DoWorldSimulation(k);
94
Matlab Code For Mars Lander Example 95
% read from sensor
z(k) = GetSensorData(k);
% estimate the state of the vehicle
[ XEst,PEst,S,Innovation] = DoEstimation(XEst,PEst,z(k));
% make decisions based on our esimated state
DoControl(k,XEst);
% store results
DoStore(k,XEst,PEst,Innovation,S,z(k));
%tick ...
k = k+1;
end;
%draw pictures....
DoMarsGraphics(k);
return;
%−−−−−−−−− PROBLEM SET UP AND INITIALISATION −−−−−−−−−−−−−−−−−−−−−%
% users changes parameters here
function DoInitialise
global Params;
global XTrue;
global VehicleStatus;
global Store;
%−−−−−− user conﬁgurable parameters −−−−−−−−−−−−−
Params.StopTime = 600;%run for how many seconds (maximum)?
Params.dT = 0.1; % Run navigation at 10Hz
Params.c light = 2.998e8;
Params.EntryDrag = 5; % linear drag constant
Params.ChuteDrag = 2.5∗Params.EntryDrag; % linear drag constant with chute open
Params.g = 9.8/3; % assumed gravity on mars
Params.m = 50; % mass of vehcile
Params.RocketBurnHeight = 1000; % when to turn on brakes
Params.OpenChuteHeight = 4000; %when to open chute
Params.X0 = 10000; % true entry height
Params.V0 = 0; % true entry velocity
Params.InitialHeightError = 0; % error on entry height
Params.InitialVelocityError = 0; % error on entry velocity
Params.InitialHeightStd = 100; %uncertainty in initial conditions
Params.InitialVelocityStd = 20; %uncertainty in initial conditions
Params.BurnTime = NaN;
Params.ChuteTime = NaN;
Params.LandingTime = NaN;
%initial vehicle condition at entry into atmosphere...
VehicleStatus.ChuteOpen = 0;
VehicleStatus.RocketsOn = 0;
VehicleStatus.Drag = Params.EntryDrag;
VehicleStatus.Thrust = 0;
95
Matlab Code For Mars Lander Example 96
VehicleStatus.Landed = 0;
%process plant model (constant velcoity with noise in acceleration)
Params.F = [1 Params.dT;
0 1];
%process noise model (maps acceleration noise to other states )
Params.G = [Params.dTˆ2/2 ;Params.dT];
%actual process noise truely occuring − atmospher entry is a bumpy business
%note this noise strength − not the deacceleration of the vehicle ....
Params.SigmaQ = 0.2; %msˆ{−2}
%process noise strength how much acceleration (varinace) in one tick
% we expect (used to ’explain’ inaccuracies in our model)
%the 3 is scale factor ( set it to 1 and real and modelled noises will
%be equal
Params.Q = (1.1∗Params.SigmaQ)ˆ2; %(msˆ2 std)
%observation model (explains observations in terms of state to be estimated)
Params.H = [2/Params.c light 0];
%observation noise strength (RTrue) is how noisey the sensor really is
Params.SigmaR = 1.3e−7; %(seconds) 3.0e−7 corresponds to around 50m error....
%observation expected noise strength (we never know this parameter exactly)
%set the scale factor to 1 to make model and reallity match
Params.R = (1.1∗Params.SigmaR)ˆ2;
%initial conditions of (true) world:
XTrue(:,1) = [Params.X0;Params.V0];
Params
return;
%−−−−−−−−−−−−−−−−−− MEASUREMENT SYSTEM −−−−−−−−−−−−−−−−−−%
function z = GetSensorData(k)
global XTrue;
global Params;
z = Params.H∗XTrue(:,k) + Params.SigmaR∗ randn(1);
return;
%−−−−−−−−−−−−−−− ESTIMATION KALMAN FILTER −−−−−−−−−−−−−−−%
function [XEst,PEst,S,Innovation] = DoEstimation(XEst,PEst,z)
global Params;
F = Params.F;G = Params.G;Q = Params.Q;R = Params.R;H = Params.H;
%prediction...
XPred = F∗XEst;
PPred = F∗PEst∗F’+G∗Q∗G’;
% prepare for update...
Innovation = z−H∗XPred;
S = H∗PPred∗H’+R;
W = PPred∗H’∗inv(S);
96
Matlab Code For Mars Lander Example 97
% do update....
XEst = XPred+W∗Innovation;
PEst = PPred−W∗S∗W’;
return;
%−−−−−−−−−−−−−−− ITERATE SIMULATION −−−−−=−−−−−−−−−−−−−−−%
function DoWorldSimulation(k)
global XTrue;global Params;global VehicleStatus;
oldvel = XTrue(2,k−1);
oldpos = XTrue(1,k−1);
dT = Params.dT;
%friction is a function of height
cxtau = 500; % spatial exponential factor for atmosphere density)
AtmospherDensityScaleFactor = (1−exp(−(Params.X0−oldpos)/cxtau) );
c = AtmospherDensityScaleFactor∗VehicleStatus.Drag;
%clamp between 0 and c for numerical safety
c = min(max(c,0),VehicleStatus.Drag);
%simple Euler integration
acc = (−c∗oldvel− Params.m∗Params.g+VehicleStatus.Thrust)/Params.m + Params.SigmaQ∗randn(1);
newvel = oldvel+acc∗dT;
newpos = oldpos+oldvel∗dT+0.5∗acc∗dTˆ2;
XTrue(:,k) = [newpos;newvel];
%−−−−−−−−−−−−−−− LANDER CONTROL −−−−−−−−−−−−−−−−−−−−−−−−−%
function DoControl(k,XEst)
global Params;global VehicleStatus;
if (XEst(1)<Params.OpenChuteHeight & ˜VehicleStatus.ChuteOpen)
%open parachute:
VehicleStatus.ChuteOpen = 1;
VehicleStatus.Drag = Params.ChuteDrag;
fprintf(’ Opening Chute at time %f\n’,k∗Params.dT);
Params.ChuteTime = k∗Params.dT;
end;
if (XEst(1)<Params.RocketBurnHeight )
if (˜VehicleStatus.RocketsOn)
fprintf(’ Releasing Chute at time %f\n’,k∗Params.dT);
fprintf(’ Firing Rockets at time %f\n’,k∗Params.dT);
Params.BurnTime = k∗Params.dT;
end;
%turn on thrusters
VehicleStatus.RocketsOn = 1;
97
Matlab Code For Ackerman Model Example 98
%drop chute..
VehicleStatus.Drag = 0;
%simple littel controller here (from vˆ2 = uˆ2+2as) and +mg for weight of vehicle
VehicleStatus.Thrust = (Params.m∗XEst(2)ˆ2−1)/(2∗XEst(1))+0.99∗Params.m∗Params.g;
end;
if (XEst(1)<1)
%stop when we hit the ground...
fprintf(’ Landed at time %f\n’,k∗Params.dT);
VehicleStatus.Landed = 1;
Params.LandingTime = k∗Params.dT;
break;
end;
return;
%−−−−−−−−−−−−−−− MANAGE RESULTS STORAGE −−−−−−−−−−−−−−−−−−−−−−%
function DoStore(k,XEst,PEst,Innovation,S,z)
global Store;
if (k==1)
Store.XEst = XEst;
Store.PEst = diag(PEst);
Store.Innovation = Innovation;
Store.S = S;
Store.z = z;
else
Store.XEst = [Store.XEst XEst];
Store.PEst = [Store.PEst diag(PEst)];
Store.Innovation = [ Store.Innovation Innovation];
Store.S = [Store.S diag(S)];
Store.z = [Store.z z ];
end;
return;
11.2 Matlab Code For Ackerman Model Example
function AckermannPredict
clear all ;
close all ;
dT = 0.1;%time steps size
nSteps = 600;%length of run
L = 2;%length of vehicle
SigmaV = 0.1; %3cm/s std on speed
SigmaPhi = 4∗pi/180; % steer inaccuracy
%initial knowledge pdf (prior @ k = 0)
P = diag ([0.2,0.2,0]); x = [0;0;0]; xtrue = x;
Q = diag([SigmaVˆ2 SigmaPhiˆ2]);
98
Matlab Code For Ackerman Model Example 99
%−−−−−−−− Set up graphics −−−−−−−−−−−%
ﬁgure(1);hold on;axis equal;grid on;axis([−20 20 −5 25])
xlabel(’x’ ); ylabel(’y’ );
title( ’ uncertainty bounds for Ackermann model’);
%−−−−−−−− Main loop −−−−−−−−−−−%
for(k = 1:nSteps)
%control is a wiggle at constant velocity
u = [1;pi/5∗sin(4∗pi∗k/nSteps)];
%calculate jacobians
JacFx = [1 0 −dT∗u(1)∗sin(x(3)); 0 1 dT∗u(1)∗cos(x(3)); 0 0 1];
JacFu = [dT∗cos(x(3)) 0; dT∗sin(x(3)) 0; dT∗ tan(u(2))/L dT ∗u(1)∗sec(u(2))ˆ2];
%prediction steps
P = JacFx ∗ P ∗JacFx’ + JacFu∗Q∗JacFu’;
xtrue = AckermannModel(xtrue,u+[SigmaV ;SigmaPhi].∗randn(2,1),dT,L);
x = AckermannModel(x,u,dT,L);
%draw occasionally
if (mod(k−1,30)==0)
PlotEllipse(x,P,0.5); DrawRobot(x,’r’);plot(xtrue(1),xtrue(2), ’ ko’ );
end;
end;
print −deps ’AckermannPredict.eps’%save the picture
%−−−−−−−−−−−− MODEL −−−−−−−−−−−−−−%
function y = AckermannModel(x,u,dT,L)
y(1,1) = x(1) + dT∗u(1)∗cos(x(3));
y(2,1) = x(2) + dT∗u(1)∗sin(x(3));
y(3,1) = x(3) + dT∗u(1)/L∗tan(u(2));
%−−−−−−−− Drawing Covariance −−−−−%
function eH = PlotEllipse(x,P,nSigma)
P = P(1:2,1:2); % only plot x−y part
x = x(1:2);
if (˜any(diag(P)==0))
[ V,D] = eig(P);
y = nSigma∗[cos(0:0.1:2∗pi);sin(0:0.1:2∗pi )];
el = V∗sqrtm(D)∗y;
el = [ el el (:,1)]+repmat(x,1,size(el,2)+1);
eH = line(el (1,:), el (2,:));
end;
%−−−−−−−− Drawing Vehicle −−−−−%
function DrawRobot(Xr,col);
p=0.02; % percentage of axes size
a=axis;
l1=(a(2)−a(1))∗p;
l2=(a(4)−a(3))∗p;
99
Matlab Code For EKF Localisation Example 100
P=[−1 1 0 −1; −1 −1 3 −1];%basic triangle
theta = Xr(3)−pi/2;%rotate to point along x axis (theta = 0)
c=cos(theta);
s=sin(theta);
P=[c −s; s c]∗P; %rotate by theta
P(1,:)=P(1,:)∗l1+Xr(1); %scale and shift to x
P(2,:)=P(2,:)∗l2+Xr(2);
H = plot(P(1,:),P(2,:), col , ’ LineWidth’,0.1);% draw
plot(Xr(1),Xr(2),sprintf(’%s+’,col));
11.3 Matlab Code For EKF Localisation Example
function EKFLocalisation
close all ; clear all ;
global xTrue;global Map;global RTrue;global UTrue;global nSteps;
nSteps = 6000;
Map = 140∗rand(2,30)−70;
UTrue = diag([0.01,0.01,1∗pi/180]).ˆ2;
RTrue = diag([2.0,3∗pi/180]).ˆ2;
UEst = 1.0∗UTrue;
REst = 1.0∗RTrue;
xTrue = [1;−40;−pi/2];
xOdomLast = GetOdometry(1);
%initial conditions:
xEst =xTrue;
PEst = diag([1,1,(1∗pi/180)ˆ2]);
%%%%%%%%% storage %%%%%%%%
InnovStore = NaN∗zeros(2,nSteps);
SStore = NaN∗zeros(2,nSteps);
PStore = NaN∗zeros(3,nSteps);
XStore = NaN∗zeros(3,nSteps);
XErrStore = NaN∗zeros(3,nSteps);
%initial graphics
ﬁgure(1); hold on; grid oﬀ; axis equal;
plot(Map(1,:),Map(2,:),’g∗’ ); hold on;
set(gcf, ’ doublebuﬀer’ , ’ on’ );
hObsLine = line ([0,0],[0,0]);
set(hObsLine,’ linestyle ’ , ’ :’ );
for k = 2:nSteps
%do world iteration
SimulateWorld(k);
%ﬁgure out control
xOdomNow = GetOdometry(k);
100
Matlab Code For EKF Localisation Example 101
u = tcomp(tinv(xOdomLast),xOdomNow);
xOdomLast = xOdomNow;
%do prediction
xPred = tcomp(xEst,u);
xPred(3) = AngleWrap(xPred(3));
PPred = J1(xEst,u)∗ PEst ∗J1(xEst,u)’ + J2(xEst,u)∗ UEst ∗ J2(xEst,u)’;
%observe a randomn feature
[ z, iFeature] = GetObservation(k);
if (˜isempty(z))
%predict observation
zPred = DoObservationModel(xPred,iFeature,Map);
% get observation Jacobian
jH = GetObsJac(xPred,iFeature,Map);
%do Kalman update:
Innov = z−zPred;
Innov(2) = AngleWrap(Innov(2));
S = jH∗PPred∗jH’+REst;
W = PPred∗jH’∗inv(S);
xEst = xPred+ W∗Innov;
xEst(3) = AngleWrap(xEst(3));
%note use of ’Joseph’ form which is numerically stable
I = eye(3);
PEst = (I−W∗jH)∗PPred∗(I−W∗jH)’+ W∗REst∗W’;
PEst = 0.5∗(PEst+PEst’);
else
%Ther was no observation available
xEst = xPred;
PEst = PPred;
Innov = [NaN;NaN];
S = NaN∗eye(2);
end;
if (mod(k−2,300)==0)
DoVehicleGraphics(xEst,PEst (1:2,1:2),8,[0,1]);
if (˜isempty(z))
set(hObsLine,’XData’,[xEst(1),Map(1,iFeature)]);
set(hObsLine,’YData’,[xEst(2),Map(2,iFeature)]);
end;
drawnow;
end;
%store results :
InnovStore(:,k) = Innov;
PStore(:,k) = sqrt(diag(PEst));
SStore (:, k) = sqrt(diag(S));
XStore(:,k) = xEst;
XErrStore(:,k) = xTrue−xEst;
101
Matlab Code For EKF Localisation Example 102
end;
DoGraphs(InnovStore,PStore,SStore,XStore,XErrStore);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function DoGraphs(InnovStore,PStore,SStore,XStore,XErrStore)
ﬁgure(1); print −depsc ’EKFLocation.eps’
ﬁgure(2);
subplot(2,1,1);plot(InnovStore(1,:)); hold on;plot(SStore(1,:), ’ r’ ); plot(−SStore(1,:), ’ r’ )
title( ’ Innovation’ ); ylabel(’range’ );
subplot(2,1,2);plot(InnovStore(2,:)∗180/pi);hold on;plot(SStore(2,:)∗180/pi,’r’ ); plot(−SStore(2,:)∗180/pi,’r’)
ylabel(’Bearing (deg)’);xlabel(’time’ );
print −depsc ’EKFLocationInnov.eps’
ﬁgure(2);
subplot(3,1,1);plot(XErrStore(1,:));hold on;plot(3∗PStore(1,:),’r’ ); plot(−3∗PStore(1,:),’r’ );
title( ’ Covariance and Error’);ylabel(’x’);
subplot(3,1,2);plot(XErrStore(2,:));hold on;plot(3∗PStore(2,:),’r’ ); plot(−3∗PStore(2,:),’r’ )
ylabel(’y’ );
subplot(3,1,3);plot(XErrStore(3,:)∗180/pi);hold on;plot(3∗PStore(3,:)∗180/pi,’r’);plot(−3∗PStore(3,:)∗180/pi,’r’)
ylabel(’\theta’ ); xlabel(’time’ );
print −depsc ’EKFLocationErr.eps’
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z,iFeature] = GetObservation(k)
global Map;global xTrue;global RTrue;global nSteps;
%fake sensor failure here
if (abs(k−nSteps/2)<0.1∗nSteps)
z = [];
iFeature = −1;
else
iFeature = ceil(size(Map,2)∗rand(1));
z = DoObservationModel(xTrue, iFeature,Map)+sqrt(RTrue)∗randn(2,1);
z(2) = AngleWrap(z(2));
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z] = DoObservationModel(xVeh, iFeature,Map)
Delta = Map(1:2,iFeature)−xVeh(1:2);
z = [norm(Delta);
atan2(Delta(2),Delta(1))−xVeh(3)];
z(2) = AngleWrap(z(2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function SimulateWorld(k)
global xTrue;
u = GetRobotControl(k);
xTrue = tcomp(xTrue,u);
xTrue(3) = AngleWrap(xTrue(3));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
102
Matlab Code For EKF Mapping Example 103
function jH = GetObsJac(xPred, iFeature,Map)
jH = zeros(2,3);
Delta = (Map(1:2,iFeature)−xPred(1:2));
r = norm(Delta);
jH(1,1) = −Delta(1) / r;
jH(1,2) = −Delta(2) / r;
jH(2,1) = Delta(2) / (rˆ2);
jH(2,2) = −Delta(1) / (rˆ2);
jH(2,3) = −1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xnow] = GetOdometry(k)
persistent LastOdom; %internal to robot low−level controller
global UTrue;
if (isempty(LastOdom))
global xTrue;
LastOdom = xTrue;
end;
u = GetRobotControl(k);
xnow =tcomp(LastOdom,u);
uNoise = sqrt(UTrue)∗randn(3,1);
xnow = tcomp(xnow,uNoise);
LastOdom = xnow;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function u = GetRobotControl(k)
global nSteps;
u = [0; 0.025 ; 0.1∗ pi/180∗sin(3∗pi∗k/nSteps)];
%u = [0; 0.15 ; 0.3∗ pi /180];
11.4 Matlab Code For EKF Mapping Example
function EKFLocalisation
close all ; clear all ;
global xVehicleTrue;global Map;global RTrue;global UTrue;global nSteps;
nSteps = 600;
nFeatures = 6;
MapSize = 200;
Map = MapSize∗rand(2,nFeatures)−MapSize/2;
UTrue = diag([0.01,0.01,1∗pi/180]).ˆ2;
RTrue = diag([8.0,7∗pi/180]).ˆ2;
UEst = 1.0∗UTrue;
REst = 1.0∗RTrue;
xVehicleTrue = [1;−40;−pi/2];
%initial conditions − no map:
xEst =[];
PEst = [];
MappedFeatures = NaN∗zeros(nFeatures,2);
103
Matlab Code For EKF Mapping Example 104
%storage:
PStore = NaN∗zeros(nFeatures,nSteps);
XErrStore = NaN∗zeros(nFeatures,nSteps);
%initial graphics − plot true map
ﬁgure(1); hold on; grid oﬀ; axis equal;
plot(Map(1,:),Map(2,:),’g∗’ ); hold on;
set(gcf, ’ doublebuﬀer’ , ’ on’ );
hObsLine = line ([0,0],[0,0]);
set(hObsLine,’ linestyle ’ , ’ :’ );
for k = 2:nSteps
%do world iteration
SimulateWorld(k);
%simple prediction model:
xPred = xEst;
PPred = PEst;
%observe a randomn feature
[ z, iFeature] = GetObservation(k);
if (˜isempty(z))
%have we seen this feature before?
if ( ˜isnan(MappedFeatures(iFeature,1)))
%predict observation: ﬁnd out where it is in state vector
FeatureIndex = MappedFeatures(iFeature,1);
xFeature = xPred(FeatureIndex:FeatureIndex+1);
zPred = DoObservationModel(xVehicleTrue,xFeature);
% get observation Jacobians
[ jHxv,jHxf] = GetObsJacs(xVehicleTrue,xFeature);
% ﬁll in state jacobian
jH = zeros(2,length(xEst));
jH(:, FeatureIndex:FeatureIndex+1) = jHxf;
%do Kalman update:
Innov = z−zPred;
Innov(2) = AngleWrap(Innov(2));
S = jH∗PPred∗jH’+REst;
W = PPred∗jH’∗inv(S);
xEst = xPred+ W∗Innov;
PEst = PPred−W∗S∗W’;
%note use of ’Joseph’ form which is numerically stable
% I = eye(size(PEst));
104
Matlab Code For EKF Mapping Example 105
% PEst = (I−W∗jH)∗PPred∗(I−W∗jH)’+ W∗REst∗W’;
%ensure P remains symmetric
PEst = 0.5∗(PEst+PEst’);
else
% this is a new feature add it to the map....
nStates = length(xEst);
xFeature = xVehicleTrue(1:2)+ [z(1)∗cos(z(2)+xVehicleTrue(3));z(1)∗sin(z(2)+xVehicleTrue(3))];
xEst = [xEst;xFeature];
[ jGxv, jGz] = GetNewFeatureJacs(xVehicleTrue,z);
M = [eye(nStates), zeros(nStates,2);% note we don’t use jacobian w.r.t vehicle
zeros(2,nStates) , jGz];
PEst = M∗blkdiag(PEst,REst)∗M’;
%remember this feature as being mapped we store its ID and position in the state vector
MappedFeatures(iFeature,:) = [length(xEst)−1, length(xEst)];
end;
else
%There was no observation available
end;
if (mod(k−2,40)==0)
plot(xVehicleTrue(1),xVehicleTrue(2),’r∗’ );
%now draw all the estimated feature points
DoMapGraphics(xEst,PEst,5);
fprintf(’ k = %d\n’,k);
drawnow;
end;
%Storage:
for(i = 1:nFeatures)
if (˜isnan(MappedFeatures(i,1)))
iL =MappedFeatures(i,1);
PStore(k,i) = det(PEst(iL:iL+1,iL:iL+1));
XErrStore(k,i) = norm(xEst(iL:iL+1)−Map(:,i));
end;
end;
end;
ﬁgure(2);
plot(PStore);
105
Matlab Code For EKF Mapping Example 106
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z,iFeature] = GetObservation(k)
global Map;global xVehicleTrue;global RTrue;global nSteps;
%choose a random feature to see from True Map
iFeature = ceil(size(Map,2)∗rand(1));
z = DoObservationModel(xVehicleTrue,Map(:,iFeature))+sqrt(RTrue)∗randn(2,1);
z(2) = AngleWrap(z(2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z] = DoObservationModel(xVeh, xFeature)
Delta = xFeature−xVeh(1:2);
z = [norm(Delta);
atan2(Delta(2),Delta(1))−xVeh(3)];
z(2) = AngleWrap(z(2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function SimulateWorld(k)
global xVehicleTrue;
u = GetRobotControl(k);
xVehicleTrue = tcomp(xVehicleTrue,u);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [jHxv,jHxf] = GetObsJacs(xPred, xFeature)
jHxv = zeros(2,3);jHxf = zeros(2,2);
Delta = (xFeature−xPred(1:2));
r = norm(Delta);
jHxv(1,1) = −Delta(1) / r;
jHxv(1,2) = −Delta(2) / r;
jHxv(2,1) = Delta(2) / (rˆ2);
jHxv(2,2) = −Delta(1) / (rˆ2);
jHxv(2,3) = −1;
jHxf(1:2,1:2) = −jHxv(1:2,1:2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [jGx,jGz] = GetNewFeatureJacs(Xv, z);
x = Xv(1,1);
y = Xv(2,1);
theta = Xv(3,1);
r = z(1);
bearing = z(2);
jGx = [ 1 0 −r∗sin(theta + bearing);
0 1 r∗cos(theta + bearing)];
jGz = [ cos(theta + bearing) −r∗sin(theta + bearing);
sin(theta + bearing) r∗cos(theta + bearing)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function u = GetRobotControl(k)
global nSteps;
%u = [0; 0.25 ; 0.3∗ pi/180∗sin(3∗pi∗k/nSteps)];
u = [0; 0.15 ; 0.3∗ pi/180];
106
Matlab Code For EKF SLAM Example 107
11.5 Matlab Code For EKF SLAM Example
function EKFSLAM
close all ; clear all ;
global xVehicleTrue;global Map;global RTrue;global UTrue;global nSteps;
global SensorSettings;
%change these to alter sensor behaviour
SensorSettings.FieldOfView = 45;
SensorSettings.Range = 100;
%how often shall we draw?
DrawEveryNFrames = 50;
%length of experiment
nSteps = 8000;
%when to take pictures?
SnapShots = ceil(linspace(2,nSteps,25));
%size of problem
nFeatures = 40;
MapSize = 200;
Map = MapSize∗rand(2,nFeatures)−MapSize/2;
UTrue = diag([0.01,0.01,1.5∗pi/180]).ˆ2;
RTrue = diag([1.1,5∗pi/180]).ˆ2;
UEst = 2.0∗UTrue;
REst = 2.0∗RTrue;
xVehicleTrue = [0;0;−pi/2];
%initial conditions − no map:
xEst =[xVehicleTrue];
PEst = diag([1,1,0.01]);
MappedFeatures = NaN∗zeros(nFeatures,2);
%storage:
PStore = NaN∗zeros(nFeatures,nSteps);
XErrStore = NaN∗zeros(nFeatures,nSteps);
%initial graphics − plot true map
ﬁgure(1); hold on; grid oﬀ; axis equal;
plot(Map(1,:),Map(2,:),’g∗’ ); hold on;
set(gcf, ’ doublebuﬀer’ , ’ on’ );
hObsLine = line ([0,0],[0,0]);
set(hObsLine,’ linestyle ’ , ’ :’ );
a = axis; axis(a∗1.1);
xOdomLast = GetOdometry(1);
107
Matlab Code For EKF SLAM Example 108
for k = 2:nSteps
%do world iteration
SimulateWorld(k);
%ﬁgure out control
xOdomNow = GetOdometry(k);
u = tcomp(tinv(xOdomLast),xOdomNow);
xOdomLast = xOdomNow;
%we’ll need this lots ...
xVehicle = xEst(1:3);
xMap = xEst(4:end);
%do prediction (the following is simply the result of multiplying
%out block form of jacobians)
xVehiclePred = tcomp(xVehicle,u);
PPredvv = J1(xVehicle,u)∗ PEst(1:3,1:3) ∗J1(xVehicle,u)’ + J2(xVehicle,u)∗ UEst ∗ J2(xVehicle,u)’;
PPredvm = J1(xVehicle,u)∗PEst(1:3,4:end);
PPredmm = PEst(4:end,4:end);
xPred = [xVehiclePred;xMap];
PPred = [PPredvv PPredvm;
PPredvm’ PPredmm];
%observe a randomn feature
[ z, iFeature] = GetObservation(k);
if (˜isempty(z))
%have we seen this feature before?
if ( ˜isnan(MappedFeatures(iFeature,1)))
%predict observation: ﬁnd out where it is in state vector
FeatureIndex = MappedFeatures(iFeature,1);
xFeature = xPred(FeatureIndex:FeatureIndex+1);
zPred = DoObservationModel(xVehicle,xFeature);
% get observation Jacobians
[ jHxv,jHxf] = GetObsJacs(xVehicle,xFeature);
% ﬁll in state jacobian
jH = zeros(2,length(xEst));
jH(:, FeatureIndex:FeatureIndex+1) = jHxf;
jH(:,1:3) = jHxv;
%do Kalman update:
Innov = z−zPred;
Innov(2) = AngleWrap(Innov(2));
S = jH∗PPred∗jH’+REst;
W = PPred∗jH’∗inv(S);
xEst = xPred+ W∗Innov;
108
Matlab Code For EKF SLAM Example 109
PEst = PPred−W∗S∗W’;
%ensure P remains symmetric
PEst = 0.5∗(PEst+PEst’);
else
% this is a new feature add it to the map....
nStates = length(xEst);
xFeature = xVehicle(1:2)+ [z(1)∗cos(z(2)+xVehicle(3));z(1)∗sin(z(2)+xVehicle(3))];
xEst = [xEst;xFeature]; %augmenting state vector
[ jGxv, jGz] = GetNewFeatureJacs(xVehicle,z);
M = [eye(nStates), zeros(nStates,2);% note we don’t use jacobian w.r.t vehicle
jGxv zeros(2,nStates−3) , jGz];
PEst = M∗blkdiag(PEst,REst)∗M’;
%remember this feature as being mapped we store its ID and position in the state vector
MappedFeatures(iFeature,:) = [length(xEst)−1, length(xEst)];
end;
else
xEst = xPred;
PESt = PPred;
end;
if (mod(k−2,DrawEveryNFrames)==0)
a = axis;
clf ;
axis(a);hold on;
n = length(xEst);
nF = (n−3)/2;
DoVehicleGraphics(xEst(1:3),PEst (1:3,1:3),3,[0 1]);
if (˜isnan(z))
h = line([xEst(1),xFeature (1)],[ xEst(2),xFeature(2)]);
set(h, ’ linestyle ’ , ’ :’ );
end;
for(i = 1:nF)
iF = 3+2∗i−1;
plot(xEst(iF),xEst(iF+1),’b∗’);
PlotEllipse(xEst(iF:iF+1),PEst(iF:iF+1,iF:iF+1),3);
end;
fprintf(’ k = %d\n’,k);
drawnow;
end;
if (ismember(k,SnapShots))
iPic = ﬁnd(SnapShots==k);
print(gcf,’−depsc’,sprintf(’EKFSLAM%d.eps’,iPic));
end;
end;
109
Matlab Code For EKF SLAM Example 110
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z,iFeature] = GetObservation(k)
global Map;global xVehicleTrue;global RTrue;global nSteps;global SensorSettings
done = 0;
Trys = 1;
z =[]; iFeature = −1;
while(˜done & Trys <0.5∗size(Map,2))
%choose a random feature to see from True Map
iFeature = ceil(size(Map,2)∗rand(1));
z = DoObservationModel(xVehicleTrue,Map(:,iFeature))+sqrt(RTrue)∗randn(2,1);
z(2) = AngleWrap(z(2));
%look forward...and only up to 40m
if (abs(pi/2−z(2))<SensorSettings.FieldOfView∗pi/180 & z(1) < SensorSettings.Range)
done =1 ;
else
Trys =Trys+1;
z =[]; iFeature = −1;
end;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z] = DoObservationModel(xVeh, xFeature)
Delta = xFeature−xVeh(1:2);
z = [norm(Delta);
atan2(Delta(2),Delta(1))−xVeh(3)];
z(2) = AngleWrap(z(2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function SimulateWorld(k)
global xVehicleTrue;
u = GetRobotControl(k);
xVehicleTrue = tcomp(xVehicleTrue,u);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [jHxv,jHxf] = GetObsJacs(xPred, xFeature)
jHxv = zeros(2,3);jHxf = zeros(2,2);
Delta = (xFeature−xPred(1:2));
r = norm(Delta);
jHxv(1,1) = −Delta(1) / r;
jHxv(1,2) = −Delta(2) / r;
jHxv(2,1) = Delta(2) / (rˆ2);
jHxv(2,2) = −Delta(1) / (rˆ2);
jHxv(2,3) = −1;
jHxf(1:2,1:2) = −jHxv(1:2,1:2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [jGx,jGz] = GetNewFeatureJacs(Xv, z);
x = Xv(1,1);
y = Xv(2,1);
theta = Xv(3,1);
r = z(1);
bearing = z(2);
jGx = [ 1 0 −r∗sin(theta + bearing);
110
Matlab Code For Particle Filter Example 111
0 1 r∗cos(theta + bearing)];
jGz = [ cos(theta + bearing) −r∗sin(theta + bearing);
sin(theta + bearing) r∗cos(theta + bearing)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xnow] = GetOdometry(k)
persistent LastOdom; %internal to robot low−level controller
global UTrue;
if (isempty(LastOdom))
global xVehicleTrue;
LastOdom = xVehicleTrue;
end;
u = GetRobotControl(k);
xnow =tcomp(LastOdom,u);
uNoise = sqrt(UTrue)∗randn(3,1);
xnow = tcomp(xnow,uNoise);
LastOdom = xnow;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function u = GetRobotControl(k)
global nSteps;
%u = [0; 0.25 ; 0.3∗ pi/180∗sin(3∗pi∗k/nSteps)];
u = [0; 0.15 ; 0.2∗ pi/180];
%−−−−−−−− Drawing Covariance −−−−−%
function eH = PlotEllipse(x,P,nSigma)
eH = [];
P = P(1:2,1:2); % only plot x−y part
x = x(1:2);
if (˜any(diag(P)==0))
[ V,D] = eig(P);
y = nSigma∗[cos(0:0.1:2∗pi);sin(0:0.1:2∗pi )];
el = V∗sqrtm(D)∗y;
el = [ el el (:,1)]+repmat(x,1,size(el,2)+1);
eH = line(el (1,:), el (2,:) );
end;
11.6 Matlab Code For Particle Filter Example
%Monte−carlo based localisation
%note this is not coded eﬃciently but rather to make the ideas clear
%all loops should be vectorized but that gets a little matlab−speak intensive
%and may obliterate the elegance of a particle ﬁlter ....
function MCL
close all ; clear all ;
global xTrue;global Map;global RTrue;global UTrue;global nSteps;
nSteps = 6000;
%change this to see how sensitive we are to the number of particle
%(hypotheses run) especially in relation to initial distribution !
111
Matlab Code For Particle Filter Example 112
nParticles = 400;
Map = 140∗rand(2,30)−70;
UTrue = diag([0.01,0.01,1∗pi/180]).ˆ2;
RTrue = diag([2.0,3∗pi/180]).ˆ2;
UEst = 1.0∗UTrue;
REst = 1.0∗RTrue;
xTrue = [1;−40;−pi/2];
xOdomLast = GetOdometry(1);
%initial conditions: − a point cloud around truth
xP =repmat(xTrue,1,nParticles)+diag([8,8,0.4])∗randn(3,nParticles);
%%%%%%%%% storage %%%%%%%%
%initial graphics
ﬁgure(1); hold on; grid oﬀ; axis equal;
plot(Map(1,:),Map(2,:),’g∗’ ); hold on;
set(gcf, ’ doublebuﬀer’ , ’ on’ );
hObsLine = line ([0,0],[0,0]);
set(hObsLine,’ linestyle ’ , ’ :’ );
hPoints = plot(xP(1,:),xP(2,:), ’ .’ );
for k = 2:nSteps
%do world iteration
SimulateWorld(k);
%all particles are equally important
L = ones(nParticles,1)/nParticles ;
%ﬁgure out control
xOdomNow = GetOdometry(k);
u = tcomp(tinv(xOdomLast),xOdomNow);
xOdomLast = xOdomNow;
%do prediction
%for each particle we add in control vector AND noise
%the control noise adds diversity within the generation
for(p = 1:nParticles)
xP(:,p) = tcomp(xP(:,p),u+sqrt(UEst)∗randn(3,1));
end;
xP(3,:) = AngleWrap(xP(3,:));
%observe a randomn feature
[ z, iFeature] = GetObservation(k);
if (˜isempty(z))
112
Matlab Code For Particle Filter Example 113
%predict observation
for(p = 1:nParticles)
%what do we expect observation to be for this particle ?
zPred = DoObservationModel(xP(:,p),iFeature,Map);
%how diﬀerent
Innov = z−zPred;
%get likelihood (new importance). Assume gaussian here but any pdf works!
%if predicted obs is very diﬀerent from actual obs this score will be low
%−>this particle is not very good at representing state . A lower score means
%it is less likely to be selected for the next generation ...
L(p) = exp(−0.5∗Innov’∗inv(REst)∗Innov)+0.001;
end;
end;
%reselect based on weights:
%particles with big weights will occupy a greater percentage of the
%y axis in a cummulative plot
CDF = cumsum(L)/sum(L);
%so randomly (uniform) choosing y values is more likely to correspond to
%more likely (better) particles ...
iSelect = rand(nParticles,1);
%ﬁnd the particle that corresponds to each y value ( just a look up)
iNextGeneration = interp1(CDF,1:nParticles,iSelect,’nearest’,’extrap’ );
%copy selected particles for next generation..
xP = xP(:,iNextGeneration);
%our estimate is simply the mean of teh particles
xEst = mean(xP,2);
if (mod(k−2,10)==0)
ﬁgure(1);
set(hPoints, ’ XData’,xP(1,:));
set(hPoints, ’ YData’,xP(2,:));
if (˜isempty(z))
set(hObsLine,’XData’,[xEst(1),Map(1,iFeature)]);
set(hObsLine,’YData’,[xEst(2),Map(2,iFeature)]);
end;
ﬁgure(2);plot(xP(1,:), xP(2,:), ’ .’ );
drawnow;
end;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z,iFeature] = GetObservation(k)
global Map;global xTrue;global RTrue;global nSteps;
%fake sensor failure here
if (abs(k−nSteps/2)<0.1∗nSteps)
113
Matlab Code For Particle Filter Example 114
z = [];
iFeature = −1;
else
iFeature = ceil(size(Map,2)∗rand(1));
z = DoObservationModel(xTrue, iFeature,Map)+sqrt(RTrue)∗randn(2,1);
z(2) = AngleWrap(z(2));
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z] = DoObservationModel(xVeh, iFeature,Map)
Delta = Map(1:2,iFeature)−xVeh(1:2);
z = [norm(Delta);
atan2(Delta(2),Delta(1))−xVeh(3)];
z(2) = AngleWrap(z(2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function SimulateWorld(k)
global xTrue;
u = GetRobotControl(k);
xTrue = tcomp(xTrue,u);
xTrue(3) = AngleWrap(xTrue(3));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xnow] = GetOdometry(k)
persistent LastOdom; %internal to robot low−level controller
global UTrue;
if (isempty(LastOdom))
global xTrue;
LastOdom = xTrue;
end;
u = GetRobotControl(k);
xnow =tcomp(LastOdom,u);
uNoise = sqrt(UTrue)∗randn(3,1);
xnow = tcomp(xnow,uNoise);
LastOdom = xnow;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function u = GetRobotControl(k)
global nSteps;
u = [0; 0.025 ; 0.1∗ pi/180∗sin(3∗pi∗k/nSteps)];
114
Contents
1 Introduction and Motivation 2 Introduction to Path Planning and Obstacle Avoidance 2.1 2.2 2.3 2.4 2.5 2.6 Holonomicity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Conﬁguration Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6 8 9 11 13 14 15 15 19 19 19 20 21 22
The MinkowskiSum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Voronoi Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bug Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Potential Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3 Estimation  A Quick Revision 3.1 3.2 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . What is Estimation? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.1 3.3 3.4 Deﬁning the problem . . . . . . . . . . . . . . . . . . . . . . . . . . .
Maximum Likelihood Estimation . . . . . . . . . . . . . . . . . . . . . . . . Maximum APosteriori  Estimation . . . . . . . . . . . . . . . . . . . . . . . 2
3 3.5 3.6 Minimum Mean Squared Error Estimation . . . . . . . . . . . . . . . . . . . Recursive Bayesian Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . 24 25 28 28 29 29 30 30 31 35 35 39 42 43 46 46 48 52 52 54
4 Least Squares Estimation 4.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.1 4.1.2 4.2 A Geometric Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . LSQ Via Minimisation . . . . . . . . . . . . . . . . . . . . . . . . . .
Weighted Least Squares . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.1 4.2.2 Nonlinear Least Squares . . . . . . . . . . . . . . . . . . . . . . . . . Long Baseline Navigation  an Example . . . . . . . . . . . . . . . . .
5 Kalman Filtering Theory, Motivation and Application 5.1 The Linear Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1.1 5.1.2 5.1.3 5.2 Incorporating Plant Models  Prediction . . . . . . . . . . . . . . . . Joining Prediction to Updates . . . . . . . . . . . . . . . . . . . . . . Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Using Estimation Theory in Mobile Robotics . . . . . . . . . . . . . . . . . . 5.2.1 5.2.2 A Linear Navigation Problem  “Mars Lander” . . . . . . . . . . . . . Simulation Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3
Incorporating NonLinear Models  The Extended Kalman Filter . . . . . . . 5.3.1 5.3.2 Nonlinear Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . Nonlinear Observation Model . . . . . . . . . . . . . . . . . . . . . . 3
. Observations . . . . . . . . . . . . . . . . . .5 Probabilistic Localisation . . . . Feature Based Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Feature Based Estimation for Mapping and Localising . . . . . . . .4 5. . . . . 7. . . . . . . . . . . .3. . . . . .2 7. . . . . . . . . . . . . Probabilistic Mapping . . .2 6. . 4 . . . . . . . . . . . . . . . . . . . .1 The role of Correlations .4 Introduction . . . . . . . . . . . . . . . 7. . . . . . . . . . . . . . . . . . . . . . .1 Composition of Transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .3 7.5. . . . . . . . . . .1 7. . . . . . . Evolution of Uncertainty . . . .1 7. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.6. . . . .4. . . . .3 Velocity Steer Model . . . 7. . 8 Multimodal and other Methods 8.SLAM . . . . . . Using DeadReckoned Odometry Measurements . . . . . . Features and Maps . . . . . . . . . . . . . . . . .3.5. . . . A Probabilistic Framework . . . . . . . . . . . . . . . . . . . . . . . .4.Particle Filters . .1 Montecarlo Methods . . . . . . . . . . . . . . . . . . . . . . . . . . 56 59 59 61 63 65 69 69 70 70 71 71 72 73 73 74 78 81 83 83 6 Vehicle Models and Odometry 6. . . . . . . . . . . .3 The Extended Kalman Filter Equations . . . . .1 6. . 7.1 7. . . . . 7 Feature Based Mapping and Localisation 7. . .2 Feature Based Localisation . . . . . . .2 7. . . . .6 Simultaneous Localisation and Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
.3 Matlab Code For EKF Localisation Example . . . . . . . . . . . . . . .5 8. .1 Matlab Code For Mars Lander Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2 Grid Based Mapping . . . . . . . . . . . . . . . .2 Matlab Code For Ackerman Model Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .2 Drawing High Dimensional Gaussians . . . . . . 111 5 . . . . . .1 Drawing Covariance Ellipses . . . . . . . 11. . . . . . . . .6 Matlab Code For Particle Filter Example . . .4 Matlab Code For EKF Mapping Example . . . . . 100 11. . . . . . . . .5 Matlab Code For EKF SLAM Example . . . . . . 11. . . . . . . . 10. . 85 89 90 90 93 94 94 98 9 In Conclusion 10 Miscellaneous Matters 10. . . . . . . . . . . . . . . 107 11. . . . . . . . . . . . . 103 11. . 11 Example Code 11. . . . . .
Lets begin by dispelling some myths. Instead we are considering somekind of platform or vehicle that moves through its environment carrying some kind of payload. For the most parts when we talk about “mobile robots” we are not talking about gold coloured humanshaped walking machines 1 .Topic 1 Introduction and Motivation This set of lectures is about navigating mobile platforms or robots. This is a huge topic and in eight lectures we can only hope to undertake a brief survey. Almost without exception it is the payload that is of interest . However the vast majority of payloads require the host vehicle to navigate —to be able to parameterize its position and surroundings and plan and execute trajectories through it. However I hope that couching the material in a robotics scenario will make the material compelling and interesting to you. CMU NavLab project) • Scientiﬁc equipment on a Mars lander • Mine detectors on an autonomous underwater vehicle • Cargo containers in a port transport vehicle • Pathology media in a hospital deliver system 1 although the Honda P5 humanoid is a good approximation 6 .not the platform. Consider some typical autonomous vehicle and payloads: • Humans in a airplane on autopilot (or car in the nearish future. The estimation part of the lectures is applicable to many areas of engineering not just mobile robotics. The course is an extension of the B4 estimation course covering topics such as linear and nonlinear Kalman Filtering.
The former operates in a highly controlled.. Hospitals are a diﬀerent ballpark altogether. This course will hopefully give you some insight into how this can be achieved. Many industrial robots work in such well known engineered environments that very little external sensing is needed — they can do their job simply by controlling their own internal joint angles..) All of the above require navigation.. It is worth enumerating in general terms what makes autonomous navigation so hard.. If computer vision is used as a sensor then the workspace can be lit arbitrarily well to mitigate against shadows and color ambiguity. known. Even if the robot is endowed with a map of the hospital and ﬁtted with an upward looking camera to navigate oﬀ markers on the ceiling it still has to avoid fast moving obstacles (humans) while moving purposely towards it’s goal destination. that makes mobile robotics so challenging is uncertainty. The more generic case involves coping with substantial scene changes (accidental or malicious ) — for example doors closing in corridors or furniture being moved. The thing then.7 • Verbal descriptions in a museum tour guide • A semisubmersible drill ship (oil recovery) • Cameras on an aerial survey drone • Obvious military uses (tend to be single mission only. The corridors are dynamic — ﬁlling and emptying (eventually) with people on stretchers... time invariant (apart from the thing being built) scene. 7 . Uncertainty is pervasive in this area and we must embrace it to make progress. Compare the workspaces of a welding robot in automotive plant to one that delivers blood samples between labs in a hospital. The primary reason is that the majority of mobile robots are required to work in unengineered environments.
obstacle avoidance.y) is reached then rotate to align with B (theta).Topic 2 Introduction to Path Planning and Obstacle Avoidance A basic requirement of a mobile autonomous vehicle is path planning. • we cannot guarantee that the vehicle in question can turnonthespot and would like to be able to operate a vehicle of arbitrary shape. However we can do some interesting things if we decouple some of the issues and make some for example drive in a straight line from A to B until B (x. kinematic constraints and uncertainty makes for a very hard problem indeed — one which is still an active area of research. The combination of pathplanning. 1 8 . This sounds pretty simple and we can think of several ways in which we could combine simple control laws that will get us from A to B 1 Unfortunately the waters quickly become muddied when we start talking about our other concerns: • while executing its trajectory the vehicle must not smash into objects in the environment (especially true regarding squishy humans). With the vehicle in an arbitrary initial position A we wish to issue a desired goal position B (including orientation) and have the vehicle execute a trajectory to reach B . • we expect only uncertain estimates of the location of the robot and objects in the environment. These are called “kinematic” constraints.
y and heading however locally. a car can only move forward or turn. in words.Holonomicity 9 Figure 2. A sum of translation and slip combine to achieve any motion irrespective of pose.1 Holonomicity Holonomicity is the term used to describe the locomotive properties of a vehicle with respect to its workspace. We will introduce a mathematical deﬁnition of the term shortly but we will begin by stating. 2. simplifying assumptions. We can make this a little more clear with a few examples: • a car is nonholonomic : the global degrees of freedom are motion in x. a deﬁnition: A vehicle is holonomic if the number of local degrees of freedom of movement equals the number of global degrees of freedom. We shall begin by discussing vehicle properties and categorizing them into two classes — holonomic and nonholonomic. It cannot slide sideways. University of Hawaii) can move in any direction irrespective of pose and the complex wheel robot PPRK (CMU) driven by a “Palm Pilot” uses wheels that allow slip parallel to their axis of rotation.(Even the turning is coupled to motion).1: Two holonomic vehicles. 9 . The underwater vehicle (ODIN.
• The robot “Roombavac” (iRobot corporation) vacuum cleaner in Figure 2.Holonomicity 10 Figure 2.1) where the state derivatives cannot be integrated out. We say a vehicle whose state is parameterised by a vector x is nonholonomic if there exists a constraint Q such that ˙ ¨ Q(x.e.1 is also nonholonomic. In fact intrinsically holonomic vehicles are so rare and complex (or so simple 2 ) that we shall not discuss them further. x. We can now place some formalism on our notion of holonomicity. Unfortunately for us automation engineers. To illustrate such a constraint we will take the case of a front wheel steered vehicle as shown in ﬁgure 2. It should be obvious to you that motion control for a holonomic vehicle is much easier than for a nonholonomic vehicle. • The “spherical” underwater robot (Odin) and the rolling wheel vehicle in Figure 2.. the vast majority of vehicles in use today (i. If this isn’t obvious consider the relative complexity of parking a car in a tight space compared to driving a vehicle that can simply slide into the space sideways (a hovercraft).1 are holonomic they can turn on the spot and translate instantaneously in any direction without having to rotate ﬁrst. This vehicle can be purchased for about 100 USD .2: A commercial nonholonomic robot vehicle (Roombavac from iRobot coorporation.but is it’s utility great enough to warrant the prices? Discuss... It can rotate in place but cannot slide in any direction — it needs to use a turntranslateturn or turnwhiledrive (like a car) paradigm to move. · · · ) = 0 (2. • A train is holonomic: it can move forwards or backwards along the track which is parameterised by a single global degree of freedom — the distance along the track.1 2 path planning for a train is quite uninteresting 10 . used by humans) are nonholonomic. x.
For a simple vehicle moving on a plane the conﬁguration space has the same dimension as the work space but for more complicated robots the dimension of the conﬁguration space is much higher. x) = 0 (2. We can view obstacles as deﬁning regions of C − space that are forbidden.4) The conﬁguration space ( or C − space ) is the set of all allowable conﬁgurations of the robot. x2 . Consider the case of the bomb disposal vehicle in ﬁgure 2. 2. We 11 .2.3: A steered nonholonomic vehicle Immediately we can write a constraint expressing that the vehicle cannot slip sideways that is a function of x and its ﬁrst derivative: x ˙ − sin θ y . cos θ = 0 ˙ (2. We call the space within which x resides the conﬁguration space C − space of the vehicle: C= x ∀x1 .Conﬁguration Space 11 x' θ x =[x.x2 ···xn (2. · · · xn ]T ( for a 2D plane vehicle commonly x1 = x x2 = y x3 = θ )which is a nstate parameterisation.y.2 Conﬁguration Space We are describing the robot by its state x = [x1 .2) ˙ 0 θ ˙ Q(x. θ] T Figure 2. The conﬁguration space for such a vehicle would be 11 — 3 for the base and another 8 for the pose of the arm and gripper.3) The state and its derivatives are inseparable and by our deﬁnition the vehicle is nonholonomic.
2. Any path planning algorithm must guide the vehicle along a trajectory within this space while satisfying any nonholonomic vehicle constraints and ﬁnally deliver the vehicle to the goal pose. 12 .4: A vehicle with a high dimensional conﬁguration space . The conﬁguration space for a human is immensely high dimensional —around 230.6 simple states what conﬁgurations are open to the vehicle —nothing more.5) If each of the constraints imposed on C − spaceby each obstacle k is represented by an equation of the form Ck (x) = 0 then the open free space C admitted by n obstacles can be written as the following intersection: k=n C = k=1 {x  Ck (x) = 0} (2. Two poses that may be adjacent to each other in state space may require an arbitrarily long path to be executed to transition between them.Conﬁguration Space 12 Figure 2. the seemingly simple task of turning a vehicle through 180o when it is near a wall. For example if the workspace of a point robot in 2D x = [xy]T is bounded by a wall ax + by + c = 0 then C = {x  ax + by + c > 0} union of all x for which ax+by+c > 0 (2.6) Equation 2. Take. This clearly is a hard problem. One solution trajectory to the problem is shown in ﬁgure 2. for example. It is often possible to deﬁne and describe the boundaries of C⊗ (and hence the boundaries of C in which the vehicle must move) as a constraint equation. can label this space as C⊗ and the remaining accessible/permissable space as C such that C = C⊗ ∪ C . The nonholonomic vehicle constraints conspire with the holonomic constraint imposed by the wall to require a complicated solution trajectory.a commercial bombdisposal platform (picture courtesy of Roboprobe Ltd.
This doesn’t mean that awkward things won’t happen — the situation shown in ﬁgure 2. So now we have a method by which we can calculate C . The idea is to replace each object with a virtual object that is the union of all poses of the vehicle that touch the obstacle. The minimal MinkowskiSum would be the union of the obstacle and all vehicle poses with the vehicle nose just touching it boundary. The next big question is how exactly do we plan a path through it? How do we get from an initial pose to a goal pose? We will consider three methods : Voronoi. 2. This is easiest to understand with a diagram shown in Figure 2. One way to do this is to transform the problem to one in which the robot can be considered as a pointobject and a technique called the “MinkowskiSum” does just this.3. The basic idea is to artiﬁcially inﬂate the extent of obstacles to accommodate the worstcase pose of the robot in close proximity.2 is still possible.turning through 180o quickly becomes complicated by C − spaceconstraints. “Bug” and Potential methods.The MinkowskiSum 13 2 3 1 4 Figure 2. With the obstacles suitably inﬂated the vehicle can be thought of a pointobject and we have a guarantee that as long as it keeps to the new.3 has taken a conservative approach and “replaced” a triangular vehicle with a surrounding circle.5: A simple task . However progress can be made by planning motion such that at object boundaries the vehicle is always capable of moving tangentially to the boundaries 3 3 13 . shrunken. free space it cannot hit an object 3 . Note it is usual to ﬁt a polygonal hull around the results of the MinkowskiSum calculation to make ensuing path planning calculations easier.3 The MinkowskiSum Real robots have arbitrary shapes and these shapes make for complicated interactions with obstacles which we would like to simplify. Figure 2.
if we follow the paths deﬁned by a Voronoi diagram we are guaranteed to stay maximally far away from 4 http://www. So the Voronoi diagram of two points a.voronoi. Voronoi diagrams are elegant geometric constructions 4 that ﬁnd applications throughout computer science — one is shown in ﬁgure 2. The eﬃcient computation of Voronoi diagrams can be quite a complex matter but what is important here is that algorithms exist that when given a set of polygonal objects can generate the appropriate equidistant loci.6: The MinkowskiSum transforms a arbitrarily shaped vehicle to a point while inﬂating the obstacle.7: A Voronoi diagram for obstacle avoidance in the presence of point objects. b is the line bisecting them — all points on that line are equidistant from a. 2.Voronoi Methods 14 Figure 2. The result is guaranteed free space outside the inﬂated object boundary.4. So how does this help us with path planning? Well.4 Voronoi Methods 120 100 80 60 40 20 0 −20 −20 0 20 40 60 80 100 120 140 160 Figure 2. b.com/ 14 . Points on a 2DVoronoi diagram are equidistant from the nearest two objects in the real world.
Firstly the robot may be using the objects to localise and moving away from them makes them harder to sense.highways” until we reach the “exit point” where we leave the highway and drive directly towards the goal point.5 Bug Methods The generation of a global Voronoi diagram requires upfront knowledge of the environment. goto 2 In contrast to the Voronoi approach this method keeps the vehicle as close to the obstacles as possible (but we won’t hit them as the obstacles have been modiﬁed by the Minkowski sum!). We can search the set of points on the diagram to ﬁnd points that are closest to and visible from the start and goal positions. Once again the idea is very simple. However the path length could be stupidly long. starting from A and given the coordinates of a goal pose B draw a line AB (it may pass through obstacles that are known or as yet unknown) 2. 2. Also Voronoi planners by deﬁnition keep the vehicle as far away from objects as possible .Bug Methods 15 nearby objects. Secondly the paths generated from a Voronoi planner can be extremely long and far from the shortest path (try playing with the Matlab Voronoi function). An alternative family of approaches go under the name of “ bug algorithms”.this can have two side eﬀects. 3. on hitting an obstacle circumnavigate its perimeter until AB is met 4. We view the goal pose as a point of low potential energy and the 15 . The basic algorithm is simple: 1.6 Potential Methods A third very popular method of path planning is a so called “Potential Method”. We initially drive to the “highway entry” point. In many cases this is unrealistic. Clearly the hugging the object boundary is not always a good plan but interestingly it is guaranteed to get the robot to the goal location if it is indeed reachable.5 shows the kind of trajectory this modiﬁed “VisBug” algorithm would execute. follow the “Voronoi . 2. move along this line until either the goal is reached or an obstacle is hit. A smarter modiﬁcation would be to replace step 3 in the original algorithm with “on hitting and obstacle circumnavigate it’s perimeter until AB is met or the line AB becomes visible in which case head for a point on AB closer to B ” Figure 2.
At any point x we can write the total potential UΣ as a sum of the potential induced Uo by k obstacles and the potential induced by the goal Ug : UΣ (x) = i=1:k Uo.we can simply ∂x ∂y ∂x move the vehicle in the manner in which a particle would move in a locationdependent forceﬁeld.9) Uo. The local curvature and magnitude of the potential ﬁeld can be used to deduce a locally preferred motion.8 tells us the direction and magnitude of the force for any vehicle position x. don’t get confused here with the estimation in coming pages! 5 notation used for jacobians in the material covering nonlinear 16 .7) Now we know that the force F(x) exerted on a particle in a potential ﬁeld U Σ (x) can be written as : F(x) = − UΣ (x) =− i=1:k (2. Equation 2.Potential Methods 16 goal start Figure 2.i (x) + Ug (x) (2. If we think of the vehicle as a ballbearing free to roll around on a “potential terrain“ then it will naturally roll around obstacles and down towards the goal point.8) Ug (x) (2.i (x) − where is the grad operator V = i ∂V +j ∂V +k ∂V 5 This is a powerful tool . We now ﬁrm up this intuitive description with some mathematics. obstacles as areas of high potential energy.8: The visbug algorithm executing a goalseek trajectory around Southern Italy.
Deﬁning ρ(x) as the shortest distance point obstacle potential goal potential Figure 2. η is just a scale factor.i (x) = η 2 ρ(x) 0 otherwise 1 Ug (x) = (x − xg )2 2 The term ρ0 places a limit on the region of space aﬀected by the potential ﬁeld — the virtual vehicle is only aﬀected when it comes with ρ0 of an obstacle.i = η 0 1 ρ(x) (2. we take the sum of all obstacle 17 . We may also choose an everywhereconvex potential for the goal so that where ever we start. we will “fall” towards the goal point.12) where ∂ρ(x) is the vector of derivatives of the distance function ρ(x) with respect to x.Potential Methods 17 The next question is what exactly do the potential functions look like. Figure 2. in the absence of obstacles. Well.10) Uo. between the obstacle and the vehicle (at x ) and xg as the goal point. ∂x We proceed similarly for the goal potential. y.11) − 1 ρ0 1 ∂ρ(x) ρ(x)2 ∂x ∀ρ(x) ≤ ρ0 otherwise (2. So to ﬁgure out the force acting on a virtual vehicle and hence the direction the real vehicle should move in. the algebraic functions for these potentials are: 2 1 1 ∀ ρ(x) ≤ ρ0 − ρ10 (2. Simple diﬀerentiation allows us to write the force vector exerted on a virtual point vehicle as: Fo. A good choice would be to make the potential of an obstacle be an inverse square function of the distance between vehicle and obstacle. there is no single answer to that — you can “roll your own”.6 shows two useful potential candidates (forgive the pun). z.9: Two typical potential functions .inverse quadratic for obstacle and quadratic for the goal.
The potential method simply directs the vehicle in a straight line to a goal unless it moves into the vicinity of of an obstacle in which case it is deﬂected. This policy can lead to the vehicle getting trapped in a local minimum as shown in ﬁgure 2.5 1 1 1. Any other motion will increase its potential. the Voronoi as far away as possible. always moving in a direction of decreasing potential. Although elegant. The potential method is someway between the bug method and Voronoi method in terms of distance to obstacles.5 0 0 −1 −0.10: A pathological potential terrain and goal combination. If you run the code you will see the vehicle getting stuck between two features from time to time 6 .5 −1 −2 0 0. Here the vehicle will descend into a local minima in front of the two features and will stop. 500 400 300 200 100 TRAP GOAL 3 2. The vehicle can get stuck in a localminima or “trap” 6 one way out of this is to detect a trap and temporarily move the goal point.5 2 2 1. The bug algorithm sticks close to them.5 1 −1. the potential ﬁeld method has one serious drawback — it only acts locally.5 2. Download it and play. There is no global path planning at play here — the vehicle simply reacts to local obstacles. The course website has example code for a potential path planner written in Matlab. to where? 18 .5 0.5 1.6. the problem is.5 −2 0 2 0.5 3 TRAP 1 GOAL 2 Figure 2.Potential Methods 18 forces and the goal force – a very simple algorithm.
by processing data that is in some way dependent on x . There is a vast literature on Estimation Theory encompassing a rich variation of techniques and ideas.A Quick Revision 3. x. We shall bring them to bear on real life mobilerobot problems. We shall focus on some of the most commonly used techniques which will provide the tools to undertake a surprisingly diverse set of problems.” 1 For this reason some of this material was previously taught in the B4Estimation class 19 . 3.2 What is Estimation? To make sure we are all on the same page it is worth stating (in a wordy fashion) what we mean by “Estimation”: “Estimation is the process by which we infer the value of a quantity of interest. you will do well to keep in your mind that these are general techniques and can be applied to loads of other nonrobotic problems1 .1 Introduction This lecture will begin to cover a topic central to mobile robotics — Estimation. Although we shall discuss and illustrate these techniques in a domain speciﬁc fashion.Topic 3 Estimation .
Pretty quickly we see that if we can build an estimator capable of handling the above conditions and requirements we are equipping ourselves with a powerful and immensely useful inference mechanism. • We expect the measurements to be noise corrupted and would expect to see this uncertainty in input transformed to uncertainty in inference. Clearly this is a much harder and more sophisticated question. we may have our interest piqued when we impose the following additional characteristics to an estimator. For example we may know the depth and altitude of a submarine but not its latitude. • We do not expect the measurements to always be of x directly.1 Deﬁning the problem Probability theory and randomvariables are the obvious way to mathematically manipulate and model uncertainties and as such much of this lecture will rely on your basic knowledge 20 . if it weren’t for uncertainty many problems would have a simple algebraic solution —“give me the distances to three known points and the algebraicallydetermined intersection of three circles will deﬁne my location”. But equipped with basic probability theory and a little calculus the next sections will derive techniques capable of answering these questions using a few modest assumptions. • We might like to incorporate prior information in our estimation.What is Estimation? 20 There is nothing surprising there —it ﬁts intuitively with our everyday meaning of the word. However. Reasonably we expect this modelling error to be handled in the estimation process and manifest itself in the uncertainty of the estimated state. 3. For example a GPS receiver processes measurements of time (4 or more satellites transmit “timeofdayatsource”) and yet it infers Euclidean position. • We expect these models to be uncertain.2. Uncertainty is central to the problem of estimation (and robotics). After all. We will be trying to ﬁnd answers to more realistic questions like “I have three measurements (all with diﬀerent uncertainty) to three surveyed points (known roughly) — what is your best estimate of my location?”. • We might like to employ a model that tells us how we expect a system to evolve over time. For example given a speed and heading we could reasonably oﬀer a model on how a ship might move between two time epochs — openloop.
Figure 3. the maximum likelihood estimator . Given an observation z and a likelihood function p(zx). We form ˆ our maximum likelihood estimate xm.2) C Notice that equation 3.Maximum Likelihood. We also suppose that measurements (also referred to as observations in these notes) are not precise and are noise corrupted. ˆ xm. Crucially we interpret L as function of x and not z as you might initially think.l by varying x until we ﬁnd the maximum likelihood. We can encapsulate both the relational and uncertain aspects by deﬁning a likelihood function: L p(zx) (3. We however undertake a quick review.ML ﬁnds the value of x which maximises the likelihood function L p(zx). Least Squares and Minimum Mean Squared Error. z2 · · · zk }. We will use a “hat” to denote an estimated quantity and allow the absence of a hat to indicate the true (and unknowable) state of the variable.l = arg max p(zx) (3. as if they were independent Zk would contain no information about x and the sensor providing the measurements wouldn’t be much good 2 21 . Imagine we have been given an observation z and an associated pdf L for which we have a functional form of (Equation 3.2 is a function of both x and z . ˆ We want to obtain our best estimate x for a parameter x given a set of k measurements Z = {z1 . Maximum aposterior.2).3 is just such a distribution — a Gaussian in this case( C is just a normalising constant): 1 1 T −1 p(zx) = e− 2 (z−x) P (z−x) (3. 3. k We will start by considering four illuminating estimation problems .3) x clearly.3 Maximum Likelihood Estimation It is sane to suppose that a measurement z that we are given is in some way related to the state we wish to estimate 2 .Maximum Likelihood Estimation 21 of these subjects.1) The distribution p(zx) is the conditional probability of the measurement z given a particular value of x.
04 0. σp ).08 0.Estimation 22 0. Imagine x is a random variable which we have good reason to suppose is distributed as p(x ).07 z=15.1: A Gaussian Likelihood function for a scalar observation z 3. p(x).05 Λ 0. Given an observation z .MAP ﬁnds the value of x which maximises p(zx)p(x) (the normalising constant is not a function of x ).03 0. If we have a sensor that can produce an observation z of actual rotation speed with a behaviour modelled as p(zx) we can use Bayes rule to come up with a posterior pdf p(xz) that incorporates not only the information from the sensor but also our assumed prior knowledge on x : p(xz) = p(zx)p(x) p(z) = C × p(zx)p(x) The maximum aposteriori . the maximum a posteriori estimator .Estimation In many cases we will already have some prior knowledge on x .1 0.4) 22 .a likelihood function p(zx) and a prior distribution on x .MAP ﬁnds the value of x which maximises the posterior distribution p(xz) ˆ xmap = arg max p(zx)p(x) x (3. perhaps we know the intended (by design) rotational velocity µp of a CDROM drive .09 0.4 Maximum APosteriori .01 0 0 5 10 15 x 20 25 30 35 Figure 3.Maximum APosteriori .000000 0.02 0. For example.06 0.we might model 2 our prior knowledge of this as a gaussian ∼ N (µp .
Maximum APosteriori  Estimation
23
Lets quickly ﬁnish the CD example, assume our sensor also has a gaussian likelihood 2 function, centered on the observation z but with a variance σz : p(x) = C1 exp{− p(zx) = C2 exp{− (x − µp )2 } 2 2σp
(z − x)2 } 2 2σz p(zx)p(x) p(zx) = p(z) = C(z) × p(zx) × p(x) (x − µp )2 (z − x)2 − } = C(z) exp{− 2 2 2σp 2σz p(zx) is maximum when the exponent is zero. A simple way to ﬁnd what value of x achieves this is to rewrite the exponent of p(zx) as (x − µp )2 (z − x)2 (x − α)2 − =− 2 2 β2 2σp 2σz Expanding the R.H.S and comparing coeﬃcients swiftly leads to: α= β2 =
2 2 σz µ p + σ p z 2 2 σz + σ p 2 2 σz σp 2 2 σz + σ p
(3.5)
ˆ Therefore the MAP estimate x is zσ2p+σ2p and it has variance σ2z p2 . It is interesting and z p z +σp informative to note that as we increase the prior uncertainty in the CD speed by increasing ˆ σp towards inﬁnity then x tends towards z —the ML estimate. In other words when we have an uninformative prior the MAP estimate is the same as the ML estimate. This makes sense as in this case the only thing providing information is the sensor (via its likelihood function). There several other things that you should be sure you appreciate before progressing:
2 Decreasing posterior variance Calling the posterior variance σ+ ( which is β 2 above), we note it is smaller than that of the prior. In the simple example above this can be seen as σ1 = β12 = σ12 + σ12 . This is not surprising as the measurement is adding 2
+ p z
σ 2 µ +σ 2 z
σ2 σ2
information
3
3
Indeed a useful metric of information is closely related to the inverse of variance — note the addition of inverses...
23
Minimum Mean Squared Error Estimation
24
ˆ Update to prior In the Gaussian example above we can also write xmap as an adjustment to the prior mean: 2 σp ˆ × (z − µp ) (3.6) xmap = µp + 2 2 σp + σ z clearly if the observation matches the mode of the prior (expected) distribution then ˆ xmap is unchanged from the prior (but it’s uncertainty still decreases). Also note that the correction z − µp is scaled by the relative uncertainty in both prior and observation 2 2  if the observation variance σz is huge compared to σp (i.e. the sensor is pretty ˆ terrible) then irrespective of the magnitude of z − µp , xmap is still µp and the decrease in uncertainty is small. This too makes sense — if a sensor is very noisy we should not pay too much attention to disparities between expected (the prior) and measured values. This is a concept that we will meet again soon when we discuss Kalman Filtering.
3.5
Minimum Mean Squared Error Estimation
Another key technique for estimating the value of a random variable x is that of minimum mean squared error estimation. Here we assume we have been furnished with a set of observations Zk . We deﬁne the following cost function which we try to minimise as a ˆ function of x : ˆ x x (3.7) xmmse = arg min E{(ˆ − x)T (ˆ − x)Zk }
ˆ x
The motivation is clear, we want to ﬁnd an estimate of x that given all the measurement minimises the expected value of sum of the squared errors between the truth and estimate. Note also that we are trying to minimise a scalar quantity. Recall from probability theory that
∞
E{g(x)y} = the cost function is then:
∞
g(x)p(xy)dx
−∞
(3.8)
J(ˆ , x) = x
−∞
(ˆ − x)T (ˆ − x)p(xZk )dx x x
(3.9)
Diﬀerentiating the cost function and setting to zero: ∂J(ˆ , x) x =2 ˆ ∂x
∞ −∞
(ˆ − x)p(xZk )dx = 0 x 24
(3.10)
Recursive Bayesian Estimation
25
ˆ Splitting apart the integral, noting that x is a constant and rearranging:
∞ ∞
ˆ xp(xZ )dx =
−∞ ∞ −∞ ∞
k
xp(xZk )dx xp(xZk )dx
−∞ ∞
(3.11) (3.12) (3.13) (3.14)
ˆ x
−∞
p(xZk )dx = ˆ x=
−∞
xp(xZk )dx
ˆ xmmse = E{xZk }
Why is this important?  it tells us that the mmse estimate of a random variable given a whole bunch of measurements is just the mean of that variable conditioned on the measurements. We shall use this result time and time again in coming derivations. Why is it diﬀerent from the Least Squares Estimator? They are related (the LSQ estimator can be derived from Bayes rule) but here x is a random variable where in the LSQ case x was a constant unknown. Note we haven’t discussed the LSQ estimator yet  but by the time you come to revise from these notes you will have.
3.6
Recursive Bayesian Estimation
The idea of MAP estimation leads naturally to that of recursive Bayesian estimation. In MAP estimation we fused both apriori beliefs and current measurements to come up with ˆ an estimate, x , of the underlying world state x . If we then took another measurement ˆ we could use our previous x as the prior, incorporate the new measurement and come up with a fresh posterior based now, on two observations. The appeal of this approach to a robotics application is obvious. A sensor (laser radar etc) produces a timestamped sequence of observations. At each time step k we would like to obtain an estimate for it’s state given all observations up to that time ( the set Zk ). We shall now use Bayes rule to frame this more precisely:
p(x, Zk ) = p(xZk )p(Zk ) and also p(x, Zk ) = p(Zk x)p(x) 25
(3.15)
(3.16)
Zk−1 ) )= p(Zk−1 ) p(Zk ) = p(Zk−1 ) (3. observations are conditionally independent we can write p(Zk x) = p(Zk−1 x)p(zk x) where zk is a single observation arriving at time k.21) (3. Substituting into 3.Recursive Bayesian Estimation 26 so p(xZk )p(Zk ) = p(Zk x)p(x) (3.24) (3.20) (3.22) p(xZk−1 )p(Zk−1 ) p(x) (3.19) (3.23) p(zk .17 we get p(xZk )p(Zk ) = p(Zk−1 x)p(zk x)p(x) now we invoke Bayes rule to reexpress the p(Zk−1 x) term p(Zk−1 x) = and substituting to reach p(xZk )p(Zk ) = p(zk x) p(xZk−1 )p(Zk−1 ) p(x) p(x) = p(zk x)p(xZk−1 )p(Zk−1 ) (3.26) .25) so p(Zk−1 ) 1 = p(Zk ) p(zk Zk−1 ) 26 (3.17) if we assume (reasonably) that given the underlying state.18) so p(xZk ) = note that p(zk Z k−1 p(zk x)p(xZk−1 )p(Zk−1 ) p(Zk ) (3.
The recursive Bayesian estimator is a powerful mechanism allowing new information to be added simply by multiplying a prior by a (current) likelihood. The relationships are true whatever the form of the pdfs. Finally p(xZk−1 ) is a prior — it is our last best estimate of x at time k − 1 which at that time was conditioned on all the k − 1 measurements that had been made up until that time..Recursive Bayesian Estimation 27 ﬁnally then we arrive at our desired result: p(xZk ) = p(zk x)p(xZk−1 ) p(zk Zk−1 ) where the denominator is just a normaliser (3. Note that nothing special has been said about the form of the distributions manipulated in the above derivation. the linear Kalman Filter..the pdf of x conditioned on all observations we have received up to and including time k. The p(zk x) term is just the likelihood of the k th measurement. we recognise p(xZk ) as our goal . 27 . However we can arrive at a very useful result if we consider the case where we assume Gaussian priors and likelihoods.27) So what does this tell us? Well.
Initially you may naively think that a valid solution is x = H−1 z (4.1 Motivation Imagine we have been given a vector of measurements z which we believe is related to a state vector of interest x by the linear equation z = Hx (4.4) ˆ x = arg min (Hx − z)T (Hx − z) x Equation 4.1) We wish to take this data and solve this equation to ﬁnd x in terms of z .2) which is only a valid solution if H is a square matrix with  H = 0 — H must be invertible. There are several ways to solve this problem we will describe two of them .4 can be seen to be a “least squares” criterion. ˆ We can get around this problem by seeking a solution x that is closest to the ideal1 The metric of “closest” we choose is the following: ˆ x = arg min  Hx − z 2 x (4.Topic 4 Least Squares Estimation 4.one appealing to geometry and one to a little calculus. For this section we will assume that we have more observations than required ie dim(z) > dim(x) which assures that there is a unique “best” solution 1 28 .3) (4.
4 to zero :  Hx − z 2 = xT HT Hx − xT HT z − zHx + zT z ∂  Hx − z 2 = 2HT Hx − 2HT z ∂x =0 ⇒ x = (HT H)−1 HT z (4.2 LSQ Via Minimisation We can expand and set the derivative of Equation 4.8) (4.6) (4. Thus e must be orthogonal to every column of H: HT (Hx − z) = 0 which can be rearranged as HT Hx = HT z x = (H H) H z T −1 T (4.5) (4. The matrix (HT H)−1 HT is called the pseudoinverse of H. We seek a vector x such that Hx is closest to the data vector z . This is achieved when the error vector e = Hx−z is orthogonal to the space in which Hx is embedded. 4.1.n ˆ x = (AT A)−1 AT b m > n is (4.9) (4.10) The least squares solution for the linear system Ax = b with Am.1.7) This is the least squares solution for x .1 A Geometric Solution Recall from basic linear algebra that the vector Hx is a linear sum of the columns of H.Motivation 29 4. In other words Hx ranges over the column space of H.11) 29 .
16) (4. . We might express this information as a diagonal measurement covariance matrix R : 2 σz1 0 0 2 R = 0 σz2 · · · (4. It would be natural to weight each element of the error vector e according to our uncertainty in each element of the measurement vector z .2.Weighted Least Squares 30 4. .18) where H x0 = ∂h .17) (4. . . . We now have a new minimisation task: ˆ x = arg min  h(x) − z 2 x (4. .1 Nonlinear Least Squares The previous section allows us to derive a least squares estimate under a linear observation model. To proceed we use a Taylor series expansion: h(x0 + δ) = h(x0 ) + Hx0 δ  h(x1 ) − z 2 = h(x0 ) + Hx0 δ − z 2 = Hx0 δ − (z − h(x0 )) 2 A b (4. The new minimisation becomes: ˆ x = arg min  R−1 (Hx − z) 2 x (4. ∂x ∂hn ∂x1 ∂h 1 ∂x1 ··· ··· ∂h1 ∂xm ∂hn ∂xm evaluated at x0 . .19) 30 .15) We begin by assuming we have some initial guess of a solution x0 . We seek a vector δ such that x1 = x0 + δ and  h(x1 ) − z 2 is minimised.2 Weighted Least Squares Imagine now that we have some information regarding how reliable each of the elements in z is. (4. . = . .13) Carrying this through the same analysis yields the weighted linear least squares estimate: ˆ x = (HT R−1 H)−1 HR−1 z (4.. .ie by R−1 .measuring the Euclidean distance between two points for example. However most interesting problems will involve nonlinear models .12) .14) 4.
Within the hull (which ﬂoods) is a transceiver which emits a “call” pulse into the water column. Figure 4. z]T . The AUV shown in ﬁgure 4. Evaluate δ= ˆ ˆ 3. By inspection then we can write an expression for δ that minimises the right hand side of 4.something we have already solved and stated in equation 4. Imagine the vehicle is operating within a network of 4 beacons.20) We now set x1 = x0 +δ and iterate again until the norm falls below a tolerance value. consider the case of an autonomous underwater vehicle (AUV) moving within a network of acoustic beacons.2 shows a diagram of this kind of navigation (which is very similar to how GPS works). x Hx T R−1 Hx ˆ ˆ −1 Hx T R−1 [z − h(ˆ )] x ˆ 4. nonlinear least squares allows us to solve some interesting and realistic navigation problems. We shall assume that the observations become available simultaneously 31 . Like the linear case. Beacons deployed at known locations detect this pulse and reply with “acknowledge” pulses which are detected by the transceiver. The diﬀerence in time between “call” and “acknowledge” is proportional to the distance between vehicle and each beacon.18: δ= H x0 T H x0 −1 Hx0 T [z − h(x0 )] (4. For example.2. Set x = x + δ 4. For a measurement z with variance R the weighted nonlinear least squares algorithm is as follows: ˆ 1.11.an Example So why is the nonlinear LSQ problem interesting? Well. We wish to ﬁnd an LSQ estimate of the vehicle’s position xv = [x. ybi .2.2 is about to be launched on a mine detection mission in the Mediterranean. Each beacon i is at known position xbi = [xbi . If  h(ˆ ) − z 2 > goto 1 else stop. Begin with an initial guess x 2. y.2 Long Baseline Navigation . zbi ]T .18 can be seen to be a linear least square problem .Weighted Least Squares 31 Equation 4.2. there is a natural extension to the weighted non linear case.
the measurements are staggered.Weighted Least Squares 32 Figure 4.21) (4.23) where ∆xi = xbi − x ∆yi = ybi − y ∆zi = zbi − z r= (xbi − x)2 ) + (ybi − y)2 ) + (zbi − z)2 ) In practice of course the signal that travels the furthest comes in last .1: A Long Baseline Acoustic Network for an Autonomous Underwater Vehicle (AUV) so we can stack them into a single vector2 . This is only a problem if the vehicle is moving 2 32 . The model of the longbaseline transceiver operating in water with speed of sound c can be written as follows: z = t1 t2 t3 t4 = h(xv )  xb1 − xv  t1 t2 = 2  xb2 − xv  c  xb3 − xv  t3  xb4 − xv  ∆x1 2 ∆x2 =− rc ∆x3 ∆x4 ∆y1 ∆y2 ∆y3 ∆y4 ∆y1 ∆y2 ∆y3 ∆y4 T (4.22) so H xv (4.
The Kalman ﬁlter which we shall discuss. acousticrefractive properties of the water column may mean that replies from a particular beacon are never detected.HT H must be invertible. Figure 4. Oil rigs are perhaps one of the largest mobilerobots you are likely to encounter.2. (photo courtesy of MIT) see ﬁgure 4. Within the hull is a transceiver which emits a “call” pulse into the water column. One issue with the Least Squares solution is that enough data has to be accumulated to make the system observable .2. The large fork on the front is a sonar designed to detect buried mines.2. Beacons deployed at known locations detect this pulse and reply with a “acknowledge” pulses which are detected by the transceiver. Alternatively acoustic noise may obliterate the true signal leading to false detections.2: An autonomous underwater vehicle about to be launched. derive and use shortly is one way to overcome this problem of instantaneous unobservability.Weighted Least Squares 33 Figure 4. You might be surprised that many of the oil rigs ﬂoating in the Gulf of Mexico basically use this method to calculate their position relative to the wellhead thousands of meters below them. Most of the noise is due to the enormously powerful forklike sonar on its nose. For example.2. in our subsea example.2 shows some real LBL data from vehicle shown in Figure 4. 33 .2 With the jacobian calculated and given a set of four measurements to the beacons we can iteratively apply the nonlinear least squares algorithm until the change in x v between iterations becomes small enough to disregard. The diﬀerence in time between “call” and “acknowledge” is proportional to the distance between vehicle and each beacon.
The y axes are time of ﬂight × c giving eﬀective distance between vehicle and beacons.3: Some real data from the ocean. The synthetic aperture adds a lot of noise to the LBL sensing scheme. However it is possible to separate true signal from noise as shown in the lower graphs.Weighted Least Squares 34 Raw Data 3000 TOF x C (s) 2000 1000 0 1500 0 500 1000 1500 Data Sketch 2000 2500 3000 SAS turns on TOF x C (m) 1000 500 0 1500 TOF x C (m) 1000 500 0 0 500 1000 1500 Recovered Data 2000 2500 3000 0 500 1000 1500 Time (s) 2000 2500 3000 Figure 4. 34 .
n/2  R 1/2 (2π) 2 35 (5.1) (5. Motivation and Application Keep in mind The following algebra is simple but a little on the laborious side and was provided here for completeness. like many things in engineering.1 The Linear Kalman Filter We will now assume that the likelihood p(zx) and a prior p(x) on x are Gaussian. initially. I hope you will derive some degree of intellectual satisfaction in seeing the equations being derived using only Bayes rule.2) . 5. we are going to model our sensor as something that produces a observation z which is a noise corrupted linear function of the state x : z = Hx + w Here w is a gaussian noise with zero mean and covariance R so that 1 1 p(w) = exp{− wT R−1 w}.Topic 5 Kalman Filtering Theory. The following section will involve a linear progression of simple algebraic steps. Furthermore. Of course. The exams will not ask for a complete derivation of the Kalman ﬁlter but may ask you to explain the underlying concepts with reference to the key equations. derivations need not be done at every invocation of a smart idea but it is important to understand the underlying principles.
(2π)nx/2 1 1 exp{− (x − x )T P−1 (x − x )} 1/2 P  2 (5.4) Now we can use Bayes rule to ﬁgure out an expression for the posterior p(xz). We shall let the state have nx dimensions and the observation vector have nz dimensions.9) (5.7) C(z) This looks pretty formidable but we do know that we will end up with a gaussian (gaussian × gaussian = gaussian) and scale factors are not important therefore we can disregard the denominator and focus on the product: 1 1 exp{− (z − Hx)T R−1 (z − Hx)} exp{− (x − x )T P−1 (x − x )} 2 2 or equivalently: exp{−1/2 (z − Hx)T R−1 (z − Hx) + (x − x )P−1 (x − x )T } (5. 1 (2π)nz/2 R 1/2 1 exp{− (z − Hx)T R−1 (z − Hx)} 2 (5.: p(xz) = p(zx)p(x) p(z) p(zx)p(x) = ∞ p(zx)p(x)dx −∞ = 1 (2π)nz/2 R1/2 1 exp{− 1 (z − Hx)T R−1 (z − Hx)} (2π)nx/2 P 2 1/2 (5.3) We can also use a multidimensional Gaussian with mean x and covariance P to describe our prior belief in x : p(x) = .The Linear Kalman Filter 36 Note that up until now the examples have often been dealing with scalar 1D distributions.5) (5. This allows us to write down a likelihood function: p(zx) = .10) . Here we generalise to the multidimensional case but things should still look familiar to you.8) We know (because gaussian × gaussian = gaussian) that we can ﬁnd a way to express the above exponent in a quadratic way: (x − x⊕ )T P−1 (x − x⊕ ) ⊕ 36 (5.6) exp{− 1 (x − x )T P−1 (x − x )} 2 (5.
9 and comparing terms with expression 5. Comparing the second terms we see that: P−1 x⊕ = P−1 x + HT R−1 z. (5.14 tells us this will be the MMSE estimate. We begin by stating a block matrix identity.16) (5. x⊕ as x⊕ = (P−1 + HT R−1 H)−1 (P−1 x + HT R−1 z).10.6.14 and 5.The Linear Kalman Filter 37 . ⊕ Therefore we can write the MMSE estimate.11) Now collecting terms this becomes: xT (P−1 +HT R−1 H)x−xT (P−1 x +HT R−1 z)−(xT P−1 +zR−1 H)x+(xT P−1 x +zT R−1 z) (5. Fortunately we can do some algebra to come up with equivalent equations that do not involve an inverse. B and C the following is true (for nonsingular A ): (A + BCBT )−1 = A−1 − A−1 B(C−1 + BT A−1 B)−1 BT A−1 1 (5.13) ⊕ ⊕ ⊕ ⊕ ⊕ ⊕ P⊕ = (P−1 + HT R−1 H)−1 . There is something about the above two equations 5.14) Comparing ﬁrst terms in 5. Remember we want to ﬁnd the new mean because Equation 3. Given matrices A .12) Expanding 5. We can ﬁgure out the new mean x⊕ and covariance P⊕ by expanding expression 5.13 we immediately see that We can combine this result with our understanding of the recursive Bayesian ﬁlter we covered in section 3. So expanding 5.15) (5.10: xT P−1 x − xT P−1 x⊕ − xT P−1 x + xT P−1 x⊕ .16 that may make them inconvenient — we have to keep inverting our prior covariance matrix which may be computationally expensive if the statespace is large 1 .12 and 5.17) in some navigation applications the dimension of x can approach the high hundreds 37 .9 we have: xT P−1 x−xT P−1 x −xT P−1 x +xT P−1 x +xT HT R−1 Hx−xT HT R−1 z−zR−1 Hx+zT R−1 z (5. (5. Every time a new measurement becomes available we update our estimate and its covariance using the above two equations.
29) (5.22) (5.32) (5.23 38 (5.25) (5.27) (5.14 to get: P⊕ = P − P HT (R + HP HT )−1 HP = P − WSWT (5.16 we have: x⊕ = (P−1 + HT R−1 H)−1 (P−1 x + HT R−1 z) = (P −1 (5.24) −1 = (P−1 + HT R−1 H)−1 (HT R−1 z) + x + W(−Hx ) = Cz + x + W(−Hx ) where C = (P−1 + HT R−1 H)−1 HT R−1 Taking a step aside we note that both + H R H) (H R z) + (I − WH)P (P x ) T −1 −1 T −1 (5.20 with 5.19) (5.21) (5.20) or = (I − WH)P where S = HP HT + R W=P H S T −1 (5.33) .26) (5. Combining 5.23) Now look at the form of the update equation 5.16 it is a linear combination of x and z .31) (5.28) HT R−1 (HP HT + R) = HT R−1 HP HT + HT and also (P−1 + HT R−1 H)P HT = HT + HT R−1 HP HT so (P−1 + HT R−1 H)−1 HT R−1 = P HT (HP HT + R)−1 therefore C = P HT S−1 = Wfrom 5.The Linear Kalman Filter 38 We can immediately apply this to 5.30) (5.18) (5.
34) The update equations are as follows.Prediction The previous section showed how an observation vector (z ) related in some linear way to a state we wish to estimate (x ) can be used to update in a optimal way a prior belief /estimate.1 Incorporating Plant Models . Given an observation z with uncertainty (covariance) R and a prior estimate x with covariance P the new estimate and covariance are calculated as: x⊕ = x + Wν P⊕ = P − WSWT where the “Innovation” ν is ν = z − Hx the “Innovation Covariance” S is given by S = HP HT + R and the “Kalman Gain” W is given by W = P HT S−1 5. a (imperfect) model of a system that models the transition of state from time k − 1 to k. This analysis lead us to the so called Kalman update equations. what is the new estimate at time k? 39 .The Linear Kalman Filter 39 Finally then we have x⊕ = x + W(z − Hx ) We can now summarise the update equations for the Linear Kalman Filter: (5. However there is another case which we would like to analyse: given a prior at time k − 1.1.
This is exactly what a Kalman ﬁlter does — it maintains the statistics of a pdf that “best” represents x given a set of measurements and control inputs 3 2 (5. For example. (A good example of u is the steer angle on a car input into the system by a driver (machine or human)) The ij notation Our general approach will be based on an inductive argument. it may model an acceleration on a model that assumes constant velocity.36 we have ˆ x(ij) = E{x(i)Zj }.38) 40 . Incorporating this into Equation 5. We have already shown (and used) the fact that given a set Z k of k observations the MMSE estimate of x is ˆ xmmse = E{xZk } (5. We use x(ij) to mean the “MMSE estimate of x at time i given observations up until time j”.36) We now drop the “mmse” subscript and start to use two parenthesised time indexes i and ˆ j. However ﬁrst we shall introduce a little more notation. It models actions taken on the plant that are independent of the state vector itself. a random variable distributed according to a zeromean gaussian pdf If this sounds confusing focus on the fact that we are maintaining a probability distribution for x .The Linear Kalman Filter 40 Mathematically we are being told that the true state behaves in the following fashion: x(k) = Fx(k − 1) + Bu(k) + Gv(k) (5. The term u(k) is the control vector at time k. If our distribution is in fact a Gaussian then these are the only statistics we need to fully characterize the pdf.37) (5. Our estimate at time i using measurements up until time j (ˆ (ij)) is simply the mean of this distribution and x it has variance P(ij). We also use this notation to deﬁne a covariance matrix P(ij) as ˆ ˆ P(ij) = E{(x(i) − x(ij))(x(i) − x(ij))T Zj } ˆ which is the mean square error of the estimate x(ij)3 .35) Here we are modelling our uncertainty in our model by adding a linear transformation of a “white noise” 2 term v with strength (covariance) Q.
44) x ˆ = F(x(k − 1) − x(k − 1k − 1)) + Gv(k) (5.41) (5. You can think of predicting as an “open loop” step as no measurements of state are used to correct the calculation.37) however we are talking about an estimate at time k. This is often called the prediction Now back to our question about incorporating a plant model F and a control signal u. We can use our “conditional expectation result” to progress: ˆ x(kk − 1) = E{x(kZk−1 } (5. It simply says that the best estimate at time k given measurements up until time k − 1 is simply the projection of the last best estimate ˆ x(k − 1k − 1) through the plant model. We are also provided with a control signal u(k) we want to know how to ﬁgure out a new ˆ estimate x(kk − 1) at time k. Imagine at time k we have been provided with MMSE estimate of x (k1).39) (5.The Linear Kalman Filter 41 ˆ x (i—j) is the estimate of x at time i given measurements up until time j.40) k−1 k−1 = FE{x(k − 1)Z } + Bu(k) + GE{v(k)Z = Fˆ (k − 1k − 1) + Bu(k) + 0 x = E{Fx(k − 1) + Bu(k) + Gv(k)Zk−1 } } (5.43) ˆ x(k) − x(kk − 1) = (Fx(k − 1) + Bu(k) + Gv(k)) − (Fˆ (k − 1k − 1) + Bu(k)) (5. Note that we are still only using measurements up until time (k − 1) ( i > j in Equation 5.42) This is a both simple and intuitive result. ˆ ˆ P(kk − 1) = E{(x(k) − x(kk − 1))(x(k) − x(kk − 1))T Zk−1 } perfoming the subtraction ﬁrst (5. Often simply called the estimate ˆ • x(kk − 1) estimate at time k given ﬁrst k − 1 measurements. Commonly you will see the following combinations: ˆ • x(kk) estimate at time k given all available measurements. By convention we call this kind of estimate a “prediction”.45) 41 . Now we turn our attention to the propagation of the covariance matrix through the prediction step. Using our new ˆ notation we write this estimate and its covariance as x(k − 1k − 1) and P(k − 1k − 1).
47) k−1 ˆ Now we are going to make a moderate assumption: that the previous estimate x(k − 1k − 1) and the noise vector (modelling inaccuracies in either model or control) are uncorrelated (i.47 all cross terms between x(k − 1k − 1) and v(k) disappear. So: ˆ ˆ P(kk − 1) = FE{(x(k − 1) − x(k − 1k − 1))(x(k − 1) − x(k − 1k − 1))T Zk−1 }FT + (5. We use x(kk − 1) as ˆ the prior x and process an observation z at time k. ˆ E{(x(k − 1) − x(k − 1k − 1))v(k)T } is the zero matrix). This means that when we expand ˆ Equation 5.2 Joining Prediction to Updates We are now almost in a position to put all the pieces together. To start with we insert our ˆ now temporalconditional notation into the Kalman update equations.49) T (5. The posterior x⊕ becomes x(kk) P(kk) = P(kk − 1) − W(k)SW(k)T ν(k) = z(k) − Hx(kk − 1) W(k) = P(kk − 1)HT S−1 S = HP(kk − 1)HT + R ˆ ˆ x(kk) = x(kk − 1) + W(k)ν(k) So now we are in a position to write down the “standard Linear Kalman Filter Equations”. If the previous pages of maths have started to haze your concentration wake up now as you will need to know and appreciate the following: 42 . Σ) and y = M x then y ∼ N (M µ.e. 5.1.50) This result should also seem familiar to you ( remember that if x ∼ N (µ.46) } (5. M ΣM T ) ?).The Linear Kalman Filter 42 so ˆ P(kk − 1) = E{(F(x(k − 1) − x(k − 1k − 1)) + Gv(k))× ˆ (F(x(k − 1) − x(k − 1k − 1)) + Gv(k)) Z T (5.48) P(kk − 1) = FP(k − 1k − 1)F + GQG GE{v(k)v(k)T Zk−1 }GT T (5.
If these x 43 . The output of one iteration becomes the input to the next. Note that the innovation ν is a diﬀerence between the actual observation and the predicted observation (Hˆ (kk − 1)).1.56) (5.55) (5. The prediction step is corrected by fusion of a measurement. These are initial guesses (hopefully derived with some good judgement) Structure The Kalman ﬁlter has a predictorcorrector architecture.3 Discussion There are several characteristics of the Kalman Filter which you should be familiar with and understand well.52) (5.54) P(kk) = P(kk − 1) − W(k)SW(k)T where ν(k) = z(k) − Hˆ (kk − 1) x T (5.The Linear Kalman Filter 43 Linear Kalman Filter Equations prediction: ˆ x(kk − 1) = Fˆ (k − 1k − 1) + Bu(k) x T (5. ˆ Initialising Initially you will have to provide P(00) and x(00). A ﬁrm grasp of these will make your task of implementing a KF for a robotics problem much easier. Recursion The Kalman Filter is recursive.57) W(k) = P(kk − 1)HT S−1 S = HP(kk − 1)H + R 5.53) (5.51) T P(kk − 1) = FP(k − 1k − 1)F + GQG update: ˆ ˆ x(kk) = x(kk − 1) + W(k)ν(k) (5.
A metric of information is related to the inverse of covariance — note how in equation 5. This too makes sense. Indeed.The Linear Kalman Filter 44 two are identical then there is no change to the prediction in the update step —both observation and prediction are in perfect agreement. Asynchronisity The update step need not happen at every iteration of the ﬁlter. This makes sense sice we are only using a preconceived idea of how x will evolve. The diagonals are the principal uncertainties in each of the state vector elements. B Q] of a plane ﬂying. The Kalman ﬁlter implicitly uses these correlations to update states that are not observed directly by the measurement model! Let’s take a real life example. However the Kalman ﬁlter can perform updates with only partial measurements. However to get useful results over time the system needs to be observable otherwise the uncertainty in unobserved state components will grow without bound. Fusion happens in data space. after 100ms the plane will have travelled 10m forward (y direction) and perhaps 1. this might suggest to you that information is additive.16 is the so called “information form” of the Kalman Filter. Correlations The covariance matrix P is highly informative. Each observation fused contains some information and this is “added” to the state estimate at each update. Clearly the changes and uncertainties in y and z are correlated — we do not expect massive changes in height for little change 4 5 technically it never increases it which will always be positive semi deﬁnite 44 . The subtraction of WSWT 5 during each update illustrates this.5 m down (in z direction). The model will explain how the plane moves between epochs as a function of both state and control. Prediction Covariance Inﬂation Each predict step inﬂates the covariance matrix — you can see this by the addition of GQGT .16 HT R−1 H is added to the inverse of P. Update Covariance Deﬂation Each update step generally4 deﬂates the covariance matrix. The oﬀ diagonals tell us about ˆ the relationships between elements of our estimated vector x — how they are correlated. Imagine we have a model [F. By our own admission — the presence of a noise vector in the model — the model is inaccurate and so we would expect certainty to dilate (uncertainty increase) with each prediction. nose down 10 deg. If at a given time step no observations are available then the best estimate at time k is simply ˆ the prediction x(kk − 1). Two factors make this possible : the fact that the priors presumably contain some information about the unobserved states (they were observed in some previous epoch) and the role of correlations. This is crucial and useful. equation 5. In a least squares problem at every update there have to be enough measurements to solve for the state x . For example at 100m/s. Observability The measurement z need not fully determine the state x (ie in general H is not invertible).
W S W k=k+1 T Figure 5.g steering wheel angle Prediction x(kk1) = x(k1k1) P(kk1) = P(k1k1) x(kk1) = Fx(k1k1) +Bu(k) P(kk1) = F P(k1k1) F T +GQG T Delay no New observation z(k) available ? Data from sensors enter algorithm here e.H x(kk1) estimate is last prediction Update x(kk) = x(kk1)+ Wv(k) P(kk) = P(kk1) . Note the recursive structure and how simple it is to implement once the model matrices F and H have been speciﬁed along with the model uncertainty matrices R and Q . The plane is equipped with an altimeter radar which measures height above the ground . The exercises associated with these lectures should illustrate this fact to you further.g compass measurement yes Prepare For Update x(kk) = x(kk1) P(kk) = P(kk1) S = H P(kk1) H T +R W = P(kk1) H T S 1 v(k) = z(k) .The Linear Kalman Filter 45 no prediction is last estimate New control u(k) available ? yes Input to plant enters here e. in distanceoverground in normal ﬂight conditions. 45 .a direct measurement of z. Fusing this measurement in the Kalman ﬁlter will result in a change in estimated height and also a change in yposition.1: Flow chart for the Kalman ﬁlter. The reason being that the correlations between height and yposition maintained in the covariance matrix mean that changes in estimated height should imply changes in estimated yposition because the two states are codependent. Now comes the clever part.
It represents noise in acceleration (hence the quadratic time dependence when adding to a positionstate). Remember estimation theory is at the core of many problems in mobile robotics — sensors are uncertain. 5. Tools like the Kalman ﬁlter 6 give a powerful framework in which we can progress.2.1 A Linear Navigation Problem . We begin modelling by assuming that the vehicle has reached its terminal velocity (but this velocity may change with height slowly).1.“Mars Lander” A Lander is at an altitude x above the planet and uses a time of ﬂight radar to detect altitude—see Figure 5. The task is to estimate both altitude and descent velocity using only the radar.2 Using Estimation Theory in Mobile Robotics Previous pages have contained a fair amount of maths taking you through the derivation of various estimators in a steady fashion. h (5. The onboard ﬂight controller needs estimates of both height and velocity to actuate control surfaces and time rocket burns.59) The process noise vector is a scalar. models are always incomplete.2.58) where δT is the time between epochs and the state vector x is composed of two elements altitude and rate of altitude change (velocity) h x(k) = ˙ . implementation and discussion a few new ideas are introduced which will be used later in the course. Now we need to write down the observation equations. The following sections are not just a standaloneexample. 6 it is not the only method though 46 . We will now apply these freshly derived techniques to problems in mobile robotics focussing particularly on navigation.Using Estimation Theory in Mobile Robotics 46 5. Within the analysis. Think of the observation model as “explaining the observations as a function of the thing you want to estimate”. gaussian process with covariance Q. A simple model would be: δT 1 δT x(k − 1) + 2 v(k) x(k) = 0 1 δT F G 2 (5.
The exact source created the following graphs and so all parameters can be read from the listing.2: A simple linear navigation problem. 47 .it can also be downloaded from the course website ¡http://www. It’s important that you can reconcile all the estimation equations we have derived with the source and also that you understand the structure of the algorithm —the interaction of simulator.uk/∼pnewman : teaching¿. We are now in a position to implement this example in Matlab. controller and estimator.61) 0 h ˙ + w(k) h These are the “truth models” of the system (note there are no “hats” or (k—k) notations involved). A Lander is at an altitude x above the planet and uses a time of ﬂight radar to detect altitude.robots.ac.1 is a print out of the entire source code for a solution . The task is to estimate both altitude and descent velocity using only the radar. So we can write z(k) = Hx(k) + w(k) z(k) = 2 c (5.60) (5. We will never known the actual value of the noises at any epoch but we model the imperfections in the sensor and motions models that they represent by using their covariance matrices (R and Q respectively) in the ﬁlter equations.ox. Section 11. The onboard ﬂight controller needs estimates of both height and velocity to actuate control surfaces and time rocket burns.Using Estimation Theory in Mobile Robotics 47 z x Figure 5.
We only examine the vertical descent velocity (i.1. 7 we shall shortly introduce a way to use nonlinear models but the idea that the world is always more complicated than we care to imagine is still valid 48 .e. One point of this example is to illustrate that sometimes we can get away with simple approximations to complex systems. we ignore the coriolis acceleration)— it’s as though we were dropping a weight vertically into a layer of atmosphere.5 −50 0 −0.5 −1 −150 100 200 300 time (s) 400 500 100 200 300 time (s) 400 500 −100 5. This of course is realistic — we may model the world within our ﬁlter as a well behaved linear system 7 but the physics of the real world can be relied upon to conspire to yield something far more complicated. Lands @ 580. The drag exerted on the vehicle by the atmosphere is a function of both vehicle formfactor. Rockets @ 402.2. velocity and atmospheric density as a function of height (modelled as a saturating exponential). the details of the models employed in the simulation are not important.2. However.Using Estimation Theory in Mobile Robotics 48 True and Estimated Trajectories 10000 8000 5 Altitude (m) 6000 0 4000 −5 2000 −10 100 200 300 400 500 0 10 Trajectories Error (XEst−XTrue) 200 400 600 Chute @ 178.2 Simulation Model The simulation model is nonlinear.9 1 0 Velocity (km/h) 0.
the parachute is also jettisoned. Soon the density increases and the vehicle brakes under the eﬀect of drag to reach a quasisteady 49 . The important point here is that the controller operates on estimated quantities.Using Estimation Theory in Mobile Robotics 49 Measurement (time of flight seconds) x 10 6 5 4 3 2 1 0 −5 Observations 50 100 150 200 250 300 time 350 400 450 500 550 x 10 Innovation (time of flight seconds) 5 −7 Innovation Sequence 0 −5 0 100 200 300 time 400 500 600 Figure 5. Initially the vehicle is high in thin atmosphere which produces little drag. At the point when rockets are ﬁred.3: Vehicle Controller The controller implemented does two things. If the estimated quantities are in error it is quite easy to envision bad things happening. The motivation to understand estimation process and its failure modes is clear! Analysis of Mars Lander Simulation Flight Pattern Figure 5. Secondly. Firstly when the vehicle descends below a predetermined height it deploys a parachute which increases its eﬀective aerodynamic drag. The vehicle accelerates through the high levels of the atmosphere.1. This is a point common to all robotic systems—actions (involving substantial energies) are frequently executed on the basis estimates. A simple control law based on altitude and velocity is used to control the vehicle to touch down with a low velocity.2. it ﬁres retroburners when it drops below a second altitude threshold.1 shows the simulated(true) and estimated states using the code listed in Section 11.
This makes sense 9 the velocity state is being dragged (and hence lags) through state space by the correlations to directly observed position. It is an important part of KF deployment and can be hard to do in practice.3. But we cannot reduce Q too far— it has to model the uncertainty in our model.. When the parachute opens the instantaneous increase drag decelerates the vehicle rapidly until another steady state terminal velocity is achieved. Note how during times of acceleration the velocity estimate lags behind the true velocity. Derivation of Velocity Note that the observation model H does not involve the velocity state and yet the ﬁlter is clearly estimating and tracking velocity pretty well. The mechanism behind this has already been discussed in section 5. The addition of GQGT at each time step dilutes the interstate correlations. 8 .Using Estimation Theory in Mobile Robotics 50 state terminal velocity.. Note that during these times the innovation sequence and truthestimated graphs ‘spike’. The process of choosing a suitable Q (and R ) is called tuning. G > is one of constant velocity (noise in acceleration). 1) is a function of the oﬀ diagonals 8 50 .. Errors in position are correlated through the vehicle model to errors in velocity.1. Filter Tuning An obvious question to ask is how can the ﬁlter be made “tighter”? How can we produce a more agile tracker of velocity? The answer lies in part with the process noise strength Q . If we reduce it too much we will have too much conﬁdence in our predictions and the update stage will have little corrective eﬀect. The ﬁlter is not explicitly diﬀerentiating position to derive velocity — instead it is inferring it through the models provided. Note how the change in velocity W(2. This is easy to spot in the plant model as predicted position is a function of current position estimate and velocity estimate. At no point in the code can you ﬁnd statements like vel = (xnew − xold)/∆T . However during periods of rapid acceleration we see poor estimates of velocity emerging. Fortunately there are a few concepts that can help in this process. However there is no free lunch. The covariance matrix starts oﬀ being diagonal but during the ﬁrst prediction step it becomes fully populated. The ﬁlter is using correlations (oﬀ diagonals) in the 2 × 2 matrix P between position and velocity states. Try forcing the oﬀ diagonals to zero and see the eﬀect 9 expand the KF update equations with a constant velocity model and full P matrix. Here the KF is working as an observer of a hidden state — an immensely useful characteristic. Their derivation is more suited for a course in stochastic estimation rather than mobile You should download the code and check this out for yourself. When the velocity is constant both position and velocity are tracked well. By making Q smaller we maintain stronger correlations and track inferred velocity. Fitness of Model The ﬁlter vehicle model < F. Finally shortly before landing the retroburners are ﬁred to achieve a smooth landing . Examining the graphs we see this is true.essentially decelerating the vehicle smoothly until touch down. Oﬀtheblocks then we should expect good estimation performance during periods of constant velocity.
Figure 5. The correct thing to do here is implement a better model. Innovation Sequence The innovation sequence should be a white (utterly random). There is some skill involved in choosing values of R and Q such that the above criteria are met. Normalised Estimate Error Squared If we denote the error at epoch k between ˜ ˜ true and estimated states as x(k) then the quantity E{˜ (k)T P(kk)−1 x(k)} is x distributed according to chisquared distribution with degrees of freedom equal to the state dimension. The ﬁlter should be unbiased and so the average error should be zero. The ith element innovation sequence should lie with √ +/ − Sii . It is helpful to think heuristically of the innovation as the exhaust from the ﬁlter.2. This of course means that the ﬁlter looses any claim to optimality. Note how during periods of constant velocity the innovation sequence (a scalar sequence in this case) is bounded around 66 percent of the time. If the ﬁlter is optimal all information will have been extracted from previous observations and the diﬀerence between actual and predicted observations will be noise10 . Normalised Innovation Squared The scalar quantity ν(k)T S−1 ν(k) is distributed according to chisquared distribution with degree of freedom equal to the dimension of the innovation vector ν(k) Estimate Error In a deployed system the only information available to the engineer is the innovation sequence (see above). If however. other engineering issues impede this course of action. There is a powerful geometric interpretation of the Kalman ﬁlter that ﬁts closely to this analogy using ideas of orthogonality 10 51 .Using Estimation Theory in Mobile Robotics 51 robotics and so some of them are stated here as a set of rules (valid for linear Gaussian problems). However if a simulation is available comparisons between estimated and nominal true states can be made.2 shows these bounds plotted. the ﬁlter must be detuned (increase noise strengths) in the hope of ‘lumping’ unmodelled characteristics into the noise vector.zero mean process. SBound The innovation covariance matrix S provides a 1σ 2 statistical bound on the innovations sequence. especially when the ﬁlter models are a poor representation of the truth.
This then requires the use of a nonlinear measurement model.a really simple GPS receiver sitting at xr = (x.3 Incorporating NonLinear Models . (5. Fx = = . k) + w(k). u(k). y. Think about an everyday example . This is done ˆ using a Taylor series expansion. Assume we have an estimate x(k − 1k − 1) then x(k) = f (ˆ (k − 1k − 1). z)T .62) (5. . y. 5. . .64) The term Fx is understood to be the jacobian of (f ) with respect to x evaluated at an elsewhere speciﬁed point: ∂f ∂f1 1 · · · ∂xm ∂x1 ∂f .3.Incorporating NonLinear Models .63) The trick behind the EKF is to linearize the nonlinear models around the “best” current estimate (best meaning prediction (kk − 1) or last estimate (k − 1k − 1)). Fortunately we shall see that nonlinear models can easily be incorporated into the Kalman Filter equations (yielding a ﬁlter called the Extended Kalman Filter or EKF) you are already familiar with. u(k).The Extended Kalman Filter 52 5. The derivations are given here for completeness and the results are stated in Section 5.3. It shouldn’t come as a surprise to you that the majority of real world applications require the use of nonlinear models. Clearly the time of ﬂight measurement is “explained” s by the norm of the diﬀerence vector  xr − xs . ∂x ∂fn ∂fn · · · ∂xm ∂x1 52 . u(k).The Extended Kalman Filter Up until now we have only considered linear models (although working in nonlinear simulated environments). k) + x ˆ Fx [x(k − 1) − x(k − 1k − 1)] + · · · (5. z)T and measuring time of ﬂight (equivalent) to a satellite sitting at xs = (x. .1 Nonlinear Prediction We begin by writing a general form for a nonlinear plant truth model: x(k) = f (x(k − 1). k) + v(k) z(k) = h(x(k).3.
k) + Fx [x(k − 1) − x(k − 1k − 1)] + v(k)− x f (ˆ (k − 1k − 1).69) (5.76) = Fx P(k − 1k − 1) Fx T + Q (5.73) ˜ = E{ Fx x(k − 1k − 1)˜ (k − 1k − 1)T Fx T Zk−1 } + E{v(k)v(k)T Zk−1 } x (5. k) + x ˆ Fx [x(k − 1) − x(k − 1k − 1)] + · · · + v(k)Zk−1 } (5.The Extended Kalman Filter 53 ˆ ˆ Now we know that x(kk − 1) = E{x(k)Zk−1 } and x(k − 1k − 1) = E{x(k − 1)Zk−1 }so ˆ x(kk − 1) = E{f (ˆ (k − 1k − 1).75) We now have the predict equations in the case of nonlinear plant models. u(k).the same result follows.65) ˆ Fx [ˆ (k − 1k − 1) − x(k − 1k − 1)] x (5.74) T k−1 (5.71) (5.72) (5.67) = E{f (ˆ (k − 1k − 1).70) (5. u(k).77) ˜ ˜ ≈ E{( Fx x(k − 1k − 1) + v(k))( Fx x(k − 1k − 1) + v(k)) Z } (5. x x ˆ Understanding that the jacobians of f are evaluated at x(k − 1k − 1) we can write ˜ ˆ x(kk − 1) = x(k) − x(kk − 1) ˆ ≈ f (ˆ (k − 1k − 1). k) (5. To ﬁgure out how to propagate the covariance (using only terms from the previous time step) we look at the behaviour of ˜ ˆ x(kk − 1) = x(k) − x(kk − 1) because P(kk − 1) = E{˜ (kk − 1)˜ (kk − 1)T Zk−1 }. v(k). u(k). k) x ˆ = Fx [x(k − 1) − x(k − 1k − 1)] + v(k) = Fx [˜ (k − 1k − 1)] + v(k) x So P(kk − 1) = E{˜ (kk − 1)˜ (kk − 1)T Zk−1 } x x (5. k) x Which is an obvious conclusion — simply pass the last estimate through the nonlinear model to come up with a prediction based on a control signal u (k).68) (5.66) (5.79) alternatively stack x and v in a new vector y and diﬀerentiate with respect to y . In this case one would proceed with a multivariate Taylor11 series which swiftly becomes notationally complex and algebraically tiresome. u(k). The state prediction remains unchanged but the prediction equation becomes: P(kk − 1) = 11 Fx P(k − 1k − 1) Fx T + 53 Gv Q Gv T (5. Note that frequently the model will be in the form x(k) = f (x(k − 1).Incorporating NonLinear Models . .78) where the noise v (k) is not simply additive. k)Zk−1 } + x = f (ˆ (k − 1k − 1). u(k). However the end result is intuitive. u(k).
They evaluate x to zero as (reasonably) we do not expect the noise in observations to be correlated to the error in our prediction.88) T k−1 x x = E{( Hx (˜ (kk − 1)) + w(k))( Hx (˜ (kk − 1)) + w(k)) Z Hx P(kk − 1) Hx + R T Hx E{˜ (kk − 1)˜ (kk − 1)T Zk−1 } Hx T + E{w(k)w(k)T Zk−1 } x x } (5.82) Now we wish to derive an expression for S. In this case Gv = Gu . 54 .84) = h(ˆ (kk − 1)) + Hx (ˆ (kk − 1) − x(k)) + · · · + w(k) − h(ˆ (kk − 1)) x x x (5.89) (5. x (5. You will see this again soon.r. the innovation covariance. In the meantime.85) = = Hx (ˆ (kk − 1) − x(k)) + w(k) x Hx (˜ (kk − 1)) + w(k) x (5.The Extended Kalman Filter 54 where Fu is the jacobian of f w.t the noise vector.3.Incorporating NonLinear Models .83) ˜ z(kk − 1) = z(k) − z(kk − 1) (5.87) So the covariance of the diﬀerence between actual and predicted observations (the innovation) can be written as: S = E{˜(kk − 1)˜(kk − 1)T Zk−1 } z z = = (5. 5. look at the example source code provided.91) You may be wondering where the E{˜ (kk − 1)w(k)T Zk−1 } terms have gone.90) (5.81) (5.2 Nonlinear Observation Model Now we turn to the case of a nonlinear observation model (for example a range and bearing sensor) of the general form z(k) = h(x(k)) + w(k) (5.80) By using the same technique used for the nonlinear prediction step we can show that the predicted observation z(kk − 1) (Hˆ (kk − 1) in the linear case) is simply the projection of x ˆ x(kk − 1) through the measurement model: z(kk − 1) = E{z(k)Zk−1 } z(kk − 1) = h(ˆ (kk − 1)).86) (5. (Don’t worry many examples to follow — also look at the source code provided ) It is a common practice to make the substitution u(k) = un (k) + v(k) where un (k) is a nominal control which is then corrupted by noise. We begin by expressing ˜ the observation and the estimated observation error z(kk − 1) using a Taylor series: z(k) ≈ h(ˆ (kk − 1)) + Hx (ˆ (kk − 1) − x(k)) + · · · + w(k) x x (5.
we have used the fact that the expectation of x is zero and so the crossterms of the expansion evaluate to zero.99) = E{([I − W Hx ]˜ (kk − 1) + Ww(k))([I − W Hx ]˜ (kk − 1) + Ww(k)) Zk } x x (5.102) = [I − W Hx ]P(kk − 1)[I − W Hx ]T + WRWT (5.104) (5.101) = [I − W Hx ]E{˜ (kk − 1)˜ (kk − 1)T Zk }[I − W Hx ]T + WE{w(k)w(k)T Zk }WT x x (5.103) ˜ Above. We now just need to ﬁnd an expression for W . Recall that behind all of this is a quest to minimise the mean square estimation error (which is scalar): ˜ x x J(ˆ ) = E{˜ (kk)T x(kk)kZk} = T r(P(kk)) 55 (5. P(kk) = E{˜ (kk)˜ (kk)T Zk } x x ˜ ˆ x(kk) = x(kk) − x(k) ˆ = x(kk − 1) + W(z(k) − h(ˆ (kk − 1))) − x(k) x ˆ z = x(kk − 1) + W˜(kk − 1) − x(k) and substituting equation 5.96) (5.98) (5.105) .100) T (5.The Extended Kalman Filter 55 We now have one last thing to ﬁgure out — how does a nonlinear observation model ˆ change the update equations resulting in x(kk) and P(kk)? The procedure should now be ˜ becoming familiar to you: ﬁgure out an expression using a series expansion for x(kk) and take squared conditional expectation to evaluate P(kk).97) (5.92) We can now use this expression to make progress in ﬁguring out P(kk).94) (5. Here we derive the gain using a little calculus.93) (5.Incorporating NonLinear Models .87 ˆ x = x(kk − 1) + W( Hx (˜ (kk − 1)) + w(k)) − x(k) ˜ ˜ = x(kk − 1) + W Hx x(kk − 1) + Ww(k) x = [I − W Hx ]˜ (kk − 1) + Ww(k) An expression for P(kk) follows swiftly as P(kk) = E{˜ (kk − 1)˜ (kk − 1)T Zk } x x (5.95) (5. Assume that “somehow” we have a gain W (we’ll derive this in a minute) then we can immediately write: ˆ ˆ x(kk) = x(kk − 1) + W ( z(k) prediction gain observation − h(ˆ (kk − 1)) x predictedobservation innovation (5.
The Extended Kalman Filter 56 Fortunately there is a closed form to the diﬀerential of such a function.106) ∂A ∂A Applying this (and the chain rule) to Equation 5.103 leads to the form of the covariance update we are already familiar with: P(kk) = P(kk − 1) − WSWT (5.107) = P(kk − 1) Hx T S−1 (5.Incorporating NonLinear Models . Make 56 .108) Note that substituting P(kk − 1) Hx T = WS into Equation 5. (You’ll need these for the classwork).3 The Extended Kalman Filter Equations We can now state in a “good to remember this box” a rule for converting the linear Kalman ﬁlter equations to the nonlinear form: To convert the linear Kalman Filter to the Extended Kalman Filter simply replace F with Fx and H with Hx in the covariance and gain calculations only.103 and setting the derivative equal to zero we get: ∂J(ˆ ) x = −2[I − W Hx ]P(kk − 1) Hx T + 2WR = 0 ∂W W = P(kk − 1) Hx T ( Hx P(kk − 1) Hx T + R)−1 where S = ( Hx P(kk − 1) Hx T + R) (5.its not the end of the world.110) 5.109) (5. The jacobians are always evaluated at the best available estimate (ˆ (k − 1k − 1) x ˆ for Fx and x(kk − 1) for Hx .3. If you don’t feel you are on top of the previous maths . For completeness here are the EKF equations. If B is symmetric for any A then ∂AC ∂T r(ABAT ) = 2AB = CT (5.
. ∂x ∂hn ∂x1 Innovation Covariance ∂f 1 ··· ∂x1 ∂h1 ∂xm ∂hn ∂xm Fx = ∂f .111) Gv Q Gv T process noise P(kk − 1) predicted covariance = Fx P(k − 1k − 1) old est covariance observation model Fx T + (5.112) x z(kk − 1) = h(ˆ (kk − 1)) predicted obs (5.The Extended Kalman Filter 57 sure however you are familiar with the general form of the equations. . .119) 57 . u(k) . (5. .115) P(kk) new covariance estimate = P(kk − 1) − WSWT update decreases uncertainty where measurement ν(k) = z(k) W = P(kk − 1) Hx S kalman gain −z(kk − 1) T −1 (5. k) predicted state old state est control (5.117) (5.114) (5. (exam useful things are in boxes in these notes (but not exclusively)) Prediction: plant model ˆ x x(kk − 1) = f (ˆ (k − 1k − 1).113) Update: prediction and correction ˆ x(kk) new state estimate ˆ = x(kk − 1) + W ν(k) innovation (5. . ∂x ∂fn ··· ··· ∂f1 ∂xm ∂fn ∂xm ∂x1 ˆ evaluated at x(k − 1k − 1) .116) (5. . = .Incorporating NonLinear Models . = .118) S= ∂f 1 ∂x1 Hx P(kk − 1) Hx T + R ∂h . Hx = ··· ˆ evaluated at x(kk − 1) .
Incorporating NonLinear Models . 58 . possessing some very powerful tools which we shall now apply to some key autonomous navigation problems.The Extended Kalman Filter 58 We are now in a great position.
1 Velocity Steer Model This is a good point to write down a simple motion model for a mobile robotic vehicle.2) (6. We allow the vehicle to move on a 2D surface ( a ﬂoor) and point in arbitrary directions.1. θv Figure 6.4) Using the instantaneous center of rotation we can calculate the rate of change of orien 59 . The angle of the steering wheels is given by φ and the instantaneous forward velocity (sometimes called throttle) is V . We parameterise the vehicle pose xv (the joint of position and orientation) as xv (6.1) x v = yv .Topic 6 Vehicle Models and Odometry 6. we immediately see that xv = V δT cos(θv ) ˙ yv = V δT sin(θv ) ˙ (6.1 is a diagram of a nonholonomic (local degrees of freedom less than global degree of freedom) vehicle with “Ackerman” steering. With reference to Figure 6.3) (6.
One popular way to do this is to insert noise terms into the control signal u such that u(k) = un (k) + v(k) (6.9 is a model for a perfect.8) (6. u(k) = φ(k) xv (k + 1) + δT V (k) cos(θv (k)) xv (k + 1) yv (k + 1) = yv (k + 1) + δT V (k) sin(θv (k)) tan(φ(k)) θv (k + 1) θv (k) + δT V (k) L (6.10) where un (k) is a nominal (intended) control signal and v(k) is a zero mean gaussian dis60 .9) (6. Equation 6. noiseless vehicle.y) L Global Reference Frame Figure 6. Clearly this is a little unrealistic — we need to model uncertainty.6) (6.5) (6. u(k)).1: A nonholonomic vehicle with Ackerman steering tation as a function of steer angle: L = tan(φ) a aθ˙v = V V θ˙v = tan(φ) L We can now discretise this model by inspection: V (k) xv (k + 1) = f (xv (k).7) Note that we have started to lump the throttle and steer into a control vector — this makes sense if you think about the controlling actions of a human driver.Velocity Steer Model 60 Instantaneous Center of Rotation φ φ a V θ (x.
12) u(k) ∼ N (un (k). We shall now see how propagation of this model aﬀects uncertainty in vehicle pose over time. 6.79 have that: Pv (kk − 1) = In this case Q= xv (kk − 1) = f (ˆ (k − 1k − 1).2. The model derived in the previous section is nonlinear and so we will have to use the nonlinear form of the prediction step.14 using the matlab code printed in Section 11.13 and 6.2 shows the results of iterating equations 6.1.2 Evolution of Uncertainty In this section we will examine how an initial uncertainty in vehicle pose increases over time as the vehicle moves when only the control signal u is available.16) 0 0 1 δT cos(θv ) 0 0 Fu = δT sin(θv ) (6. u(k). The uncertainty 61 . Hopefully you will realise that one way to approach this is repetitive application of the prediction step of a Kalman ﬁlter discussed in Section 5. 2 σV 0 0 2 ) σφ 2 σV 0 (6. k) x (6.67 and 5.Evolution of Uncertainty 61 tributed noise vector: v(k) ∼ N (0.11) (6. Things are pretty much as we might expect.17) tan(φ) L δT V sec2 (φ) L Figure 6. Equations 5.14) (6. 0 2 ) σφ This completes a simple probabilistic model of a vehicle. We do this by diﬀerentiating each row of f by each state and each control respectively: 1 0 −δT V sin(θv ) Fx = 0 1 δT V cos(θv ) (6.1.15) 0 2 σφ ˆ We need to evaluate the Jacobians with respect to state and control noise at x(k − 1k − 1). ˆ Assume at time k we have been given a previous best estimate of the vehicle pose xv (k − 1k − 1) and an associated covariance Pv (k − 1k − 1).13) T Fx Pv (k − 1k − 1) Fx + 2 σV 0 Fv Q Fv T (6.
Of course. In actual real life the real robot is integrating the noisy control signal. Figure 6. Typically robot wheels are ﬁtted with encoders that measure the rotation of each wheel. one would expect a gross accumulation of error as the time spent walking “open loop” increases. The true trajectory will therefore always drift away from the trajectory estimated by the algorithms running inside the robot. Your inner ears and legs give you u which you pass through your own kinematic model of your body in your head. This is typical of all dead reckoning methods — the only thing that can be changed is the rate of divergence. The problem is a wheel or radius r may have turned through θ but due to slip/skid the distance travelled over the ground is only (1 − η)rθ where η is an unobservable slip parameter.3 shows the eﬀect of integrating odometry on a real robot called “B21” shown in ﬁgure 6. This always leads to what is called “dead reckoning drift”. Note the divergence between true and deadreckoned locations. injected into the system via the noisy control makes the estimated covariance of the vehicles grow without bound. The main cause of this divergence on land vehicles is wheel slip. The point is that all measurements such as velocity and rate of turn are measured in the vehicle frame and must be integrated. The circles are the true location of the vehicle whereas the crosses mark the deadreckoned locations. There is an important point to make here that you must understand.2: Evolution of uncertainty evolution of open loop the Ackermann Model. Position is then an integralfunction of these “wheel counts”. along with the noise on the measurements.Evolution of Uncertainty 62 uncertainty bounds for Ackermann model 25 20 15 10 y 5 0 −5 −20 −15 −10 −5 0 x 5 10 15 20 Figure 6. 62 . This is exactly the same as closing your eyes and trying to walk across University Parks. The orientation of the vehicle is made clear by the orientation of the triangles.3(right).
3: Sometimes a navigation system will be given a dead reckoned position as input without recourse to the control signals that were involved.Using DeadReckoned Odometry Measurements 63 6.3 is a typical example. We align a bodycentered coordinate frame on the vehicle as shown.3. Assume it has two wheels (left and right).3 Using DeadReckoned Odometry Measurements The model in Section 6.com 63 .irobot. δθr — as shown in Figure 6. The architecture in ﬁgure 6. We can make a guess at the kind of model it uses1 . It is common to ﬁnd that this low level knowledge is not easy to obtain or that the relationship between control. prior and prediction is not readily discernable. Dead Reckoned output (drifts badly) Physics (to be estimated) required (corrected) navigation estimates Hidden Control Physics Other Measurements Encoder Counts DR Model Nav Supplied Onboard System integration Navigation (to be installed by us) Figure 6.the real b21 vehicle is actually a synchronous drive machine in which all four wheels change direction http://www. Clearly by the end of the experiment we cannot reasonably interpret deadreckoned position as an unbiased measurement of position! The low level controller on the vehicle reads encoders on the vehicle’s wheels and outputs an estimate (with no metric of uncertainty) of its location. It would clearly be a bad plan to simply use a deadreckoned odometry estimate as a direct measurement of state in something like a Kalman Filter.1 used velocity and steer angles as control input into the model.3 which is the dead reckoned position of a B21 mobile robot (shown on right of ﬁgure) moving around some corridors. We want 1 This for illustration only . Consider Figure 6. Nevertheless the deadreckoned position can be converted into a control input (a stream of small motions) for use in the core navigation system. radius r mounted either side of its center of mass which in one time interval turn an amount δθl .
4: Dead Reckoned position from a B21 mobile robot. δθr : rδθr = (c − L/2)α rδθl = (c + L/2)α L δθl + δθr ⇒c= 2 δθl − δθr 2r ⇒ α = (δθl − δθr ) L Immediately then we have (1 − cos α)c dx dy = c sin α −α dθ Which for small α becomes: 0 dx dy = r(δθl + δθr )/2 −2r(δθl −δθr ) dθ L 64 (6.18) (6.21) (6. This is typical of dead reckoned trajectories — small angular errors integrate to give massive long term errors to express the change of position of the center of the vehicle as a function of δθl .23) The deadreckoning system in the vehicle simply compounds these small changes in position .22) (6. The start and end locations are actually the same place! See how you could roll the trajectory back over itself.19) (6.Using DeadReckoned Odometry Measurements 64 Figure 6.20) (6.
Here x and y are translations in frame i to a point p and θ is anticlockwise rotation around p.k xj. Of course the “addition” is slightly more complex than simple adding (otherwise the x coordinate would always be zero!).Using DeadReckoned Odometry Measurements 65 dθ y' x' y [dx .5: Geometric Construction for a two wheel drive vehicle and orientation to obtain a global position estimate.1 shows three relationships between three coordinate frames.1 Composition of Transformations Figure 6.k = xi.25) 65 α r . What actually happens is that the vehicle composes successive coordinate transformation.dy ] L x c Figure 6.j = [xyθ].i = xi. and then “adds” this to its last deadreckoned position. This is an important concept (which you will probably have met in other courses but perhaps with a diﬀerent notation) and will be discussed in the next section.3.3.j ⊕ xj. We can express any coordinate j frame with respect to another frame i as a threevector xi.24) (6. We deﬁne two operators ⊕ and to allow us to compose (chain together) multiple transformations: xi. Starting from an initial nominal frame at each iteration of its sensing loop it deduces a small change in position and orientation.j (6. 6.
2 X 1.1 shows a vehicle with a prior pose xo (k − 1). However we set out to ﬁgure out a way in which a deadreckoned pose could be used to form a control input or measurement into a navigation system. In 66 .3 . The processing of wheel rotations between successive readings (via Equation 6. The task of combining this new motion u(k) with the old deadreckoned estimate xo to arrive at a new deadreckoned pose xo is trivial. in another alternative frame.23) has indicated a vehiclerelative transformation (i. They allow you to express something (perhaps a point or vehicle) described in one frame. Figure 6. It is simply: xo (k) = xo (k − 1) ⊕ u(k) (6.Using DeadReckoned Odometry Measurements 66 2 3 X 2.3 1 X 1.28) We have now explained a way in which measurements of wheel rotations can be used to estimate deadreckoned pose.1 we see that x1.e.27) Be sure you understand exactly what these equations allow.3.2 ⊕ x2.3 Figure 6.6: Three transformations between coordinate frames With reference to ﬁgure 6. they are just a short hand for a function of one or two transformations: x1 ⊕ x 2 = x1 + x2 cos θ1 − y2 sin θ1 y1 + x2 sin θ1 + y2 cos θ1 −x1 cos θ1 − y1 sin θ1 x1 = x1 sin θ1 − y1 cos θ1 −θ1 (6. But what exactly are these operators? Well.3.26) (6. in the frame of the vehicle) u . We can use this notation to explain the compounding of odometry measurements.3 = x1.
We are now in a position to write down a plant model for a vehicle using a deadreckoned position as a control input: xv (k + 1) = f (xv (k). xo (2) · · · xo (k) etc and we want to ﬁgure out u (k). This is now simple— we can invert equation 6.28 to get u(k) = xo (k − 1) ⊕ xo (k) (6.32) = xv (k) ⊕ uo (k) It is reasonable to ask how an initial uncertainty in vehicle pose Pv propagates over time. This gives us a small control vector u (k) derived from two successive deadreckoned poses that is suitable for use in another hopefully less error prone navigation algorithm. u(k)) = xv (k) ⊕ ( xo (k − 1) ⊕ xo (k)) dr−control (6. This is one area where the compositional representation we have adopted simpliﬁes matters.79. We know that one way to address this question is to propagate the second order statistics (covariance) of a pdf for xv through f using equation 5.29 subtracts out the common deadreckoned gross error . Eﬀectively equation 6.Using DeadReckoned Odometry Measurements 67 x o(k) u(k) x o (k1) Global Frame Figure 6. other words we are given from the lowlevel vehicle software a sequence xo (1).7: Using transformation compositions to compound a local odometry measurement with a prior deadreckoned estimate to deduce a new deadreckoned estimate.30) (6.32 with respect to xv and u. To do this we need to ﬁgure out the jacobians of equation 6.locally odometry is good .1 we can see that the transformation u(k) is equivalent to going back along xo (k − 1) and forward along xo (k).3.29) Looking at the Figure 6.globally it is poor.31) (6. We can deﬁne and calculate the following jacobians: 67 .
Even when uo = 0 the covariance Pv will continue to inﬂate.37) (6. x2 ) (6. For example if the odometry loop ran at 20Hz and the vehicle is moving at 1m/s the magnitude of translation in u would be 5cm. This motivates the use of a time varying Uo which is a function of uo (k).35) (6. uo )Uo J2 (xv .05/100)2 . uo ) + J2 (xv .. An exercise you should think about. If we say slip accounts for perhaps one percent of distance travelled we might “try” a value 2 2 of σox = σoy = (0.Using DeadReckoned Odometry Measurements 68 ∂(x1 ⊕ x2 ) ∂x1 1 0 −x2 sin θ1 − y2 cos θ1 = 0 1 x2 cos θ1 − y2 sin θ1 0 0 1 ∂(x1 ⊕ x2 ) J2 (x1 .. Allowing a maximum rotation of w perhaps a good starting 2 guess for σoθ would be (w/100)2 . x2 ) ∂x2 cos θ1 − sin θ1 0 = sin θ1 cos θ1 0 0 0 1 J1 (x1 .79) : P(kk − 1) = Fx Pv (k − 1k − 1) Fx T + Fv Q Fv T T T (6.33) (6.38) = J1 (xv .39) 2 0 0 σoθ implying we expect errors in x y and orientations to be uncorrelated which is probably not true in reality but we will live with this approximation for now 2 68 . Here the matrix Uo describes the strength of noise in the small shifts in pose represented by uo derived from two sequential deadreckoned poses. uo )Pv (k − 1k − 1)J1 (xv . A simple form of this matrix would be purely diagonal 2 : 2 σox 0 0 2 Uo = 0 σoy 0 (6. These numbers will give sensible answers while the vehicle is moving but not when it is stopped. uo ) where the diagonals are variances in odometry noise...34) (6.36) This allows us to write (substituting into equation 5.
laser cameras A common way to approach these problems is to parameterise both the robot pose and aspects of the environment’s geometry into one or more state vectors.Topic 7 Feature Based Mapping and Localisation 7. Inertial xray acoustic Acoustic Beacons. Inertial Cameras.Mapping and Localisation. Radar. Scanning Laser. Theses two tasks are fundamental to successful deployment of autonomous vehicles and systems for example: Mapping Managing autonomous opencast mining Battleﬁeld Surveillance Fracture detection Subsea Oil ﬁeld detection on an AUV Localisation GPS Museum Guides Hospital Delivery System GPS. Cameras Radio tags.1 Introduction We will now apply the estimation techniques we have learnt to two very important mobile robotics tasks . For this course we 69 . GPS. Inertial Satellite and time of ﬂight Sonar.
xf .2 Features and Maps We suppose that the world is populated by a set of discrete landmarks or features whose location / orientation and geometry (with respect to a deﬁned coordinate frame) can be described by a set of parameters which we lump into a feature vector xf .2) where x and y are the coordinates of the point in a global frame of reference. To keep notation simple sometimes we will use M to denote the map vector which is simply the concatenation of all the features: xf . our own vision system operates at a higher level recognising things like gorillas and sailboats as complete entities but lowerlevel geometricprimitive detection is buried in there as well.1 .2 . We call a collection of n features a Map such that M = {xf . 7.n In this course we will constrain ourselves to using the simplest feature possible . .3 Observations We deﬁne two distinct types of observations all denoted as z — vehicle relative and absolute. valid for the full 3D case. xf . Points occur at the intersection of lines. 1 70 . corners of rectangles.2 (7.1) M= . of course.3 · · · xf . .a point feature such that for the ith feature: xf .Features and Maps 70 will work mostly in 2D but the deﬁnitions that follow are.n }.i = xi yi (7. edges of objects (regions of maximal curvature)1 . xf . The best examples Sure. Absolute Absolute observations are made with the help of some external device and usually involve a direct measurement of some aspect of the vehicle’s pose.1 xf . Point features are not as uncommon as one might initially think. 7.
Features and Landmarks VehicleFeature Relative Observation x fi Mobile Vehicle xv Global Reference Frame Figure 7. Vehicle Relative This kind of observation involves sensing the relationship between the vehicle and its immediate surroundings –especially the map.1 Probabilistic Localisation For the localisation task we assume we have been given a map and receive a sequence of vehiclerelative observations described by a likelihood p(Zk M. They depend on external infrastructure (taken as known) and are nothing to do with the map.A Probabilistic Framework 71 are a GPS and a compass.4. We shall also class odometry (integration of wheel movement) as a vehiclerelative measurement because it is not a direct measurement of the vehicle’s state.4 A Probabilistic Framework It is informative to describe the localisation and mapping tasks in terms of likelihoods (observation pdf) and priors. see ﬁgure 7.3.1: Feature Based Navigation and Mapping 7. 7. xv ). We wish to ﬁgure a pdf 71 . A great example is the measurement of the angle and distance to a point feature with respect to the robot’s own frame of reference.
8) 7.11) (7.4. x)p(xv M)p(M)dxv −∞ (7. xv )p(M.4) (7.3) (7.6) (7.9) (7. z) k p(Z M.e.2 Probabilistic Mapping For the mapping side of things we are given the vehicle location (probably derived from absolute observation) and an sequence of vehicle relative observations. xv )p(xv M)p(M) = ∞ p(Zk M. xv )p(xv M)p(M) = p(M. Zk ) p(Zk M.7) (7. p(Mxv .5) If we are given a nominally perfect map (i. Zk ) = p(Zk M. told to take it as absolute truth) then p(M) = 1 this simpliﬁes the above: p(xv M. x)p(xv M)p(M)dxv (7. Zk ) = p(Zk M. xv ) p(xv . xv )p(Mxv )p(xv ) = p(xv .12) If we assume perfect knowledge of the vehicle location then p(xv ) = 1 and so 72 . We wish to ﬁnd the distribution of M conditioned on Zk and xv . Zk ) p(Zk M. xv )p(Mxv )p(xv )dM −∞ (7.10) (7.A Probabilistic Framework 72 for the vehicle pose given map and observations: p(xv M. z) k p(Z M. xv )p(xv M)p(M) p(Zk M. xv )p(Mxv )p(xv ) = ∞ p(Zk M. xv ) p(M. Zk ) = = = ∞ −∞ p(Zk xv )p(xv ) ∞ p(Zk x)p(xv )dxv −∞ p(Zk xv )p(xv ) C(Zk ) p(Zk M. xv )p(M.
3).16): 73 .15) Equations 7. In this case the Kalman ﬁlter is the optimal Bayesian estimator. We assume the vehicle we are navigating is equipped with a rangebearing sensor which returns the range and bearing to point like objects (the features). recursive bayesian estimation which collectively lead us to discuss and explore the Kalman ﬁlter. The ﬁrst derivation of the Kalman ﬁlter presented proceeded by assuming Gaussian distributions. We assume to begin with that an oracle is telling us the associations between measurements and observed features. xv )p(Mxv )p(xv )dM (7. If we parameterise the random vectors x v and M with ﬁrst and second order statistics (mean and variance) then the Kalman Filter will calculate the MMSE estimate of the posterior. xv )p(Mxv )p(xv ) p(Zk M. The Kalman ﬁlter provides a real time way to perform state estimation on board a vehicle. We will base the ensuing simulation on a real vehicle .15 and 7. The Kalman ﬁlter would appear to be an excellent way in which to implement these equations. 7.13) (7. We denote these as xo We have already have the prediction equations from previous discussions ( Equation 7.8 should have a familiar form — we met them when discussing Maximum a priori estimators.the B21 we have already met .Feature Based Estimation for Mapping and Localising 73 p(Mxv .5 Feature Based Estimation for Mapping and Localising Feature Based Localisation 7.this means that we will have an additional sequence of deadreckoned positions as input into the prediction stage. Zk ) = = = ∞ −∞ p(Z M)p(M) C(Zk ) ∞ p(Zk M)p(M)dM −∞ k p(Zk M)p(M) p(Zk M. We are given a map M containing a set of features and a stream of observations of measurements between the vehicle and these features (see ﬁgure 7.1 This is the simplest task.5.14) (7.
r. uo )Uo J2 (xv . When the sensor comes back on line there is a jump in estimated vehicle location back to one close to the true position (see ﬁgure 7.ac. 7.3 is a print out of an implementation of feature based localisation using the models we have just discussed. Figure 7.25) R= r 2 0 σθ Section 11.24) We assume independent errors on range and bearing measurements and use a diagonal observation covariance matrix R : σ2 0 (7.18) (7.21) (7. Perhaps the vehicle is ﬁtted with a GPS 74 .17) (7.1 shows the trajectory of the vehicle as it moves through a ﬁeld of random point features. uo )T + J2 (xv .uk/∼pnewman : teaching¿.t xv to arrive at the observation model jacobian: Hx (7.5.1).Mapping.5.16) (7.19) Fv Q Fv T = J1 (xv . uo )T Now we come to the observation equation which is simply a range ri and bearing θi to the ith feature: r z(k) (7. w(k)) = (xi − xv (k))2 + (yi − yv (k))2 y atan2( xi −yv (k) ) − θv i −xv (k) ∂h ∂x = xi −xv (k) r yi −yv (k) r2 yi −yv (k) r − xi −x2v (k) r (7.robots.20) θ = h(x(k). You can also download the code from the course website ¡http://www. Note that the code simulates a sensor failure for the middle twenty percent of the mission.2 Feature Based Mapping Now we will consider the dual of Localisation .22) We diﬀerentiate w. During this time the vehicle becomes more and more lost. uo )Pv (k − 1k − 1)J1 (xv .23) 0 −1 (7.Feature Based Estimation for Mapping and Localising 74 P(kk − 1) = ˆ ˆ x(kk − 1) = x(k − 1k − 1) ⊕ ( xo (k − 1) ⊕ xo (k)) ˆ ˆ x(kk − 1) = x(k − 1k − 1) ⊕ uo (k) Fx P(k − 1k − 1) Fx T + (7.ox. In this case the vehicle knows where it is but not what is in the environment.5.
3: Feature Based Localisation innovation and error/covariance plots.Feature Based Estimation for Mapping and Localising 75 60 40 20 0 −20 −40 −60 −80 −60 −40 −20 0 20 40 60 80 Figure 7. or some other localisation system using an apriori map. To avoid initial confusion we’ll imagine the vehicle has a ’supergps’ on board telling it where it is at all times. Notice how when no measurements are made the vehicle uncertainty grows rapidly but reduces when features are reobserved. The state vector for this problem is now much larger it will be the Map itself and 75 . The covariance bounds are 3sigma and 1sigmabound on state error and innovation plots respectively.2: Feature Based Localisation Innovation 10 40 20 0 −20 −40 0 1000 2000 3000 4000 5000 6000 x Covariance and Error 5 range 0 −5 20 −10 0 1000 2000 3000 4000 5000 6000 y 10 0 −10 −20 200 100 0 −100 θ 0 1000 2000 3000 4000 5000 6000 30 20 Bearing (deg) 10 0 −10 −20 −30 0 1000 2000 3000 time 4000 5000 6000 −200 0 1000 2000 3000 time 4000 5000 6000 Figure 7.
We assume that features don’t move and so x(k + 1k)map = x(kk)map .28) (7. z(k). xv (kk)) x(kk) g(x(k).4: Evolution of a map over time. The prediction model for the state is trivial.Feature Based Estimation for Mapping and Localising 76 the concatenation of all point features.26) (7. Note how the ﬁlter converges to a perfect map because no process noise is added in the prediction step Initially the map is empty and so we need some method to add “newly discovered’ features to it. longer state vector with the new feature at its end. xv (kk)) x(kk) = xv + r cos(θ + θv ) yv + r sin(θ + θv ) = (7. vehicle trajectory decreasing feature uncertainty Figure 7.29) 76 . For the case of a range bearing measurement we would have the following: x(kk)∗ = y(x(kk). The observation equation is the same as for the localisation case only now the feature coordinates are the free variables. z(k). Covariance ellipses for each feature are plotted in the z = k planes.27) (7. The vertical axis is time (k). To this end we introduce a feature initialisation function Y that take as arguments the old state vector and an observation to a landmark and returns a new.
Of course we can use the jacobian of the transformation: P(kk)∗ = where Yx. Therefore: P(kk)∗ = P(kk) 0 0 Gz R Gz T (7. Note that as no process noise is ever added to the system the uncertainty in feature locations after initialisation is always decreasing.5. z(k).z = Yx.z T (7.33) Now for the mapping case Gx = 0 as the new feature is not dependent on any element in the state vector (which only contains features). The code used to generate this simulation for feature based mapping can be downloaded from the course website. You might think it odd that in the limit the map becomes perfectly known.2 shows the evolution of the map over time.Feature Based Estimation for Mapping and Localising 77 where the coordinates of the new feature are given by the function g: xf new = g(x(k). You might think that intuitively there should always be some residual uncertainty.30) (7.z P(kk) 0 0 R Yx. The point is that the 77 .31) We also need to ﬁgure out how to transform the covariance matrix P when adding a new feature.35) Figure 7. xv (kk)) =( xv + r cos(θ + θv ) yv + r sin(θ + θv ) (7.34) One thing to realise is that the observation Jacobian is now “long and thin”. When observing feature i it is only nonzero at the “location” (indexes) of the feature in the state vector: Hx = otherf eatures observedf eature otherf eatures ···0··· H xﬁ ···0··· (7. In the limit the map will be known perfectly.32) In×n 0n×2 Gx Gz (7.
Simultaneous Localisation and Mapping  SLAM
78
vehicle is continually being told where (via the superGPS) it is and is not changing its position estimate as a function of landmark observations. As a consequence all features are independent (check this by examining the P matrix –it is block diagonal) and each observation of them simply adds information and therefore reduces their uncertainty  again and again and again.... This is in contrast to the next section where landmark observations will be allowed to adjust map and vehicle estimates simultaneously!
7.6
Simultaneous Localisation and Mapping  SLAM
SLAM is the generalised navigation problem. It asks if it is possible for a robot, starting with no prior information, to move through its environment and build a consistent map of the entire environment. Additionally the vehicle must be able to use the map to navigate (localise) and hence plan and control its trajectory during the mapping process. The applications of a robot capable of navigating , with no prior map, are diverse indeed. Domains in which ’man in the loop’ systems are impractical or diﬃcult such as Martian exploration, subsea surveys, and disaster zones are obvious candidates. Beyond these, the sheer increase in autonomy that would result from reliable, robust navigation in large dynamic environments is simply enormous. Autonomous navigation has been an active area of research for many years. In SLAM no use is made of prior maps or external infrastructure such as GPS. A SLAM algorithm builds a consistent estimate of both environment and vehicle trajectory using only noisy proprioceptive (e.g., inertial, odometric) and vehiclecentric (e.g., radar, camera and laser) sensors. Importantly, even though the relative observations are of the local environment, they are fused to create an estimate of the complete workspace. The SLAM problem is of fundamental importance in the quest for autonomous mobile machines. It binds aspects of machine learning, uncertainty management, perception, sensing and control to enable a machine to discover and understand its surroundings with no prior knowledge or external assistance. This section will introduce a simple feature based approach to the SLAM problem. It doesn’t work in real life for deployments in large areas because it involves running a Kalman ﬁlter to estimate the entire map and vehicle state. The update of the covariance matrix is therefore at best proportional to the square of the number of features. Given enough features the system will grind to a halt. Figure 7.6 shows some of the kind of area that can be mapped and localised in using this technique.
78
Simultaneous Localisation and Mapping  SLAM
79
Figure 7.5: SLAM in a real environment. These ﬁgures are screen shots from an experiment using a B21 robot in a hallway at MIT. Having built the map the vehicle used it to navigate home to within a few cm of its start position. Videos can be found at ¡http://www.robots.ox.ac.uk/∼pnewman : teaching¿. You’ll be pleased to know we have pretty much done all the work required to implement a full SLAM algorithm. We will still employ the oracle that tells us which feature is being seen with each observation. All we do now is change our state vector to include both vehicle and map: xv xf ,1 . . f ,n .x
x
(7.36)
Of course to start with n = 0 but as new features are seen the state vector is grown 79
Simultaneous Localisation and Mapping  SLAM
80
just as in the pure mapping case. The diﬀerence now is that the observation and feature initialisation jacobians have two nonzero blocks. one with respect to the vehicle and one with respect to the observed feature.
Hx = Yx,z = =
In×n 0n×2 Gx Gz In×n G xv · · · 0 · · · 0n×2 Gz
H xv · · ·
Hxf ,1
(7.37) (7.38) (7.39)
Also note that the jacobian of the prediction models have changed in an obvious way: Fx = = Fu = F xv 0 I2n×2n 0 J1(xv , u) 0 0 I2n×2n J2(xv , u) 0 0 02n×2n (7.40) (7.41) (7.42) (7.43) because the prediction model is now: xv (k) ⊕ u(k) xv (k + 1) xf ,1 (k) xf ,1 (k) = . . . . . . xf ,n (k) xf ,n (k)
(7.44)
Note that the features are not expected to move between time steps (pretty much what you would hope for when using a map!) and they are noiseless. Only the vehicle has process noise injected into its covariance matrix during a prediction step. The matlab code for an EKF, feature based SLAM algorithm can be found on the course website. You should be able to recognise it as a union of previous localisation and mapping examples. Figure 7.6 shows a few snap shots of the algorithm running.
80
SLAM 81 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 −50 0 50 100 −50 0 50 100 −50 0 50 100 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 −50 0 50 100 −50 0 50 100 −50 0 50 100 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 100 80 60 40 20 0 −20 −40 −60 −80 −100 −100 −50 0 50 100 −50 0 50 100 −50 0 50 100 7. P= Pvv Pvm PT Pmm vm (7. Its pretty clear that there should be correlations between map and vehicle as they are so interrelated. It’s as though they are all tied up together with elastic bands . Consider the following sequence of events.3 — correlations cause adjustments in one state to ripple into adjustments in other states.1 The role of Correlations Note that the covariance matrix now has some structure to it you can partition map P mm and vehicle Pvv blocks.1.pulling at one will pull at the others in turn. This is of course also true in this kind of approach to the SLAM problem.Simultaneous Localisation and Mapping . 81 . Remarkably every observation of a feature aﬀects the estimate of every other feature in the map. From the moment of initialisation the feature’s location is a function of vehicle location and so errors in the vehicle location will also appear as errors in feature location. Now recall the discussion regarding correlations in section 5.6.45) Of diagonals Pvm are the correlations between map and vehicle.
robotics and machine learning communities.. This makes sense.SLAM 82 There are some further characteristics of the SLAM problem that transcend the estimation method being used. The best you can hope for is to reduce all features to the lower bound your initial uncertainty. if you start with say 10m of uncertainty relative to Carfax tower mapping and reobserving things within the Thom building is never going to reduce your overall uncertainty. Compare this to the ever decreasing uncertainty of features in the mapping only case. If you want to ﬁnd out more just ask. The SLAM problem is a very topical problem and is attracting interest from the AI. 82 . • The feature uncertainties never increase but the vehicle uncertainty can. Furthermore the map has a noiseless model and so the prediction step does not inﬂate the covariance of the map..Simultaneous Localisation and Mapping . You should be able to check that they are true by running the example code: • The feature uncertainties never drop below the initial uncertainty of the vehicle..we don’t expect it to move. The prediction model is the identity matrix for the map .
Much use has been made of the Kalman ﬁlter which estimates the mean and covariance of a pdf which under gaussian noise assumptions is the MMSE estimator. Collectively these thousands of possible states and their scores deﬁne the pdf we are seeking to estimate.1 Montecarlo Methods . The Kalman ﬁlter is an important tool (and has great value outside of this course) but it is not the whole story. one of two schemes are adopted. We maintain lots of diﬀerent versions of the state vector — all slightly diﬀerent from each other.Topic 8 Multimodal and other Methods The previous discussion on Estimation. The single EKF approach we have talked about so far does not really behave well in large environments . The ﬁrst partitions the world into local maps and uses unimodal methods (EKF) within each map. We never have to make the assumption of Gaussian noise or perform 83 . We then make copies of the best ﬁtting vectors and randomly perturb them (the equivalent of adding Q in the EKF) to form a new generation of candidate states. Instead. montecarlo methods are used. Secondly unimodal methods are abandoned altogether.the big problem is that linearisation errors (that came from the Taylor series approximations in deriving the EKF) compound and the linearisation starts to occur around points that are far from the true state. There is neither the space nor time to cover in detail how this can be overcome.Particle Filters The basic idea is simple. Broadly though. Mapping and Localisation has focused almost exclusively on estimating unimodel pdfs. 8. When a measurement comes in we score how well each version explains the data. The maps are then carefully glued back together to form a uniﬁed global map.
1 shows this process graphically for the localisation problem. Goto 2 Figure 8.Montecarlo Methods . Apply the plant model to each particle. Well technically it is the distribution of particles themselves which represents a pdf. the mode. One solution to this would be to copy from this “winning set” until we have n particles again. The trick often played is to be smart about selecting which samples should be copied between timesteps. In contrast to the prediction step in the Kalman approach we actually inject the process noise as well — i. This may turn out to be a good plan because they may do a better job for the next observation. This will be a scalar L which is associated with each particle. 5.Particle Filters 84 a linearisation. The PFLocalisation algorithm proceeds as follows: 1. For a 2D vector we would require something like 1002 . This has a hidden consequence though: it would artiﬁcially reduce the diversity (spread) of the particle set. This is in some ways analogous to adding GUGT to the predicted covariance in the Kalman approach because it increases the variance of the estimate (in this case the particle set). A common question is “what is the estimate?”. This is a very current area of research. for 3D 1003 and so on. If you want a single vector estimate. 3.the mean is somewhere between them all in a location with no support1 ! As with all the algorithms however soon the clouds should coverge into one large cloud as the motion of the vehicle disambiguates its location 1 84 . This means that there is a ﬁnite chance that particles that really didn’t explain the observation well. Use a likelihood function to evaluate the likelihood of the observation given the state represented by each particle. 4. The big problem with this elegant approach is the number of samplestates (versions) we need to try out increase geometrically with the number of states being estimated. Initialise n particles in a map. mean and median are all viable options. Select the particles that best explain the observation. will be reselected. If you run the code yourself initially you will see distinct clouds of particles representing diﬀering modes of the pdf . This scalar is referred to as a “weight” or “importance”. Instead we randomly sample from the samples biased by the importance values. Compare this to the measured value. For example to estimate a 1D vector we may keep 100 samples. However you need to think carefully.e we add a random vector to the control vector u. One way to do this would be to simply sort and select the top candidates based on L but this would reduce the number of particles. Each particle is a 3 by 1 state vector of the vehicle 2. For each particle predict the observation.
Perhaps the biggest problem of grid based methods stems from the diﬃculties in localising with respect to a grid. We then store and estimate (using further measurements) this sparse parameterisation (often by stacking all the feature parameters in to a long state vector and calling it x ).edges. We make no assertions as to what is occupying it just that something or part of something is at the coordinates covered by cell(40. It is particularly well suited for problems with small state spaces .2 shows one possible set of functions for a widebeam inair sonar. As the sensor (mounted on a vehicle) moves around the environment and if the location of the sensor is known a grid based map can be built.one to express the likelihood of a cells within the observed region being empty and one to express the likelihood of them being occupied. 2) = 1 we are sure that the cell at (40. Each cell c(i. lines etc. We use two functions to model the behaviour of the sensor . However cunning data structures can overcome this.it becomes expensive for 3D maps. One problem with grid based methods is memory usage .po (i. A crucial point is that it does not require any linearisation assumptions (there are no jacobians involved) and there are no Gaussian assumptions. A mesh of cells (a grid) is laid over the (initially unknown) world.2 shows the occupancy and empty maps for a map built with the CMU Terragator vehicle in ﬁgure 8. the particle ﬁlter approach is very elegant and very simple to implement (see the code on the website). 8. In contrast grid based approaches tend not to reason about the geometry of features in the environment but take a more sensorcentric view.Grid Based Mapping 85 in the course you are strongly advised to down load the code (MCL. Figure 8. j) has a number associated with it corresponding to an occupancy probability . It requires trying to match a small sensor centric map with 85 .2 Grid Based Mapping Up until now we have adopted a feature based approach to navigation.20). points. So for example if po (40. Figure 8. We assumed that the environment contains features possessing some geometry that can be extracted by suitable processing of sensor data .m) and peruse it to understand the translation from concept to implementation.20) is occupied.2. In summary.it is arbitrary and as such cells are likely to cover areas that are awkwardly partially full as such is not possible to capture the sharp discontinuities at the edges of objects. For each cell under the beam a Bayes rule update is used to ﬁgure out the new probability of a cell being empty or being occupied. A second issue is that of grid alignment . j) and a probability of being empty pe .the example code has a state dimension of three.
Obs Prediction p(zx i ) 3. The best particles are those that best explain the actual range measurement. These are preferentially choosen to form the next generation.u(k). Fitness Evaluation L i = f(zh(x i)) 4 3 21 5 6 7 zh(x i ) obspred cumulative importance ( ΣL i) uniformly choosen random numbers 4. 86 . Regeneration 4 updated sample set 2 33 3 x3 Figure 8. Selection particle #copies 1 1 2 1 3 3 4 1 5 1 6 0 7 0 1 5 5. Plant Model x i = f(x i.1: Graphical interpretation of a particle ﬁlter. The method is general but this diagram can be easily understood in terms of a rangetobeacon sensor (like the long baseline acoustic sensor mentioned in the Least Squares section).Grid Based Mapping 86 1.v(k)) 1 5 2 2 6 4 3 7 3 6 4 7 1 5 motion + perturbation starting sample set Predicted Observations 1 5 Feature Actual Observation 2 6 4 7 3 h(x i) 2.
The sensor modelled here is a wide beam sonar that sends out a cone of sound and measures the time between transmit and echo reception. The CMU terragator vehicle used grid based mapping in various settings.2: From the early days of mobile robotics. The actual object is at distance R from the sensor. Figure 8.3: The shape of two functions used to describe the probability Pe (r) of empty cells at a range r from the sensor and the probability of them being occupied po (r).Grid Based Mapping 87 Figure 8. 87 .
Grid Based Mapping 88 Figure 8. 88 . Each map is maintained using a diﬀerent likelihood function a large global map.4: The two typical occupancy maps built by a grid based approach. The left map show the probability of regions being occupied and the left the probability of regions being empty. The vehicle can be localised by rotating and translating the local map (what is seen by the sensor) until the best match is found.
Perhaps in a decade we will have cars that avoid crashes. goods trucks that drive themselves. However if we don’t know what the world looks like how can we be sure it is bad data and not faithful sensing of an unexpected world state? I also hope you have found the discussions on mobile robotics thought provoking.(but it would be useful).. an alwaysdeployed ﬂeet of ocean vehicles monitoring ElNino currents.Topic 9 In Conclusion Hopefully you have found this introductory course interesting and have picked up a sense that the tools we have been using are applicable to many domains. This is a crucial point — we need to decide whether the data is even worth processing. P. We don’t need the “fullmonty” of AI to be able to make a big diﬀerence. 89 . smart machines on Mars and robots that massively increase the independence and mobility of our old and inﬁrm. In may situations sensors return grossly erroneous data (outliers).M..N October 2003. If this were a 10 lecture course we would have spent more time considering how to appraise in realtime the validity of sensor data..
Topic 10 Miscellaneous Matters This section is meant to help you with the class work and covers things that you may need to know but that don’t really ﬁt in the main body of the notes 10. So we have xT P−1 x = 1 (10.2) we can use eigenvalue decomposition to factorise P as VDV T where V is some rotation matrix and D is a diagonal matrix of principal values xT (VDVT )−1 x = 1 90 (10. We can disregard the vector µ x as it simply shifts the distribution.3) . It is really useful to be able to plot this contour to illustrate uncertainties in 2D.1 Drawing Covariance Ellipses The ndimensional multivariatenormal distribution has form 1 1 exp{− (x − µx )T P−1 (x − µx )} 1/2 P 2 p(x) = (2π)n/2 (10.1) The exponent (x − µx )T P−1 (x − µx ) deﬁnes contours on the bell curve. A particularly useful contour is the “1 − σ bound” where (x − µx )T P−1 (x − µx ) = 1.
8) (10. yT y = 1.5) (10. This makes sense.1: A bivariate normal distribution.Drawing Covariance Ellipses 91 p(x. so xT KKT x = yT y K x=y ⇒ x = VD 1/2 T (10. intuitively the variance is in “units squared”— D1/2 is a diagonal matrix of standard deviations .y) Y X Figure 10. The ﬁst thing we do is scale the unit circle by these factors.6) D −1/2 V x=1 T xT KKT x = 1 where K = VD−1/2 .the semimajor and semiminor axes of the ellipse.7) (10. The thick contour is the 1 − σ bound where (x − µx )T P−1 (x − µx ) = 1 . now for any point y = [x. using the fact that V is orthonormal: xT VD−1 VT x = 1 x VD T −1/2 (10. Then we rotate the ellipse by V — recall that the correlations between x and y (oﬀ diagonals in P ) induce a rotation in the ellipse? 91 .9) y (10. y]T which is on the unit circle.10) So to draw an ellipse described by a 2 × 2 covariance matrix P we take a whole bunch of points on the unit circle and multiply them by VD1/2 and plot the resulting points.4) (10.
Drawing Covariance Ellipses 92 2σ xx α 2σ yy Figure 10. P= 2 2 σxx σxy 2 2 σxy σyy (10.12) 2 2σxy tan 2α = 2 2 σxx − σyy 92 .1 shows the relation ship between P and the plotted form for a bivariate normal distribution.2: The relationship between geometry of the 1 − σ bound for a bivariate normal distribution and its covariance matrix.11) (10. Figure 10.
σc11 2 σc12 2 .14) P= . . . . Obviously it is hard to draw a high dimensional gaussian on a screen or paper.Drawing High Dimensional Gaussians 93 10. Pcc . σb11 2 σb12 2 . For example we may have a covariance matrix that corresponds to a large state vector: a b (10.2 Drawing High Dimensional Gaussians So to plot a 2D representation of the ith entity in a state vector simply plot the ellipse for the ith 2 by 2 block in P . . 2 2 σc21 σc22 93 .13) x= c σa11 2 σa12 2 ··· ··· σa21 2 σa22 2 Paa marginal of component b (10. Pbb ··· 2 2 σb21 σb22 .
Params.V0+Params. global Params.clear all .global VehicleStatus.[0]. Newman 2003 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− %−−−−−−−−− TOP LEVEL LOOP −−−−−−−−−−−−−−−−−−−−−% function SimulateMarsLander close all .1 Matlab Code For Mars Lander Example %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− % A MARS LANDER −−− EXAMPLE CODE C4 % P. PEst = diag([Params. k = 2.[0]. while(˜VehicleStatus.StopTime/Params. NaN).dT) % simulate the world DoWorldSimulation(k).Params. 94 .XEst. %store initial conditions : DoStore(1.InitialVelocityError].X0+Params.PEst .InitialVelocityStdˆ2]).Landed & k <Params. %set up parameters DoInitialise . % initial conditions of estimator XEst = [Params.InitialHeightError.global Store.global XTrue.Topic 11 Example Code 11.InitialHeightStdˆ2.
dT = 0.%run for how many seconds (maximum)? Params. VehicleStatus..c light = 2. %when to open chute Params. %uncertainty in initial conditions Params. return.PEst.InitialHeightStd = 100.5∗Params. global VehicleStatus.Innovation] = DoEstimation(XEst.998e8.X0 = 10000.8/3.ChuteOpen = 0.z(k)). % true entry velocity Params...Matlab Code For Mars Lander Example 95 % read from sensor z(k) = GetSensorData(k).RocketsOn = 0.StopTime = 600. %−−−−−− user conﬁgurable parameters −−−−−−−−−−−−− Params.XEst.EntryDrag = 5. Params. Params. % error on entry velocity Params.InitialVelocityError = 0. VehicleStatus. % linear drag constant with chute open Params. % linear drag constant Params. k = k+1. global Store. DoMarsGraphics(k). % when to turn on brakes Params.z(k)).InitialHeightError = 0. % assumed gravity on mars Params.PEst.g = 9. % Run navigation at 10Hz Params.EntryDrag. end. % estimate the state of the vehicle [XEst. % mass of vehcile Params.1. %tick . % store results DoStore(k. %−−−−−−−−− PROBLEM SET UP AND INITIALISATION −−−−−−−−−−−−−−−−−−−−−% % users changes parameters here function DoInitialise global Params. % true entry height Params.PEst.S. % initial vehicle condition at entry into atmosphere. % error on entry height Params. VehicleStatus.RocketBurnHeight = 1000. % make decisions based on our esimated state DoControl(k. %uncertainty in initial conditions Params.LandingTime = NaN. VehicleStatus. 95 .Innovation..BurnTime = NaN.ChuteTime = NaN.Thrust = 0. %draw pictures .S..EntryDrag.InitialVelocityStd = 20. global XTrue. Params.XEst).V0 = 0..OpenChuteHeight = 4000..m = 50.ChuteDrag = 2.Drag = Params.
. z = Params.Matlab Code For Mars Lander Example 96 VehicleStatus.SigmaR∗ randn(1)..k) + Params. %process noise model (maps acceleration noise to other states ) Params.G. %(msˆ2 std) %observation model (explains observations in terms of state to be estimated) Params.G = Params. %−−−−−−−−−−−−−−− ESTIMATION KALMAN FILTER −−−−−−−−−−−−−−−% function [XEst. Innovation = z−H∗XPred.R.R = (1. global Params. %actual process noise truely occuring − atmospher entry is a bumpy business %note this noise strength − not the deacceleration of the vehicle .F.dTˆ2/2 . % initial conditions of (true) world: XTrue(:. Params.2.SigmaR = 1. 0 1].Params.V0]. % prepare for update. %observation expected noise strength (we never know this parameter exactly) %set the scale factor to 1 to make model and reallity match Params..SigmaQ = 0.dT].R = Params...Params.H = [2/Params.1∗Params.Q = Params. %−−−−−−−−−−−−−−−−−− MEASUREMENT SYSTEM −−−−−−−−−−−−−−−−−−% function z = GetSensorData(k) global XTrue. PPred = F∗PEst∗F’+G∗Q∗G’.PEst.H = Params. W = PPred∗H’∗inv(S).SigmaR)ˆ2. XPred = F∗XEst. %observation noise strength (RTrue) is how noisey the sensor really is Params.X0.Q.Innovation] = DoEstimation(XEst.S..H.. S = H∗PPred∗H’+R.1∗Params.c light 0]. %prediction ..SigmaQ)ˆ2.Q = (1.H∗XTrue(:. F = Params.Landed = 0. %(seconds) 3.1) = [Params..PEst.z) global Params. %msˆ{−2} %process noise strength how much acceleration (varinace) in one tick % we expect (used to ’explain ’ inaccuracies in our model) %the 3 is scale factor ( set it to 1 and real and modelled noises will %be equal Params.dT..G = [Params. %process plant model (constant velcoity with noise in acceleration ) Params.0e−7 corresponds to around 50m error. Params return.F = [1 Params. return. 96 .3e−7.
97 .Drag.m∗Params. newvel = oldvel+acc∗dT. dT = Params. return.ChuteDrag.dT.ChuteTime = k∗Params.XEst) global Params..global VehicleStatus.global VehicleStatus.RocketBurnHeight ) if (˜VehicleStatus. oldvel = XTrue(2.BurnTime = k∗Params. Params. XEst = XPred+W∗Innovation. %−−−−−−−−−−−−−−− LANDER CONTROL −−−−−−−−−−−−−−−−−−−−−−−−−% function DoControl(k.. fprintf(’Opening Chute at time %f\n’. % spatial exponential factor for atmosphere density) AtmospherDensityScaleFactor = (1−exp(−(Params. PEst = PPred−W∗S∗W’. end.X0−oldpos)/cxtau) ).dT.Thrust)/Params.global Params.ChuteOpen) %open parachute: VehicleStatus.m + Params.dT).RocketsOn = 1.k∗Params.k) = [newpos. oldpos = XTrue(1. Params.ChuteOpen = 1.k∗Params.k−1). c = AtmospherDensityScaleFactor∗VehicleStatus. if (XEst(1)<Params.g+VehicleStatus. %simple Euler integration acc = (−c∗oldvel− Params.Drag = Params.k∗Params. %friction is a function of height cxtau = 500. if (XEst(1)<Params. %clamp between 0 and c for numerical safety c = min(max(c.0).k−1).Matlab Code For Mars Lander Example 97 % do update. end.RocketsOn) fprintf(’Releasing Chute at time %f\n’.Drag). XTrue(:.newvel].dT). VehicleStatus.VehicleStatus.dT). %−−−−−−−−−−−−−−− ITERATE SIMULATION −−−−−=−−−−−−−−−−−−−−−% function DoWorldSimulation(k) global XTrue.OpenChuteHeight & ˜VehicleStatus. fprintf(’ Firing Rockets at time %f\n’.. %turn on thrusters VehicleStatus. newpos = oldpos+oldvel∗dT+0.dT.SigmaQ∗randn(1).5∗acc∗dTˆ2.
k∗Params. end.2.S diag(S)]. return. VehicleStatus.S.g.Drag = 0. %3cm/s std on speed SigmaPhi = 4∗pi/180.XEst = XEst.Innovation Innovation].Innovation = [ Store.0.PEst = diag(PEst). if (XEst(1)<1) %stop when we hit the ground.Innovation = Innovation. xtrue = x. return.%length of run L = 2.99∗Params. else Store.z = [Store. Store.PEst diag(PEst)]. % steer inaccuracy % initial knowledge pdf (prior @ k = 0) P = diag ([0.Innovation. if (k==1) Store.2. Store.Matlab Code For Ackerman Model Example 98 %drop chute. x = [0.XEst = [Store.1. Store. Q = diag([SigmaVˆ2 SigmaPhiˆ2]). VehicleStatus..m∗Params. 11.z) global Store.S = S.dT.2 Matlab Code For Ackerman Model Example function AckermannPredict clear all .LandingTime = k∗Params.0]).. fprintf(’Landed at time %f\n’. %simple littel controller here (from vˆ2 = uˆ2+2as) and +mg for weight of vehicle VehicleStatus.m∗XEst(2)ˆ2−1)/(2∗XEst(1))+0.PEst. %−−−−−−−−−−−−−−− MANAGE RESULTS STORAGE −−−−−−−−−−−−−−−−−−−−−−% function DoStore(k.z = z.Landed = 1.S = [Store. 98 . Store. dT = 0. close all ..XEst XEst].0].Thrust = (Params. end. Params.XEst.%length of vehicle SigmaV = 0.z z ].%time steps size nSteps = 600. Store. Store. Store.1. end.dT). Store.PEst = [Store. break.0.
title ( ’uncertainty bounds for Ackermann model’).grid on. y(3.hold on.Matlab Code For Ackerman Model Example 99 %−−−−−−−− Set up graphics −−−−−−−−−−−% ﬁgure(1). eH = line(el (1.u+[SigmaV .sin(0:0. %prediction steps P = JacFx ∗ P ∗JacFx’ + JacFu∗Q∗JacFu’. y = nSigma∗[cos(0:0. 99 .∗randn(2. 0 0 1].’r’).1) = x(1) + dT∗u(1)∗cos(x(3)).2)+1). ’ko’ ). %−−−−−−−− Main loop −−−−−−−−−−−% for(k = 1:nSteps) %control is a wiggle at constant velocity u = [1. dT∗sin(x(3)) 0.1:2∗pi )].:)).nSigma) P = P(1:2.SigmaPhi].pi/5∗sin(4∗pi∗k/nSteps)].col). %−−−−−−−− Drawing Covariance −−−−−% function eH = PlotEllipse(x.D] = eig(P).plot(xtrue(1).1) = x(3) + dT∗u(1)/L∗tan(u(2)). ylabel(’y’ ). xtrue = AckermannModel(xtrue. print −deps ’AckermannPredict. el = [ el el (:. % only plot x−y part x = x(1:2). % percentage of axes size a=axis.1) = x(2) + dT∗u(1)∗sin(x(3)). 0 1 dT∗u(1)∗cos(x(3)). if (˜any(diag(P)==0)) [V.u.P .u.0. %−−−−−−−− Drawing Vehicle −−−−−% function DrawRobot(Xr. dT∗ tan(u(2))/L dT ∗u(1)∗sec(u(2))ˆ2]. y(2.axis equal.size(el.1:2).1)]+ repmat(x.L).:).L). el = V∗sqrtm(D)∗y. el (2.axis([−20 20 −5 25]) xlabel(’x’ ).dT.1:2∗pi).1. p=0. %calculate jacobians JacFx = [1 0 −dT∗u(1)∗sin(x(3)).xtrue(2). end. l2=(a(4)−a(3))∗p. x = AckermannModel(x. %draw occasionally if (mod(k−1. l1=(a(2)−a(1))∗p.02. DrawRobot(x.5).dT.eps’%save the picture %−−−−−−−−−−−− MODEL −−−−−−−−−−−−−−% function y = AckermannModel(x.1).dT.L) y(1.30)==0) PlotEllipse (x.P. JacFu = [dT∗cos(x(3)) 0. end. end.
s c]∗P.ˆ2. UTrue = diag([0.nSteps). RTrue = diag([2.30)−70.Xr(2).global UTrue.nSteps). set(hObsLine.1.:). PEst = diag([1.global Map. s=sin(theta).:).[0.nSteps).global nSteps. xOdomLast = GetOdometry(1). ’doublebuﬀer’ .0∗UTrue.1). UEst = 1.nSteps).:).:)=P(2. ’ :’ ). set(gcf. hold on. SStore = NaN∗zeros(2.01. hold on.:).:)∗l1+Xr(1). global xTrue. clear all .01.sprintf(’%s+’. XStore = NaN∗zeros(3.0]). P=[c −s.’ linestyle ’ . 100 . % initial graphics ﬁgure (1).0.0. ’on’ ).Matlab Code For EKF Localisation Example 100 P=[−1 1 0 −1. col . hObsLine = line ([0.global RTrue.% draw plot(Xr(1).(1∗pi/180)ˆ2]).Map(2.:)∗l2+Xr(2). axis equal. Map = 140∗rand(2.3∗pi/180]). %ﬁgure out control xOdomNow = GetOdometry(k).:)=P(1.ˆ2.0]. nSteps = 6000.0∗RTrue.0. % initial conditions : xEst =xTrue. PStore = NaN∗zeros(3.%basic triangle theta = Xr(3)−pi/2.col)). H = plot(P(1.3 Matlab Code For EKF Localisation Example function EKFLocalisation close all . %scale and shift to x P(2. XErrStore = NaN∗zeros(3.nSteps). 11. for k = 2:nSteps %do world iteration SimulateWorld(k). ’LineWidth’.%rotate to point along x axis (theta = 0) c=cos(theta).P (2.−40.−pi/2]. grid oﬀ . −1 −1 3 −1].’g∗’ ). xTrue = [1. %%%%%%%%% storage %%%%%%%% InnovStore = NaN∗zeros(2. REst = 1. %rotate by theta P(1.1∗pi/180]). plot(Map(1.
Map).Map(1. S = jH∗PPred∗jH’+REst. W = PPred∗jH’∗inv(S).Matlab Code For EKF Localisation Example 101 u = tcomp(tinv(xOdomLast). k) = sqrt(diag(S)).PEst (1:2.k) = xTrue−xEst. %do prediction xPred = tcomp(xEst.[0. XStore(:.[xEst(2).u)∗ PEst ∗J1(xEst.’XData’. Innov(2) = AngleWrap(Innov(2)). PEst = 0.iFeature)]).iFeature)]). %do Kalman update: Innov = z−zPred. else %Ther was no observation available xEst = xPred.xOdomNow).Map).iFeature. S = NaN∗eye(2).1]).[xEst(1). % get observation Jacobian jH = GetObsJac(xPred.iFeature. xOdomLast = xOdomNow.u)∗ UEst ∗ J2(xEst.1:2).k) = sqrt(diag(PEst)).k) = Innov. if (˜isempty(z)) %predict observation zPred = DoObservationModel(xPred.u)’ + J2(xEst.u).’YData’. xEst(3) = AngleWrap(xEst(3)). PEst = PPred. iFeature] = GetObservation(k).NaN].5∗(PEst+PEst’). xPred(3) = AngleWrap(xPred(3)). PPred = J1(xEst. if (˜isempty(z)) set(hObsLine. %observe a randomn feature [z.8. PStore(:. drawnow. XErrStore(:.k) = xEst. end. end.u)’. if (mod(k−2. %store results : InnovStore(:. %note use of ’Joseph’ form which is numerically stable I = eye(3). xEst = xPred+ W∗Innov.300)==0) DoVehicleGraphics(xEst.Map(2. 101 . end. Innov = [NaN. set(hObsLine. SStore (:. PEst = (I−W∗jH)∗PPred∗(I−W∗jH)’+ W∗REst∗W’.
:)∗180/pi). iFeature = −1. z = [norm(Delta).1∗nSteps) z = [].plot(InnovStore (1.hold on. z(2) = AngleWrap(z(2)).:)∗180/pi). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 102 .SStore. print −depsc ’EKFLocation.PStore. %fake sensor failure here if (abs(k−nSteps/2)<0.plot(3∗PStore(1. xlabel(’time’).’r’). end.:)).plot(−3∗PStore(3.1.:).eps’ ﬁgure(2).XErrStore). plot(−SStore(1.:). subplot(3. ’ r ’ ).1). hold on.hold on.’r’ ) ylabel(’y’ ).’r’) ylabel(’\theta’ ).:). ylabel(’range’).3).2)∗rand(1)).plot(3∗PStore(2. z(2) = AngleWrap(z(2)).1).:)∗180/pi.plot(XErrStore(2.hold on. iFeature.eps’ ﬁgure(2).plot(InnovStore(2.:)∗180/pi. subplot(3.’r’ ).iFeature)−xVeh(1:2). z = DoObservationModel(xTrue.xlabel(’time’).plot(SStore(1.XStore.plot(XErrStore(3.u).hold on.XStore. xTrue(3) = AngleWrap(xTrue(3)). title ( ’Covariance and Error’). else iFeature = ceil(size(Map.2). print −depsc ’EKFLocationInnov.:).Map)+sqrt(RTrue)∗randn(2.’ r ’ ) title ( ’Innovation’ ).2).Map) Delta = Map(1:2.global nSteps.ylabel(’x’). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [z] = DoObservationModel(xVeh.’r’) ylabel(’Bearing (deg)’).iFeature] = GetObservation(k) global Map. plot(−SStore(2.:)). subplot(2.:).XErrStore) ﬁgure (1).1.plot(XErrStore(1.plot(3∗PStore(3.:)∗180/pi.1. plot(−3∗PStore(2. plot(−3∗PStore(1.global xTrue. DoGraphs(InnovStore.1).1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function SimulateWorld(k) global xTrue. subplot(2. xTrue = tcomp(xTrue.Delta(1))−xVeh(3)]. subplot(3.Matlab Code For EKF Localisation Example 102 end. atan2(Delta(2).1.’r’ ).global RTrue. print −depsc ’EKFLocationErr.PStore.’r ’ ).:)∗180/pi.SStore.’r ’ ).eps’ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [z. u = GetRobotControl(k).:)). iFeature.:).plot(SStore(2. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function DoGraphs(InnovStore.
%internal to robot low−level controller global UTrue. end.Matlab Code For EKF Mapping Example 103 function jH = GetObsJac(xPred. 0. jH(1. nSteps = 600. RTrue = diag([8.Map) jH = zeros(2.1) = Delta(2) / (rˆ2).7∗pi/180]). xnow =tcomp(LastOdom. clear all . %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function u = GetRobotControl(k) global nSteps. MapSize = 200. jH(1. xnow = tcomp(xnow.3).0∗RTrue. r = norm(Delta). PEst = []. jH(2.uNoise).0∗UTrue.1∗ pi/180∗sin(3∗pi∗k/nSteps)]. nFeatures = 6. global xVehicleTrue. 0.15 . LastOdom = xnow. jH(2.1) = −Delta(1) / r. u = [0. 11. uNoise = sqrt(UTrue)∗randn(3. LastOdom = xTrue. REst = 1. 103 .025 . iFeature.−40.3) = −1. Delta = (Map(1:2.01.−pi/2].2) = −Delta(1) / (rˆ2). jH(2. %u = [0.ˆ2. u = GetRobotControl(k).3∗ pi /180]. xVehicleTrue = [1.iFeature)−xPred(1:2)).1∗pi/180]).u).0.2) = −Delta(2) / r.global UTrue. MappedFeatures = NaN∗zeros(nFeatures. 0.ˆ2. UEst = 1. Map = MapSize∗rand(2.global nSteps.2).nFeatures)−MapSize/2. if (isempty(LastOdom)) global xTrue.01. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [xnow] = GetOdometry(k) persistent LastOdom.4 Matlab Code For EKF Mapping Example function EKFLocalisation close all .0.1). 0. UTrue = diag([0.global RTrue. % initial conditions − no map: xEst =[].global Map.
’g∗’ ). hObsLine = line ([0. zPred = DoObservationModel(xVehicleTrue. % get observation Jacobians [jHxv. set(gcf. % initial graphics − plot true map ﬁgure (1). %simple prediction model: xPred = xEst.1). % ﬁll in state jacobian jH = zeros(2. %observe a randomn feature [z. for k = 2:nSteps %do world iteration SimulateWorld(k). XErrStore = NaN∗zeros(nFeatures. % 104 .Matlab Code For EKF Mapping Example 104 %storage: PStore = NaN∗zeros(nFeatures. %note use of ’Joseph’ form which is numerically stable I = eye(size (PEst)). W = PPred∗jH’∗inv(S). PEst = PPred−W∗S∗W’. FeatureIndex:FeatureIndex+1) = jHxf. xFeature = xPred(FeatureIndex:FeatureIndex+1). ’on’ ). %do Kalman update: Innov = z−zPred. S = jH∗PPred∗jH’+REst. if (˜isempty(z)) %have we seen this feature before? if ( ˜isnan(MappedFeatures(iFeature. iFeature] = GetObservation(k).nSteps).:).0].length(xEst)). Innov(2) = AngleWrap(Innov(2)). plot(Map(1. hold on. jH (:. ’ :’ ).1))) %predict observation: ﬁnd out where it is in state vector FeatureIndex = MappedFeatures(iFeature.xFeature).:).jHxf] = GetObsJacs(xVehicleTrue. xEst = xPred+ W∗Innov. axis equal. ’doublebuﬀer’ . PPred = PEst. hold on.0]).xFeature).[0. grid oﬀ .Map(2. set(hObsLine.nSteps).’ linestyle ’ .
nStates) .1). %now draw all the estimated feature points DoMapGraphics(xEst.z).5∗(PEst+PEst’).% note we don’t use jacobian w..k). ﬁgure(2). end.z(1)∗sin(z(2)+xVehicleTrue(3))].PEst. jGz].. end.iL:iL+1)).. end.r. xEst = [xEst. 105 . M = [eye(nStates). %remember this feature as being mapped we store its ID and position in the state vector MappedFeatures(iFeature. fprintf(’k = %d\n’. nStates = length(xEst). length(xEst)]. %Storage: for(i = 1:nFeatures) if (˜isnan(MappedFeatures(i.t vehicle zeros(2.i) = norm(xEst(iL:iL+1)−Map(:.2).40)==0) plot(xVehicleTrue(1). xFeature = xVehicleTrue(1:2)+ [z(1)∗cos(z(2)+xVehicleTrue(3)). else %There was no observation available end.1))) iL =MappedFeatures(i.:) = [length(xEst)−1. XErrStore(k. PStore(k.Matlab Code For EKF Mapping Example 105 % PEst = (I−W∗jH)∗PPred∗(I−W∗jH)’+ W∗REst∗W’. end.xVehicleTrue(2).5). plot(PStore).xFeature]. if (mod(k−2.’r∗’ ). end.REst)∗M’. else % this is a new feature add it to the map. zeros(nStates. PEst = M∗blkdiag(PEst. [jGxv.i) = det(PEst(iL:iL+1. %ensure P remains symmetric PEst = 0.i)). drawnow. jGz] = GetNewFeatureJacs(xVehicleTrue.
jGx = [ 1 0 −r∗sin(theta + bearing). xVehicleTrue = tcomp(xVehicleTrue.1:2).jHxf] = GetObsJacs(xPred.2) = −Delta(2) / r.3∗ pi/180].global nSteps.1). jHxv(2.2)∗rand(1)). %choose a random feature to see from True Map iFeature = ceil(size(Map. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [jHxv.Map(:.global xVehicleTrue. jHxf(1:2. Delta = (xFeature−xPred(1:2)).global RTrue.Delta(1))−xVeh(3)]. jHxv(2. jHxv(1. jHxv(2. 106 .2). %u = [0. u = [0. jGz = [ cos(theta + bearing) −r∗sin(theta + bearing). z = [norm(Delta). z = DoObservationModel(xVehicleTrue.3) = −1. 0.iFeature] = GetObservation(k) global Map. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [jGx. 0. u = GetRobotControl(k). r = norm(Delta). xFeature) jHxv = zeros(2. z). z(2) = AngleWrap(z(2)). 0. z(2) = AngleWrap(z(2)). theta = Xv(3.1).1). jHxv(1. bearing = z(2). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function SimulateWorld(k) global xVehicleTrue. r = z(1). atan2(Delta(2).1) = Delta(2) / (rˆ2). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function u = GetRobotControl(k) global nSteps.jGz] = GetNewFeatureJacs(Xv.1:2) = −jHxv (1:2.3∗ pi/180∗sin(3∗pi∗k/nSteps)]. 0.1) = −Delta(1) / r.jHxf = zeros(2.2) = −Delta(1) / (rˆ2). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [z] = DoObservationModel(xVeh.Matlab Code For EKF Mapping Example 106 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [z.u).3).1).25 . xFeature) Delta = xFeature−xVeh(1:2).iFeature))+sqrt(RTrue)∗randn(2.15 . sin(theta + bearing) r∗cos(theta + bearing)]. x = Xv(1. 0 1 r∗cos(theta + bearing)]. y = Xv(2.
axis(a∗1.nSteps. %change these to alter sensor behaviour SensorSettings. Map = MapSize∗rand(2. MappedFeatures = NaN∗zeros(nFeatures.0.ˆ2. % initial conditions − no map: xEst =[xVehicleTrue]. a = axis.1. REst = 2. xOdomLast = GetOdometry(1).5 Matlab Code For EKF SLAM Example function EKFSLAM close all . UEst = 2.01.’g∗’ ). ’doublebuﬀer’ . hObsLine = line ([0.:).01.0∗RTrue. ’ :’ ).ˆ2.’ linestyle ’ .0∗UTrue. grid oﬀ . RTrue = diag([1.1).FieldOfView = 45. %how often shall we draw? DrawEveryNFrames = 50.nFeatures)−MapSize/2. set(gcf. 107 .0.−pi/2]. ’on’ ). hold on. global SensorSettings. hold on.2). %when to take pictures? SnapShots = ceil(linspace(2.nSteps). MapSize = 200.global Map.global UTrue.1. clear all . SensorSettings.0]).Map(2.global nSteps.nSteps).global RTrue.Matlab Code For EKF SLAM Example 107 11.01]).[0.Range = 100. set(hObsLine.25)). axis equal. % initial graphics − plot true map ﬁgure (1).:). %length of experiment nSteps = 8000. %size of problem nFeatures = 40.5∗pi/180]). xVehicleTrue = [0.1.5∗pi/180]). UTrue = diag([0. XErrStore = NaN∗zeros(nFeatures.0. plot(Map(1. PEst = diag([1.0]. %storage: PStore = NaN∗zeros(nFeatures. global xVehicleTrue.
if (˜isempty(z)) %have we seen this feature before? if ( ˜isnan(MappedFeatures(iFeature. S = jH∗PPred∗jH’+REst. % ﬁll in state jacobian jH = zeros(2. xMap = xEst(4:end).xOdomNow).u)∗ PEst(1:3. iFeature] = GetObservation(k).jHxf] = GetObsJacs(xVehicle. Innov(2) = AngleWrap(Innov(2)). PPredvm’ PPredmm]. xPred = [xVehiclePred.xFeature). xEst = xPred+ W∗Innov..u)∗PEst(1:3. %do prediction (the following is simply the result of multiplying %out block form of jacobians) xVehiclePred = tcomp(xVehicle. zPred = DoObservationModel(xVehicle..u)’.1:3) = jHxv. xOdomLast = xOdomNow.1).xMap]. PPredvm = J1(xVehicle.u). jH (:.1:3) ∗J1(xVehicle.1))) %predict observation: ﬁnd out where it is in state vector FeatureIndex = MappedFeatures(iFeature.4:end). xFeature = xPred(FeatureIndex:FeatureIndex+1). %observe a randomn feature [z. u = tcomp(tinv(xOdomLast). PPred = [PPredvv PPredvm. % get observation Jacobians [jHxv.u)∗ UEst ∗ J2(xVehicle.u)’ + J2(xVehicle. PPredvv = J1(xVehicle.length(xEst)). %ﬁgure out control xOdomNow = GetOdometry(k). PPredmm = PEst(4:end. 108 . FeatureIndex:FeatureIndex+1) = jHxf. W = PPred∗jH’∗inv(S). jH (:.xFeature). %we’ll need this lots .Matlab Code For EKF SLAM Example 108 for k = 2:nSteps %do world iteration SimulateWorld(k). %do Kalman update: Innov = z−zPred.4:end). xVehicle = xEst(1:3).
clf . end.nStates−3) .[0 1]). drawnow. if (˜isnan(z)) h = line([xEst(1).r. axis(a).2).sprintf(’EKFSLAM%d. M = [eye(nStates). end. PlotEllipse (xEst(iF:iF+1).xFeature]. else xEst = xPred. xFeature = xVehicle(1:2)+ [z(1)∗cos(z(2)+xVehicle(3)).1:3). fprintf(’k = %d\n’.Matlab Code For EKF SLAM Example 109 PEst = PPred−W∗S∗W’. if (mod(k−2.PEst (1:3. end.DrawEveryNFrames)==0) a = axis.. end. print(gcf.[ xEst(2). %remember this feature as being mapped we store its ID and position in the state vector MappedFeatures(iFeature. length(xEst)]. plot(xEst(iF). %ensure P remains symmetric PEst = 0. ’ :’ ).eps’. PEst = M∗blkdiag(PEst.3. n = length(xEst).iPic)). end.. DoVehicleGraphics(xEst(1:3). if (ismember(k.’−depsc’.z(1)∗sin(z(2)+xVehicle(3))].5∗(PEst+PEst’). xEst = [xEst.PEst(iF:iF+1.xFeature (2)]).SnapShots)) iPic = ﬁnd(SnapShots==k).3). nF = (n−3)/2. else % this is a new feature add it to the map. PESt = PPred. %augmenting state vector [jGxv. for(i = 1:nF) iF = 3+2∗i−1.% note we don’t use jacobian w. end.xEst(iF+1). ’ linestyle ’ .’b∗’).hold on. end. zeros(nStates. set(h.xFeature (1)]. jGz].t vehicle jGxv zeros(2. jGz] = GetNewFeatureJacs(xVehicle.k)..REst)∗M’.iF:iF+1). 109 . nStates = length(xEst).:) = [length(xEst)−1.z).
xVehicleTrue = tcomp(xVehicleTrue. Trys = 1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [z] = DoObservationModel(xVeh.global SensorSettings done = 0.Map(:.1). z = [norm(Delta).2) = −Delta(1) / (rˆ2).jHxf] = GetObsJacs(xPred. jHxv(2. z =[]. jHxv(1. jGx = [ 1 0 −r∗sin(theta + bearing).3).1).Range) done =1 .FieldOfView∗pi/180 & z(1) < SensorSettings.jHxf = zeros(2. end. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function SimulateWorld(k) global xVehicleTrue. jHxv(1. iFeature = −1.jGz] = GetNewFeatureJacs(Xv.global RTrue. Delta = (xFeature−xPred(1:2)). z(2) = AngleWrap(z(2))..1). r = z(1). %look forward.3) = −1.5∗size(Map.2)∗rand(1)).u). x = Xv(1. y = Xv(2.global nSteps..1:2). else Trys =Trys+1.Delta(1))−xVeh(3)]. bearing = z(2).global xVehicleTrue.iFeature))+sqrt(RTrue)∗randn(2. jHxv(2. z). z = DoObservationModel(xVehicleTrue. z =[]. end.1) = −Delta(1) / r.Matlab Code For EKF SLAM Example 110 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [z.and only up to 40m if (abs(pi/2−z(2))<SensorSettings. jHxv(2.2)) %choose a random feature to see from True Map iFeature = ceil(size(Map. jHxf(1:2. xFeature) Delta = xFeature−xVeh(1:2). iFeature = −1. while(˜done & Trys <0.1:2) = −jHxv (1:2. z(2) = AngleWrap(z(2)).2). atan2(Delta(2). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [jGx.iFeature] = GetObservation(k) global Map. u = GetRobotControl(k).1) = Delta(2) / (rˆ2). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [jHxv. 110 . r = norm(Delta).1). theta = Xv(3.2) = −Delta(2) / r. xFeature) jHxv = zeros(2.
if (˜any(diag(P)==0)) [V.global Map. jGz = [ cos(theta + bearing) −r∗sin(theta + bearing).size(el.1)]+ repmat(x. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function u = GetRobotControl(k) global nSteps.2)+1). sin(theta + bearing) r∗cos(theta + bearing)].global UTrue. LastOdom = xnow.uNoise). end. el = V∗sqrtm(D)∗y.. uNoise = sqrt(UTrue)∗randn(3. y = nSigma∗[cos(0:0.global nSteps. %u = [0.1:2∗pi )].:). u = GetRobotControl(k).1:2∗pi). 11.15 .u).global RTrue. %change this to see how sensitive we are to the number of particle %(hypotheses run) especially in relation to initial distribution ! 111 ...1:2).P. %internal to robot low−level controller global UTrue. % only plot x−y part x = x(1:2). el = [ el el (:. 0.2∗ pi/180].Matlab Code For Particle Filter Example 111 0 1 r∗cos(theta + bearing)].:) ).25 . el (2. nSteps = 6000. function MCL close all . u = [0. %−−−−−−−− Drawing Covariance −−−−−% function eH = PlotEllipse(x. 0.sin(0:0. P = P(1:2. if (isempty(LastOdom)) global xVehicleTrue.6 Matlab Code For Particle Filter Example %Monte−carlo based localisation %note this is not coded eﬃciently but rather to make the ideas clear %all loops should be vectorized but that gets a little matlab−speak intensive %and may obliterate the elegance of a particle ﬁlter .3∗ pi/180∗sin(3∗pi∗k/nSteps)]. xnow = tcomp(xnow. LastOdom = xVehicleTrue. global xTrue.D] = eig(P). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [xnow] = GetOdometry(k) persistent LastOdom. 0. end.nSigma) eH = [].1.1). 0. xnow =tcomp(LastOdom. clear all . eH = line(el (1.
%%%%%%%%% storage %%%%%%%% % initial graphics ﬁgure (1). set(hObsLine. Map = 140∗rand(2. %do prediction %for each particle we add in control vector AND noise %the control noise adds diversity within the generation for(p = 1:nParticles) xP(:.Matlab Code For Particle Filter Example 112 nParticles = 400. REst = 1. for k = 2:nSteps %do world iteration SimulateWorld(k).01. ’doublebuﬀer’ . xOdomLast = xOdomNow.−40.0.xOdomNow). hold on.30)−70.Map(2.:).ˆ2.0∗RTrue. %ﬁgure out control xOdomNow = GetOdometry(k). if (˜isempty(z)) 112 . % initial conditions: − a point cloud around truth xP =repmat(xTrue.:).ˆ2. RTrue = diag([2.1)). set(gcf.1∗pi/180]).p) = tcomp(xP(:. grid oﬀ . ’on’ ).:). plot(Map(1.:) = AngleWrap(xP(3. UEst = 1. %all particles are equally important L = ones(nParticles.u+sqrt(UEst)∗randn(3.01.0].0∗UTrue.−pi/2].xP (2. xTrue = [1. %observe a randomn feature [z. u = tcomp(tinv(xOdomLast).0.p).4])∗randn(3. hObsLine = line ([0.nParticles).:)).0]). hold on.1.’g∗’ ).8.[0.’ linestyle ’ . end.nParticles)+diag([8. ’ . xP(3. xOdomLast = GetOdometry(1).:). ’ :’ ). iFeature] = GetObservation(k).1)/nParticles . UTrue = diag([0. hPoints = plot(xP(1.3∗pi/180]). axis equal.’ ).0.
p).iSelect.global xTrue.:)).:).Map(1. %reselect based on weights: %particles with big weights will occupy a greater percentage of the %y axis in a cummulative plot CDF = cumsum(L)/sum(L). %get likelihood (new importance).1).’YData’.10)==0) ﬁgure(1). xP = xP(:.’extrap’ ).xP (2..’XData’. ﬁgure(2). drawnow. end. end. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [z. end.. set(hPoints.global nSteps.xP(1. set(hPoints.:). L(p) = exp(−0.iFeature.[xEst(2).iFeature)]). %so randomly (uniform) choosing y values is more likely to correspond to %more likely (better) particles .001.’nearest’.5∗Innov’∗inv(REst)∗Innov)+0.’YData’. %our estimate is simply the mean of teh particles xEst = mean(xP.:)). %how diﬀerent Innov = z−zPred.iFeature)])...iFeature] = GetObservation(k) global Map. ’ .’ ). A lower score means %it is less likely to be selected for the next generation ..global RTrue. Assume gaussian here but any pdf works! %if predicted obs is very diﬀerent from actual obs this score will be low %−>this particle is not very good at representing state . %fake sensor failure here if (abs(k−nSteps/2)<0. %copy selected particles for next generation . end. iSelect = rand(nParticles.2). %ﬁnd the particle that corresponds to each y value ( just a look up) iNextGeneration = interp1(CDF.1∗nSteps) 113 .xP(2. if (mod(k−2.iNextGeneration).plot(xP(1.Map(2.1:nParticles.Map).’XData’. end. if (˜isempty(z)) set(hObsLine.[xEst(1). set(hObsLine.Matlab Code For Particle Filter Example 113 %predict observation for(p = 1:nParticles) %what do we expect observation to be for this particle ? zPred = DoObservationModel(xP(:.
0.2)∗rand(1)). z(2) = AngleWrap(z(2)). xnow = tcomp(xnow. LastOdom = xnow.Map)+sqrt(RTrue)∗randn(2. atan2(Delta(2).u).1).025 .Delta(1))−xVeh(3)]. end.iFeature)−xVeh(1:2). iFeature = −1. uNoise = sqrt(UTrue)∗randn(3. LastOdom = xTrue. iFeature. u = GetRobotControl(k).Map) Delta = Map(1:2. xnow =tcomp(LastOdom. z = DoObservationModel(xTrue. z = [norm(Delta). xTrue = tcomp(xTrue.uNoise).u). 0.1). end. 114 . z(2) = AngleWrap(z(2)).Matlab Code For Particle Filter Example 114 z = []. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function u = GetRobotControl(k) global nSteps. xTrue(3) = AngleWrap(xTrue(3)). %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function SimulateWorld(k) global xTrue. iFeature. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [z] = DoObservationModel(xVeh. u = [0. u = GetRobotControl(k). %internal to robot low−level controller global UTrue.1∗ pi/180∗sin(3∗pi∗k/nSteps)]. else iFeature = ceil(size(Map. if (isempty(LastOdom)) global xTrue. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [xnow] = GetOdometry(k) persistent LastOdom.
This action might not be possible to undo. Are you sure you want to continue?
We've moved you to where you read on your other device.
Get the full title to continue listening from where you left off, or restart the preview.