Intelligent Vehicle – Error propagation
Introduction
In mobile robotics we need a way of estimating robot’s position uncertainty. The uncertainty is
represented by the covariance matrix, Σk of the pose Xk = [x,y,θ], see Figure 1. The uncertainty of the
new position, Σk, is based on previous state uncertainty, Σk-1, and the additive uncertainty depending
on current movement. This means that the error will grow at each movement if we don’t get any
correction of our position. The reason to this is that we have inaccuracies in actuators and in the
mechanical design of the robot, meaning that even if we know our control outputs (where the robot
suppose to go) we don’t end up exactly there. As an alternative to using control outputs we can
measure the movement by measurement of the wheels revolutions with encoders. But these
measurements also contain errors that will result in growing position uncertainty.
Σk
Σk-1
Xk
Xk-1
Figure 1. Prediction of robot uncertainty based on previous state uncertainty and current movement.
If we have a sensor that measures our pose in the global coordinate system, with some uncertainty,
we can correct our position by combining our pose estimate from the robot movement with the
sensor that measures the global position. To be able to combine these measurements we need a tool
for estimate the robot movement uncertainty. For that we use the law or error propagation.
Error propagation
Sometimes you don’t measure exactly the properties that are in your interests, but another property
that you use to calculate the interesting property. For example, if you want to estimate the speed of
a car you can measure the time it takes to drive a certain distance. If we assume that the distance is
perfectly known then our uncertainty in the speed estimate is a function of the uncertainty in the
time estimate, i.e. speed +∆s = distance/(time + ∆t) see Figure 2.
1 Intelligent Vehicles - MK8005, Halmstad University, Sweden
©Björn Åstrand – 2013, ver. 1.4
Speed
s +∆s
t+∆t
Time
Figure 2. The uncertainty in speed is linear dependent on the uncertainty in time.
If the relation between the interesting and the measured property is nonlinear then an
approximation can be used. In Figure 3 the function is made linear by a tangent in point (x, y). To
calculate y+∆y the tangent is used. The reason for this is that much simpler calculation can be used to
find out how a change in x effects a change in y and that the Gaussian property of the uncertainty of
x is preserved in the uncertainty of y, see figure 3.
y +∆y
x+∆x x
Figure 3. Linearization by calculate the derivative in point (x, y).
We can now write an expression for ∆y as ∆y = k∙∆x there k is the slope. An alternative expression is
describing the slope as a derivative. Then the relation between ∆y and ∆x becomes:
𝜕𝑦
∆𝑦 = ∆𝑥
𝜕𝑥
In this example the interesting property only depends on the uncertainty in one parameter.
Following is an example there the area of a rectangle are depending on two uncertainty parameters,
i.e. the uncertainty in width and height, see Figure 4.
2 Intelligent Vehicles - MK8005, Halmstad University, Sweden
©Björn Åstrand – 2013, ver. 1.4
∆w
h
Area
∆h
Figure 4. The calculation of the area is depending on two uncertainty parameters ∆w and ∆h.
One possible expression for the uncertainty of the area, ∆A, is to add together the contribution from
each uncertainty parameter like ∆A = ∆Ah + ∆Aw (∆Aw = ±h∙∆w and ∆Ah = ±w∙∆h). However this
assumption is not really likely. In this case we got one standard deviation of A if h and w changes one
standard deviation as the same time. In reality, the probability that w and h change in same
direction at the same time, assuming that they are independent, is unlikely. So the uncertainty is
much less than the sum of the contributing uncertainties. Actually it can be shown that the
uncertainty is the square root of the sum of all contributing uncertainties in square:
2 2
𝜕𝐴 𝜕𝐴
∆𝐴 = �� ∆ℎ� + � ∆𝑤�
𝜕ℎ 𝜕𝑤
Or if we use the variances instead of the ∆:s.
2 2
𝜕𝐴 𝜕𝐴 𝜕𝐴 2 𝜕𝐴 2
𝜎𝐴 = �� 𝜎ℎ � + � 𝜎𝑤 � = �� � 𝜎ℎ2 + � � 𝜎𝑤2
𝜕ℎ 𝜕𝑤 𝜕ℎ 𝜕𝑤
This is the general form of the law of error propagation. If there are more terms we just add them
under the square. This equation can also be rewritten in matrix form:
𝜕𝐴
𝜕𝐴 𝜕𝐴 𝜎ℎ2 0 𝜕ℎ
ΣA = 𝐽𝐴 Σ𝐴 𝐽𝐴𝑇 = � �� �� �
𝜕ℎ 𝜕𝑤 0 𝜎𝑤2 𝜕𝐴
𝜕𝑤
There JA is the Jacobian matrix (partial derivatives) of the equation A = h∙w. Now we are ready to
write an expression of the law of error propagation in general matrix form. The output covariance
matrix is given by:
ΣY = 𝐽𝑋 Σ𝑋 𝐽𝑋𝑇
3 Intelligent Vehicles - MK8005, Halmstad University, Sweden
©Björn Åstrand – 2013, ver. 1.4
Where
ΣX = covariance matrix representing the input uncertainties
ΣY = covariance matrix representing the propagated uncertainties for the outputs
JX = is the Jacobian matrix defined as
𝜕𝑓1 𝜕𝑓1
⎡ ⋯ ⎤
⎢ 𝜕𝑋1 𝜕𝑋𝑛 ⎥
𝐽𝑋 = ⎢ ⋮ ⋱ ⋮ ⎥
⎢𝜕𝑓𝑚 ⋯
𝜕𝑓𝑚 ⎥
⎣𝜕𝑋1 𝜕𝑋𝑛 ⎦
As mentioned in the introduction, we use the law of error propagation as a tool to calculate the
position (or pose) uncertainty of a mobile robot. The pose of the robot is often a function of a
number of uncertain parameters. New pose uncertainty depends on last state, Xk-1 = [xk-1 yk-1 θk-1]T and
current movement length, dk, and heading change, αk. The variables dk and αk are measured by some
sensor or is a function of control parameters. Next state of the robot may be written as a function of
previous state and the measured (or estimated) movement, i.e. f(Xk-1, Uk).
𝛼𝑘 𝛼𝑘
⎡𝑑𝑘 cos(𝜃𝑘−1 + 2 )⎤ ⎡𝑥𝑘−1 + 𝑑𝑘 cos(𝜃𝑘−1 + 2 )⎤
⎢
𝑋𝑘 = 𝑓(𝑋𝑘−1 , 𝑈𝑘 ) = 𝑋𝑘−1 + ⎢ 𝛼𝑘 ⎥ = ⎢ 𝛼𝑘 ⎥
𝑑𝑘 sin(𝜃𝑘−1 + ) ⎥ ⎢ 𝑦𝑘−1 + 𝑑𝑘 sin(𝜃𝑘−1 + ) ⎥
⎢ 2 ⎥ ⎢ 2 ⎥
⎣ 𝛼𝑘 ⎦ ⎣ 𝜃𝑘−1 + 𝛼𝑘 ⎦
To calculate the variance (or more correctly the covariance) at current time step k we use the law of
error propagation, which for matrices becomes:
𝑇
ΣXk = ��
𝐽𝑋𝑘−1
��Σ��� 𝐽�
𝑋𝑘−1�𝑋��
𝑘−1
+ 𝐽�� 𝐽𝑈𝑇𝑘
Σ𝑈𝑘 ��
𝑈𝑘���
𝑆𝑡𝑎𝑡𝑒 𝑀𝑒𝑎𝑠𝑢𝑟𝑒𝑚𝑒𝑛𝑡
𝑢𝑛𝑐𝑒𝑟𝑡𝑎𝑖𝑛𝑡𝑦 𝑢𝑛𝑐𝑒𝑟𝑡𝑎𝑖𝑛𝑡𝑦
There
𝛼𝑘
⎡1 0 −𝑑𝑘 sin(𝜃𝑘−1 + 2 )⎤
𝐽𝑋𝑘−1 = ⎢⎢ 𝛼𝑘 ⎥
0 1 𝑑𝑘 cos(𝜃𝑘−1 + ) ⎥
⎢ 2 ⎥
⎣0 0 1 ⎦
is the partial derivatives of the state parameters [x y θ] and
𝛼𝑘 𝑑𝑘 𝛼𝑘
⎡cos(𝜃𝑘−1 + ) − sin(𝜃𝑘−1 + )⎤
⎢ 2 2 2 ⎥
𝐽𝑈𝑘 = ⎢ 𝛼𝑘 𝑑𝑘 𝛼𝑘 ⎥
⎢ sin(𝜃𝑘−1 + 2 ) 2
cos(𝜃𝑘−1 + ) ⎥
2
⎣ 0 1 ⎦
is the partial derivatives of the measurement parameters [d α] and
4 Intelligent Vehicles - MK8005, Halmstad University, Sweden
©Björn Åstrand – 2013, ver. 1.4
𝜎2 0
Σ𝑈𝑘 = � 𝑑 �
0 𝜎𝛼2
is the variance of the measurement paramenters.
Note that this is an iterative way of calculating the robot pose uncertainty. First time you have to set
the start position ant the corresponding pose uncertainty:
x0 𝜎𝑥20 0 0
X 0 = �y0 � ΣX0 = � 0 𝜎𝑦20 0 �
θ0 0 0 𝜎𝜃20
Exercises
Exercise 1: Calculate the expression for the pose uncertainty for a differential drive robot there the
distance traveled is given by:
𝛼𝑘
⎡𝑥𝑘−1 + 𝑑𝑘 cos(𝜃𝑘−1 + 2 )⎤
⎢ 𝛼𝑘 ⎥ ∆𝑅 + ∆𝐿
𝑋𝑘 = ⎢ 𝑦 )⎥ there 𝑑𝑘 =
𝑘−1 + 𝑑𝑘 sin(𝜃𝑘−1 + 2
⎢ 2 ⎥
⎣ 𝜃𝑘−1 + 𝛼𝑘 ⎦
Pose uncertainty is given by the law of error propagation:
ΣXk = 𝐽𝑋𝑘−1 Σ𝑋𝑘−1 𝐽𝑋𝑇𝑘−1 + 𝐽𝑈𝑘 Σ𝑈𝑘 𝐽𝑈𝑇𝑘
Calculated the Jacobians:
𝐽𝑋𝑘−1 = ?
𝐽𝑈𝑘 = ?
Exercise 2: Calculate the expression for the pose uncertainty for a differential drive robot there the
heading change is given by:
𝛼𝑘
⎡𝑥𝑘−1 + 𝑑𝑘 cos(𝜃𝑘−1 + 2 )⎤
⎢ 𝛼𝑘 ⎥ ∆𝑅 − ∆𝐿
𝑋𝑘 = ⎢ 𝑦 )⎥ there 𝛼𝑘 =
𝑘−1 + 𝑑𝑘 sin(𝜃𝑘−1 + 𝑏
⎢ 2 ⎥
⎣ 𝜃𝑘−1 + 𝛼𝑘 ⎦
Pose uncertainty is given by the law of error propagation:
ΣXk = 𝐽𝑋𝑘−1 Σ𝑋𝑘−1 𝐽𝑋𝑇𝑘−1 + 𝐽𝑈𝑘 Σ𝑈𝑘 𝐽𝑈𝑇𝑘
Calculated the Jacobians:
𝐽𝑋𝑘−1 = ?
𝐽𝑈𝑘 = ?
5 Intelligent Vehicles - MK8005, Halmstad University, Sweden
©Björn Åstrand – 2013, ver. 1.4
Exercise 3: Calculate the expression for the pose uncertainty for a differential drive robot there
uncertain parameters are ∆R, ∆L and pose:
𝛼𝑘
⎡𝑥𝑘−1 + 𝑑𝑘 cos(𝜃𝑘−1 + 2 )⎤
⎢ 𝛼𝑘 ⎥ ∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿
𝑋𝑘 = ⎢ 𝑦 + 𝑑 sin(𝜃 + )⎥ there 𝑑𝑘 = and 𝛼𝑘 =
2 𝑏
⎢ 𝑘−1 𝑘 𝑘−1
2 ⎥
⎣ 𝜃𝑘−1 + 𝛼𝑘 ⎦
Pose uncertainty is given by the law of error propagation:
ΣXk = 𝐽𝑋𝑘−1 Σ𝑋𝑘−1 𝐽𝑋𝑇𝑘−1 + 𝐽𝑈𝑘 Σ𝑈𝑘 𝐽𝑈𝑇𝑘
Calculated the Jacobians:
𝐽𝑋𝑘−1 = ?
𝐽𝑈𝑘 = ?
Exercise 4: Calculate the expression for the pose uncertainty for a differential drive robot there the
there the only uncertain parameter is distance between the wheels b and pose.
𝛼𝑘
⎡𝑥𝑘−1 + 𝑑𝑘 cos(𝜃𝑘−1 + 2 )⎤
⎢ ∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿
𝑋𝑘 = ⎢ 𝑦 𝛼𝑘 ⎥ there 𝑑𝑘 = and 𝛼𝑘 =
𝑘−1 + 𝑑𝑘 sin(𝜃𝑘−1 + )⎥ 2 𝑏
⎢ 2 ⎥
⎣ 𝜃𝑘−1 + 𝛼𝑘 ⎦
Pose uncertainty is given by the law of error propagation:
ΣXk = 𝐽𝑋𝑘−1 Σ𝑋𝑘−1 𝐽𝑋𝑇𝑘−1 + 𝐽𝑈𝑘 Σ𝑈𝑘 𝐽𝑈𝑇𝑘
Calculated the Jacobians:
𝐽𝑋𝑘−1 = ?
𝐽𝑈𝑘 = ?
Exercises Solutions
Exercise 1:
∆𝑅 + ∆𝐿 𝛼𝑘
⎡1 0 − sin(𝜃𝑘−1 + )⎤
⎢ 2 2 ⎥
𝐽𝑋𝑘−1 = ⎢ ∆𝑅 + ∆𝐿 𝛼𝑘 ⎥
⎢0 1 2
cos(𝜃𝑘−1 + ) ⎥
2
⎣0 0 1 ⎦
1 𝛼𝑘 1 𝛼𝑘 ∆𝑅 + ∆𝐿 𝛼𝑘
⎡ cos �𝜃𝑘−1 + � cos �𝜃𝑘−1 + � − sin �𝜃𝑘−1 + �⎤
⎢2 2 2 2 4 2 ⎥
𝐽𝑈𝑘 = ⎢1 𝛼𝑘 1 𝛼𝑘 ∆𝑅 + ∆𝐿 𝛼𝑘 ⎥
⎢ 2 sin �𝜃𝑘−1 + 2 � 2 sin �𝜃𝑘−1 + 2 � 4
cos �𝜃𝑘−1 + � ⎥
2
⎣ 0 0 1 ⎦
6 Intelligent Vehicles - MK8005, Halmstad University, Sweden
©Björn Åstrand – 2013, ver. 1.4
∆𝑅 ∆𝐿 𝛼𝑘
Exercise 2:
∆𝑅 − ∆𝐿
⎡1 0 −𝑑𝑘 sin(𝜃𝑘−1 + )⎤
⎢ 2𝑏 ⎥
𝐽𝑋𝑘−1 = ⎢ ∆𝑅 − ∆𝐿 ⎥
⎢0 1 𝑑𝑘 cos(𝜃𝑘−1 + 2𝑏 ) ⎥
⎣0 0 1 ⎦
∆𝑅 − ∆𝐿 −𝑑𝑘 ∆𝑅 − ∆𝐿 𝑑𝑘 ∆𝑅 − ∆𝐿
⎡cos(𝜃𝑘−1 + ) sin(𝜃𝑘−1 + ) sin(𝜃𝑘−1 + ) ⎤
⎢ 2𝑏 2𝑏 2𝑏 2𝑏 2𝑏 ⎥
∆𝑅 − ∆𝐿 𝑑𝑘 ∆𝑅 − ∆𝐿 𝑑𝑘 ∆𝑅 − ∆𝐿 ⎥
𝐽𝑈𝑘 = ⎢ sin(𝜃𝑘−1 + ) cos(𝜃𝑘−1 + ) − cos(𝜃𝑘−1 + )
⎢ 2𝑏 2𝑏 2𝑏 2𝑏 2𝑏 ⎥
⎢ 1 1 ⎥
⎣ 0 − ⎦
𝑏 𝑏
𝑑𝑘 ∆𝐿 ∆𝑅
Exercise 3:
∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿
⎡1 0 − sin(𝜃𝑘−1 + )⎤
⎢ 2 2𝑏 ⎥
𝐽𝑋𝑘−1 = ⎢ ∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿 ⎥
⎢0 1 2
cos(𝜃𝑘−1 +
2𝑏
)⎥
⎣0 0 1 ⎦
𝐽𝑈𝑘
1 𝛼𝑘 ∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿 1 𝛼𝑘 ∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿
⎡ cos(𝜃𝑘−1 + ) − sin(𝜃𝑘−1 + ) cos(𝜃𝑘−1 + ) + sin(𝜃𝑘−1 + )⎤
⎢2 2 4𝑏 2𝑏 2 2 4𝑏 2𝑏 ⎥
1 𝛼 ∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿 1 𝛼𝑘 ∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿 ⎥
= ⎢ sin(𝜃𝑘−1 + 𝑘 ) + cos(𝜃𝑘−1 + ) sin(𝜃𝑘−1 + ) − cos(𝜃𝑘−1 + )
⎢2 2 4𝑏 2𝑏 2 2 4𝑏 2𝑏 ⎥
⎢ 1 1 ⎥
⎣ − ⎦
𝑏 𝑏
∆𝐿 ∆𝑅
Exercise 4:
∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿
⎡1 0 − sin(𝜃𝑘−1 + )⎤
⎢ 2 2𝑏 ⎥
𝐽𝑋𝑘−1 = ⎢ ∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿 ⎥
⎢0 1 2
cos(𝜃𝑘−1 +
2𝑏
)⎥
⎣0 0 1 ⎦
∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿 ∆𝑅 − ∆𝐿
⎡ ∙ 2
sin(𝜃𝑘−1 + ) ⎤
⎢ 2 2𝑏 2𝑏 ⎥
∆𝑅 + ∆𝐿 ∆𝑅 − ∆𝐿 ∆𝑅 − ∆𝐿 ⎥
𝐽𝑈𝑘 = ⎢− ∙ cos(𝜃𝑘−1 + )
⎢ 2 2𝑏 2 2𝑏 ⎥
⎢ ∆𝑅 − ∆𝐿 ⎥
⎣ − 2 ⎦
𝑏
7 Intelligent Vehicles - MK8005, Halmstad University, Sweden
©Björn Åstrand – 2013, ver. 1.4
8 Intelligent Vehicles - MK8005, Halmstad University, Sweden
©Björn Åstrand – 2013, ver. 1.4
