SWISS FEDERAL INSTITUTE OF TECHNOLOGY LAUSANNE

EIDGENÖSSISCHE TECHNISCHE HOCHSCHULE LAUSANNE

POLITECNICO FEDERALE DI LOSANNA

DÉPARTEMENT DE MICROTECHNIQUE Autonomous Systems Lab

INSTITUT DE SYSTÈMES ROBOTIQUE Prof. Dr. Roland Siegwart

An Introduction To Error Propagation:

Derivation, Meaning and Examples
of Equation CY = FX CX FXT

Kai Oliver Arras

Technical Report

Nº EPFL-ASL-TR-98-01 R3

of the Autonomous Systems Lab, Institute of Robotic Systems,

Swiss Federal Institute of Technology Lausanne (EPFL)

September 1998
Contents

1. Introduction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2. Error Propagation: From the Beginning . . . . . . . . . . . . . . . . . . . . . . . 2

2.1 A First Expectation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2 When is the Approximation a Good One? . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.3 The Almost General Case: Approximating the Distribution of
Y = f ( X 1, X 2, …, X n ) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.3.1 Addendum to Chapter 2.2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.4 Getting Really General: Adding Z = g ( X 1, X 2, …, X n ) . . . . . . . . . . . . . . . . . . 6

3. Derivating the Final Matrix Form . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

4. Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

4.1 Probabilistic Line Extraction From Noisy 1D Range Data . . . . . . . . . . . . . . . 9

4.2 Kalman Filter Based Mobile Robot Localization:
The Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

5. Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

Literature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

Appendix A Finding the Line Parameters r and α in the Weighted

Least Square Sense . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

Appendix B Deriving the Covariance Matrix for r and α . . . . . . . . . . . . . . . . 17

B.1 Practical Considerations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

1
1. Introduction

This report attempts to shed light onto the equation

T
CY = FX C X FX , (1)
where C X is a n × n , C Y a p × p covariance matrix and FX some matrix of dimension p × n .
In estimation applications like Kalman filtering or probabilistic feature extraction we fre-
T
quently encounter the pattern FX C X FX . Many texts in literature introduce this equation with-
out further explanation. But relationship (1), called the error propagation law, can be
explicitely derived and understood, being important for a comprehension of its underlying
approximative character. Here, we want to bridge the gap between these texts and the novice
to the world of uncertainty modeling and propagation.
Applications of (1) are e.g. error propagation in model-based vision, Kalman filtering, reli-
ability or probabilistic systems analysis in general.

2. Error Propagation: From the Beginning

We will first forget the matrix form of (1) and change to a different perspective. Error propa-
gation is the problem of finding the distribution of a function of random variables. Often we
have mathematical models of the system of interest (the output as a function of the input and
the system components) and we know something about the distribution of the input and the
components.

X System Y

Figure 1: The simplest case: one input random variable ( N = 1 ), and one output
random variable ( P = 1 ).

Then, we desire to know the distribution of the output, that is the distribution function of Y
when Y = f ( X ) where f ( . ) is some known function and the distribution function of the ran-
dom variable X is known†.
If f ( . ) is a nonlinear function, the probability distribution function of Y , p Y ( y ) , becomes
quickly very complex, particularly when there is more than one input variable. Although a
general method of solution exists (see [BREI70] Chapter 6-6), its complexity can be demon-
strated already for simple problems. An approximation of p Y ( y ) is therefore desirable. The
approximation consists in the propagation of only the first two statistical moments, that is the
mean µ Y and the second (central) moment σ Y , the variance. These moments do not in general
2

describe the distribution of Y . However if Y is assumed to be normally distributed they do‡.

† We do not consider pathological functions f ( . ) where Y is not a random variable – however, they exist
‡ That is simply one of the favorable properties of the normal distribution.

2
2.1 A First Expectation

Look at figure 2 where the simple case with one input and one output is illustrated. Suppose
that X is normally distributed with mean µ X and standard deviation σ X †. Now we would like

µy + σy f (X)
µy

µy – σy

X
µx – σx µx µx + σx

Figure 2: One-dimensional case of a nonlinear error propagation problem

to know how the 68% probability interval [ µ X – σ X, µ X + σ X ] is propagated through the ‘sys-
tem’ f ( . ) .
First of all, from figure 2 it can be seen that if the shaded interval would be mapped onto
the y -axis by the original function its shape would be somewhat distorted and the resulting
distribution would be asymmetric, certainly not Gaussian anymore. When approximating
f ( X ) by a first-order Taylor series expansion about the point X = µ X ,

∂f
Y ≈ f ( µX ) + ( X – µX ) , (2)
∂X X = µX

we obtain the linear relationship shown in figure 2 and with that a normal distribution for
p Y ( y ) ‡. Now we can determine its parameters µ Y and σ Y .
µY = f ( µ X ) , (3)

∂f
σY = σX . (4)
∂X X = µX

Finally the expectation is rised that in the remainder of this text we will again bump into
some generalized form of equation (3) and (4).
At this point, we should not forget that the output distribution, represented by µ Y and σ Y ,
is an approximation of some unknown truth. This truth is impertinently nonlinear, non-nor-
mal and asymmetric, thus inhibiting any exact closed form analysis in most cases. We are
then supposed to ask the question:

† Remember that the standard deviation σ is by definition the distance between the most probable
value, µ , and the curve’s turning points.
‡ Another useful property of the normal distribution, worth to be remembered: Gaussian stays Gaussian

under linear transformations.

3
2.2 When is the Approximation a Good One?

Some textbooks write equations (3) and (4) as inequalities. But if the left hand sides denote
the parameters of the output distribution which is, by assumption, normal, we can write them
as equalities. The first two moments of the true (but unknown) output distribution – let’s call
them µ 0 and σ 0 – are definitely different from these values†. Hence
2

µ0 ≈ µY (5)

σ0 ≈ σY
2 2
(6)
It is evident that if f ( . ) is linear we can write (5) and (6) as equalities. However the following
factors affect approximation quality of µ Y and σ Y by the actual values y∗ and s y∗ .

µ Y ≈ y∗ (7)

σ Y ≈ s y∗ (8)
Thus they apply for both cases; when f ( . ) is linear and when it is nonlinear‡.
• The guess x∗ : In general we do not know the expected value µ X . In this case, its best (and
only available) guess is the actual value of X , for example the current measurement x∗ . We
then differentiate f ( X ) at the point X = x∗ hoping that x∗ is close to E [ X ] such that y∗
is not too far from E [ Y ].
• Extent of nonlinearity of f ( . ) : Equations (3) and (4) are good approximations as long as the
first-order Taylor series is a good approximation which is the case if f ( . ) is not too far from
linear within the region that is within one standard deviation of the mean [BREI70]. Some
people even ask f ( . ) to be close to linear within a ± 2σ -interval.
The nonlinearity of f ( . ) and the deviation of x∗ from E [ X ] have a combined effect on σ Y . If
both factors are of high magnitude, the slope at X = x∗ can differ strongly and the resulting
approximation of σ Y can be poor. At the other hand, if already one of the them is small, or
even both, we can expect s y∗ to be close to σ Y .
• The guess s x∗ : There are several possibilities to model the input uncertainty. The model
could incorporate some ad hoc assumptions on σ X or it might rely on an empirically gained
relationship to µ X . Sometimes it is possible to have a physically based model providing
‘true’ uncertainty information for each realization of X . By systematically following all
sources of perturbation during the emergence of x∗ , such a model has the desirable property
that it accounts for all important factors which influence the outcome x∗ and s x∗ .
In any case, the actual value s x∗ remains a guess of σ X which is hopefully close to σ X .
But there is also reason for optimism. The conditions suggested by Figure 2 are exaggerated.
Mostly, σ X is very small with respect to the range of X . This makes (2) an approximation of
sufficient quality in many practical problems.

† Remember that the expected value and the variance (and all other moments) have a general definition,
i.e. are independent whether the distribution functions exhibits some nice properties like symmetry. They
are also valid for arbitrarily shaped distributions and always quantify the most probable value and its
‘spread’.
‡ It might be possible that in some nonlinear but nice-conditioned cases the approximation effects of non-

normality and the other factors compensate each other yielding a very good approximation. Nevertheless,
the opposite might also be true.

4
2.3 The Almost General Case: Approximating the Distribution of
Y = f ( X 1, X 2, …, X n )

The next step towards equation (1) is again a practical approximation based on a first-order
Taylor series expansion, this time for a multiple-input system. Consider

X1
X2
X3 System Y

Xn

Figure 3: Error propagation in a multi-input single-output system: N = n , P = 1 .

Y = f ( X 1, X 2, …, X n ) where the X i ‘s are n input random variables and Y is represented by

its first-order Taylor series expansion about the point µ 1, µ 2, …, µ n
n
∂f
Y ≈ f ( µ 1, µ 2, …, µ n ) + ∑ ( µ , µ , …, µ n ) [ X i – µ i ] .
∂ Xi 1 2
(9)
i=1

Equation (9) is of the form Y ≈ a 0 + ∑ a i ( X i – µ i ) with

a 0 = f ( µ 1, µ 2, …, µ n ) , (10)

∂f
ai = ( µ , µ , …, µ n ) . (11)
∂ Xi 1 2
As in chapter 2.1, the approximation is linear. The distribution of Y is therefore Gaussian and
we have to determine µ Y and σ Y .
2

µY = E [ Y ] = E [ a0 + ∑ ai ( X i – µi ) ] (12)
i
= E [ a0 ] + ∑ E [ ai X i ] – E [ ai µi ] (13)
i
= a0 + ∑ ai E [ X i ] – ai E [ µi ] (14)
i
= a0 + ∑ ai µi – ai µi (15)
i
= a0 (16)

µ Y = f ( µ 1, µ 2, …, µ n ) (17)

σY = E [ ( Y – µY ) 2 ] = E [ ( ∑ ai ( X i – µi ) ) 2 ]
2
(18)
i
= E [ ∑ ai ( X i – µi ) ∑ a j ( X j – µ j ) ] (19)
i j
= E [ ∑ a i2 ( X i – µ i )2 + ∑ ∑ ai a j ( X i – µi ) ( X j – µ j ) ] (20)
i i≠ j

5
 = ∑ a i2 E [ ( X i – µ i ) 2 ] + ∑ ∑ a i a j E [ ( X i – µ i ) ( X i – µ j ) ] (21)
i i≠ j

∑ ai σi ∑ ∑ a i a j σ ij
2 2
= + (22)
i i≠ j
∂f 2
∂f ∂f
σY = ∑  ∂ X i σ i + ∑ ∑  ∂ X i  ∂ X j σ ij
2 2
(23)
i i≠ j

The vector µ 1, µ 2, …, µ n has been omitted. If the X i ‘s are independent the covariance σ ij dis-
appears, and the resulting approximated variance is

∂f  2 2
σY ≈ ∑  σ .
2
(24)

i ∂ Xi
 i
This is the moment to validate our expectation from chapter 2.1 for the one-dimensional
case. Equation (17) corresponds directly with (3) whereas (23) somewhat contains equation
(4).

2.3.1 Addendum to Chapter 2.2

In order to close the discussion on factors affecting approximation quality, we have to consider
briefly two aspects which play a role if there is more than one input.
• Independence of the inputs: Equation (24) is a good approximation if the stated assumption
of independence of all X i ‘s is valid.
It is finally to be mentioned that, even if the input distributions are not strictly Gaussian, the
assumption of the output being normal is often reasonable. This follows from the central limit
theorem when the X i ‘s somehow additively constitute the output Y .

2.4 Getting Really General: Adding Z = g ( X 1, X 2, …, X n )

Often there is not just a single Y which depends on the X i ‘s but there are more system out-
puts, that is, more random variables like Y . Suppose our system has an additional output Z
with Z = g ( X 1, X 2, …, X n ) .

X1
X2 Y
X3 System
Z
Xn

Figure 4: Error propagation in a multi-input multi-output system: N = n , P = 2

Obviously µ Z and σ Z can exactly be derived as shown before. The additional aspect which is
2

introduced by Z is the question of the statistical dependence of Y and Z which is expressed

by their covariance σ YZ = E [ ( Y – µ Y ) ( Z – µ Z ) ] . Let’s see where we arrive when substituting
Y and Z by their first-order Taylor series expansion (9).

6
 σ YZ = E [ ( Y – µY ) ( Z – µZ ) ] (25)

= E [ Y ⋅ Z ] – E [ Y ]E [ Z ] (26)

= E  µ Y + ∑ ∂ f [ X i – µ i ] ⋅  µ Z + ∑ ∂g [ X i – µ i ] – µ Y µ Z (27)

∂ Xi ∂ Xi

= E µ Y µ Z + µ Z ∑ ∂ f [ X i – µ i ] + µ Y ∑ ∂g [ X i – µ i ] + ∑ ∂ f [ X i – µ i ] ∑ ∂g [ X i – µ i ] – µ Y µ Z (28)
∂ Xi ∂ Xi ∂ Xi ∂ Xi

∂f ∂f ∂g ∂g
= E [ µY µZ ] + µZ E ∑ ∂ Xi X i – ∑ ∂ Xi µi + µY E ∑ ∂ Xi X i – ∑ ∂ Xi µi
∂ f ∂g
+E ∑ ∑ ∂ Xi ∂ X j [ X i – µi ] [ X j – µ j ] – µY µZ (29)

= µ Y µ Z + µ Z ∑ ∂ f E [ X i ] – µ Z ∑ ∂ f E [ µ i ] + µ Y ∑ ∂g E [ X i ] – µ Y ∑ ∂g E [ µ i ]
∂ Xi ∂ Xi ∂ Xi ∂ Xi
∂ f ∂g ∂ f ∂g
∑ ∂ Xi ∂ Xi [ X i – µi ] +∑∑ (30)
2
+E [ X – µi ] [ X j – µ j ] – µY µZ
i≠ j
∂ Xi ∂ X j i

= ∑ ∂ f ∂g E [ ( X i – µ i ) 2 ] + ∑ ∑ ∂ f ∂g E [ ( X i – µ i ) ( X j – µ j ) ] (31)
∂ Xi ∂ Xi i≠ j
∂ Xi ∂ X j

σ YZ = ∑ ∂ f ∂g σ 2i + ∑ ∑ ∂ f ∂g σ 2ij (32)
∂ Xi ∂ Xi i≠ j
∂ Xi ∂ X j

If X i and X j are independent, the second term, holding their covariance, disappears. Adding
more output random variables brings in no new aspects. In the remainder of this text we shall
consider P = 2 without loss of generality.

3. Derivating the Final Matrix Form

Now we are ready to return to equation (1). We will now see that we only have to reformulate
equations (23) and (32) in order to obtain the initial matrix form. We recall the gradient oper-
ator with respect to the n -dimensional vector X
T
∇X = ∂ ∂ ∂ . (33)
…
∂X1 ∂X2 ∂Xn
T
f ( X ) is a p -dimensional vector-valued function f ( X ) = f 1 ( X ) f 1 ( X ) … f p ( X ) . The
Jacobian FX is defined as the transpose of the gradient of f ( X ) , whereas the gradient is the
outer product of ∇X and f ( X )

∂f 1 ∂f 1
…
T f 1( X ) ∂ ∂ ∂X1 ∂Xn
FX = ∇X ⋅ f ( X ) T = … = (34)
f 2( X ) ∂X1 ∂Xn ∂f 2 ∂f 2
…
∂X1 ∂Xn

FX has dimension 2 × n in this case, p × n in general. We introduce the symmetric n × n

input covariance matrix C X which contains all variances and covariances of the input random
variables X 1, X 2, …, X n . If the X i ‘s are independent all σ ij with i ≠ j disappear and C X is
diagonal.

7
 σX1 σX1 X2 … σX1 Xn
2

σX2 X1 σX2 … σX2 Xn

2
CX = (35)
: : :
σXn X1 σXn X2 … σXn
2

We further introduce the symmetric 2 × 2 output covariance matrix C Y ( p × p in general

with outputs Y 1, Y 2, …, Y p )

σY 1 σY 1Y 2
2
CY = (36)
σY 2Y 1 σY 2
2

Now we can instantly form equation (1)

T
C Y = FX C X FX (37)

σX
2
σX … σX ∂f 1 ∂f 2
∂f 1 ∂f 1 1 1 X2 1 Xn
2 … ∂X1 ∂X1
σY σY ∂X1 ∂Xn σX
2
σX … σX
1 1Y 2
= 2 X1 2 2 Xn : : (38)
σY
2
σY ∂f 2 ∂f 2 : : :
2Y 1 2 … ∂f 1 ∂f 2
∂X1 ∂Xn σX σX …
2
σX
n X1 n X2 n
∂Xn ∂Xn

∂f 1 ∂f 2
∂f 1 2 ∂f 1 ∂f 1 ∂f 1 2
σ +…+ σ … σ +…+ σ ∂X1 ∂X1
∂ X1 X1 ∂ Xn Xn X1 ∂ X1 X1 Xn ∂ Xn Xn
= : : (39)
∂f 2 2 ∂f 2 ∂f 2 ∂f 2 2
σ +…+ σ … σ +…+ σ ∂f 1 ∂f 2
∂ X1 X1 ∂ Xn Xn X1 ∂ X1 X1 Xn ∂ Xn Xn
∂Xn ∂Xn

Looks nice. But what has it to do with chapter 2? To answer this question we evaluate the
first element σ Y 1 , the variance of Y 1 :
2

2 2
∂ f 1  2 ∂f 1 ∂f 1 ∂f 1 ∂f 1 ∂f 1 ∂f 1 ∂ f 1  2 ∂f 1 ∂f 1
σY
2
=  σX1 + σX X + … + σX X + σX X +   σX2 + … + σ
1
 ∂ X 1 ∂ X2∂ X1 2 1 ∂ Xn∂ X1 n 1 ∂ X1∂ X2 1 2
 ∂ X 2 ∂ Xn∂ X2 Xn X2
2
∂f 1 ∂f 1 ∂f 1 ∂f 1 ∂ f 1  2
+…+ σ + σ +…+  σXn (40)
∂ X1∂ Xn X1 Xn ∂ X2∂ Xn X2 Xn  ∂ X n

2
∂ f  ∂f 1 ∂f 1
= ∑  1 σ 2X i + ∑ ∑ ∂ X i ∂ X j σX X (41)
i 
∂ X i i≠ j
i j

If we now reintroduce the notation of chapter 2.3, that is, f 1 ( X ) = f ( X ) , Y 1 = Y , and

σ X i = σ i , we see that (41) equals exactly equation (23). Assuming the reader being a notori-
ous skeptic, we will also look at the off-diagonal element σ Y 1 Y 2 , the covariance of Y and Z :
∂f 1 ∂f 2 2 ∂f 1 ∂f 2 ∂f 1 ∂f 2 ∂f 1 ∂f 2 ∂f 1 ∂f 2 2 ∂f 1 ∂f 2
σY = σ + σ +…+ σ + σ + σ +…+ σ
1Y 2 ∂ X1∂ X1 X1 ∂ X2∂ X1 X2 X1 ∂ Xn∂ X1 Xn X1 ∂ X1∂ X2 X1 X2 ∂ X2∂ X2 X2 ∂ Xn∂ X2 Xn X2
∂f 1 ∂f 2 ∂f 1 ∂f 2 ∂f 1 ∂f 2 2
+…+ σX X + σX X + … + σ (42)
∂ X1∂ Xn 1 n ∂ X2∂ Xn 2 n ∂ Xn∂ Xn Xn

8
 ∂f 1 ∂f 2 ∂f 1 ∂f 2
= ∑ ∂ X i ∂ X i σX + ∑ ∑ ∂ X i ∂ X j σX X (43)
2
i i j
i i≠ j

Again, by substituting f 1 ( X ) by f ( X ) , f 2 ( X ) by g ( X ) and σ X i by σ i , equation (43) corre-

spond exactly to the previously derived equation (32) for σ YZ .
We were obviously able, having started from a simple one-dimensional error propagation
T
problem, to derive the error propagation law CY = FX C X FX . Putting the results together
yielded its widely used matrix form (1).
Now we can also understand the informal interpretation of figure 5.

...
y
nt
ai
rt
.”
ely t

ce
iv tpu

un
t
a u

t
m o

pu
xi the gh

in
o ou
pr o

he
r
ap ed t th ..
“T
d p
an p T t ed f(.).
... ma a
CY = FX C X F ag m
X pop syste
is
... the

Figure 5: Interpretation of the error propagation law in its matrix form

4. Examples

4.1 Probabilistic Line Extraction From Noisy 1D Range Data

Model-based vision where geometric primitives are the sought image features is a good exam-
ple for uncertainty propagation. Suppose the segmentation problem has already been solved,
that is, the set of inlier points with respect to the model is known. Suppose further that the
regression equations for the model fit to the points have a closed-form solution – which is the
case when fitting straight lines. And suppose finally, that the measurement uncertainties of
the data points are known as well. Then we can directly apply the error propagation law in
order to get the output uncertainties of the line parameters.

xi = (ri , qi)

r di

Figure 6: Estimating a line in the least squares sense. The model parameters r
(length of the perpendicular) and α (its angle to the abscissa) describe uniquely a
line.

9
Suppose n measurement points in polar coordinates x i = ( ρ i, θ i ) are given and modeled as
random variables X i = ( P i , Q i ) with i = 1, …, n . Each point is independently affected by
Gaussian noise in both coordinates.

P i ~ N ( ρ i, σ ρi )
2
(44)

Q i ~ N ( θ i, σ θi )
2
(45)

E [ P i ⋅ P j ] = E [ P i ]E [ P j ] ∀ i, j = 1, …, n (46)

E [ Q i ⋅ Q j ] = E [ Q i ]E [ Q j ] ∀ i, j = 1, …, n (47)

E [ P i ⋅ Q j ] = E [ P i ]E [ Q j ] ∀ i, j = 1, …, n (48)

Now we want to find the line x cos α + y sin α – r = 0 where x = ρ cos ( θ ) and
y = ρ sin ( θ ) yielding ρ cos θ cos α + ρ sin θ sin α – r = 0 and with that, the line model

ρ cos ( θ – α ) – r = 0 . (49)

This model minimizes the orthogonal distances from the points to the line. It is important to
note that fitting models to data in some least square sense yields not a satisfying geometric
solution in general. It is crucial to know which error is minimized by the fit equations. A good
illustration is the paper of [GAND94] where several algorithms for fitting circles and ellipses are
presented which minimize algebraic and geometric distances. The geometric variety of solu-
tions for the same set of points demonstrate the importance of this knowledge if geometric
meaningful results are required.
The orthogonal distance d i of a point ( ρ i, θ i ) to the line is just

ρ i cos ( θ i – α ) – r = d i . (50)

Let S be the (unweighted) sum of squared errors.

∑ di ∑ ( ρ i cos ( θ i – α ) – r )
2 2
S = = (51)
i i

The model parameters ( α, r ) are now found by solving the nonlinear equation system

∂S ∂S
= 0 = 0. (52)
∂α ∂r
Suppose further that for each point a variance σ i modelling the uncertainty in radial and
2

angular direction is given a priori or can be measured. This variance will be used to determine
a weight w i for each single point, e.g.

w i = 1 ⁄ σ i †.
2
(53)

Then, equation (51) becomes

∑ wi d i ∑ w i ( ρ i cos ( θ i – α ) – r )
2 2
S = = . (54)

† The issue of determining an adequate weight when σ i (and perhaps some additional information) is given
is complex in general and beyond the scope of this text. See [CARR88] for a careful treatment.

10
It can be shown (see Appendix A) that the solution of (52) in the weighted least square sense†
is

 w i ρ i sin 2θ i – ------- ∑ ∑ w i w j ρ i ρ j cos θ i sin θ j 

2
∑
2
1  Σ wi
α = --- atan2  ------------------------------------------------------------------------------------------------------------------
- (55)
2  ∑ w ρ cos 2θ – ------ 1
w w ρ ρ cos ( θ i + θ j )
Σ wi ∑ ∑ i j i j
2
-
 i i i


∑ w i ρ i cos ( θ i – α )- .
r = --------------------------------------------- (56)
∑ wi
Now we would like to know how the uncertainties of the measurements propagate through
‘the system’ (55), (56). See figure 7.

X1
X2 A
X3 Model Fit
R
Xn

Figure 7: When extracting lines from noisy measurement points X i , the model fit
module produces line parameter estimates, modeled as random variables A, R . It is
then interesting to know the variability of these parameters as a function of the noise
at the input side.

This is where we simply apply equation (1). We are looking for the 2 × 2 output covariance
matrix

σ A σ AR
2
C AR = , (57)
σ AR σ R
2

given the 2n × 2n input covariance matrix

diag ( σ ρ i )
2
CP 0 0
CX = = (58)
diag ( σ θ i )
0 CQ 2
0

and the system relationships (55) and (56). Then by calculating the Jacobian

∂α ∂α ∂α ∂α ∂α ∂α
… …
∂ P1 ∂ P2 ∂ Pn ∂ Q1 ∂ Q2 ∂ Qn
F PQ = (59)
∂r ∂r ∂r ∂r ∂r ∂r
… …
∂ P1 ∂ P2 ∂ Pn ∂ Q1 ∂ Q2 ∂ Qn

† We follow here the notation of [DRAP88] and distinguish a weighted least squares problem if C X is diag-
onal (input errors are mutually independent) and a generalized least squares problem if C X is non-diago-
nal.

11
we can instantly form the error propagation equation (60) yielding the sought C AR .

T
C AR = F PQ C X F PQ (60)

Appendix A is concerned about the step-by-step derivation of the fit equations (55) and
(56), whereas in Appendix B equation (60) is once more derived. Under the assumption of
negligible angular uncertainties, implementable expressions for the elements of C AR are deter-
mined, also in a step-by-step manner.

4.2 Kalman Filter Based Mobile Robot Localization:

The Measurement Model

The measurement model in a Kalman filter estimation problem is another place where we
encounter the error propagation law. The reader is assumed to be more or less familiar with
the context of mobile robot localization, Kalman filtering and the notation used in [BAR93] or
[LEON92].
In order to reduce the unbounded growth of odometry errors the robot is supposed to up-
date its pose by some sort of external referencing. This is achieved by matching predicted en-
vironment features with observed ones and estimating the vehicle pose in some sense with the
set of matched features. The prediction is provided by an a priori map which contains the po-
sition of all environment features in global map coordinates. In order to establish correct cor-
respondence of observed and stored features, the robot predicts the position of all currently
visible features in the sensor frame. This is done by the measurement model

ẑ ( k + 1 k ) = h ( x̂ ( k + 1 k ) ) + w ( k + 1 ) . (61)

The measurement model gets the predicted robot pose x̂ ( k + 1 k ) as input and ‘asks’ the map
which features are visible at the current location and where they are supposed to appear when
starting the observation. The map evaluates all visible features and returns their transformed
positions in the vector ẑ ( k + 1 k ) . The measurement model is therefore a world-to-robot-to-
sensor frame transformation†.
However, due to nonsystematic odometry errors, the robot position is uncertain and due to
imperfect sensors, the observation is uncertain. The former is represented by the (predicted)
state covariance matrix P ( k + 1 k ) and the latter by the sensor noise model
w ( k + 1 ) ∼ N ( 0, R ( k + 1 ) ) . They are assumed to be independent. Sensing uncertainty
R ( k + 1 ) affects the observation directly, whereas vehicle position uncertainty P ( k + 1 k ) –
which is given in world map coordinates – will propagate through the frame transformations
world-to-robot-to-sensor h ( . ) , linearized about the prediction x̂ ( k + 1 k ) . Then the observa-
tion is made and the matching of predicted and observed features can be performed in the sen-
sor frame yielding the set of matched features. The remaining position uncertainty of the
matched features given all observations up to and including time k ,

S ( k + 1 ) = cov [ z ( k + 1 ) Z ] ,
k
(62)

† Note that h ( . ) is assumed to be ‘intelligent’, that is, it contains both, the mapping x̂ ( k + 1 k ) → m j
(with m j as the position vector of feature number j ) and the world-to-sensor frame transformation of all
visible m j for the current prediction x̂ ( k + 1 k ) . This is in contrast to e.g. [LEON92], where solely the
frame transformation is done by h ( x̂ ( k + 1 k ), m j ) and the mapping x̂ ( k + 1 k ) → m j is somewhere else.

12
is the superimposed uncertainty of observation and the propagated one from the robot pose,

S ( k + 1 ) = ∇h P ( k + 1 k ) ∇h + R ( k + 1 ) . (63)

S ( k + 1 ) is also called measurement prediction covariance or innovation covariance.

5. Exercises

As stated in the introduction, the report has educational purposes and accompanies a lecture
on autonomous systems in the microengineering departement at EPFL. Thus, the audience of
this report are people not yet too familarized with the field of uncertainty treatment. Some
propositions for exercises are given:
• Let them do the derivation of µ Y (equations (12) to (17)) and σ Y (equations (18) to (23))
2

given a few rules for the expected value.

• Let them do the derivation of σ YZ (equations (25) to (32)) or, in the context of example 1,
equations (93) to (101) of Appendix B for σ AR . Some rules for the expected value and dou-
ble sums might be helpful.
• Let them make an illustration of each factor affecting approximation quality discussed in
chapter 2.2 with drawings like figure 2.
• If least squares estimation in a more general sense is the issue, derivating the fit equations
for a regression problem is quite instructive. The standard case, linear in the model parame-
ters, and with uncertainties in only one variable is much simpler than the derivation of
example 1 in Appendix A. Additionally the output covariance matrix can be determined
with (1).

13
Literature

[BREI70] A.M. Breipohl, Probabilistic Systems Analysis: An Introduction to Probabilistic

Models, Decisions, and Applications of Random Processes, John Wiley & Sons,
1970.
[GAND94] W. Gander, G. H. Golub, R. Strebel, “Least-Squares Fitting of Circles and
Ellipses”, BIT, vol. 34, no. 4, p. 558-78, Dec. 1994.
[DRAP88] N.R. Draper, H. Smith, Applied Regression Analysis, 3rd edition, John Wiley &
Sons, 1988.
[CARR88] R. J. Carroll, D. Ruppert, Transformation and Weighting in Regression, Chapman
and Hall, 1988.
[BAR93] Y. Bar-Shalom, X.-R. Li, Estimation and Tracking: Principles, Techniques, and
Software, Artech House, 1993.
[LEON92] J.J. Leonard, H.F. Durrant-Whyte, Directed Sonar Sensing for Mobile Robot Nav-
igation, Kluwer Academic Publishers, 1992.
[ARRAS97] K.O. Arras, R.Y. Siegwart, “Feature Extraction and Scene Interpretation for Map-
Based Navigation and Map Building”, Proceedings of SPIE, Mobile Robotics XII,
Vol. 3210, p. 42-53, 1997.

14
Appendix A: Finding the Line Parameters r and α in the Weighted
Least Square Sense

Consider the nonlinear equation system

∂S
=0 (64)
∂r

∂S
=0 (65)
∂α

where S is the weighted sum of squared errors

n
= ∑ wi ( ρi cos θi cos α + ρi sin θi sin α – r ) . (66)
2
S
i=1

We start solving the system (64), (65) by working out parameter r .

∂S
∂r
= 0 = ∑ 2wi ( ρi cos θi cos α + ρi sin θi sin α – r ) ( –1 ) (67)

= – 2 ∑ w i ρ i ( cos θ i cos α + sin θ i sin α ) + 2 ∑ w i r (68)

= – 2 ∑ w i ρ i cos ( θ i – α ) + 2r ∑ w i (69)

2r ∑ w i = 2 ∑ w i ρ i cos ( θ i – α ) (70)

r
∑ wi ρ cos ( θi – α )
= -----------------------------------------------
i
(71)
∑ wi
Parameter α is slightly more complicated. We introduce the following notation
cos θ i = c i sin θ i = s i . (72)

∂S
= ∑ 2w i ( ρ i c i cos α + ρ i s i sin α – r ) ∂ ( ρ i c i cos α + ρ i s i sin α – r ) (73)
∂α ∂α

∂ 
= 2 ∑ w i  ρ i c i cos α + ρ i s i sin α – --------
1
- ∑ w j ρ j cos ( θ j – α )  – ρ i c i sin α + ρ i s i cos α –
 
r (74)
Σ w j ∂ α 

= 2 ∑ w i  ρ i c i cos α + ρ i s i sin α – --------

1
- ∑ w j ρ j cos ( θ j – α )  ρ i s i cos α – ρ i c i sin α – --------- ∑ w j ρ j sin ( θ j – α ) (75)
 
1

Σ j w Σ wj

= 2 ∑ w i ρ 2i c i s i cos2 α – ρ 2i c 2i cos α sin α – --------

1
- ρ c cos α ∑ w j ρ j sin ( θ j – α ) + ρ i s i cos α sin α
2 2
Σ wj i i
– ρ i s i c i sin2 α – --------- ρ i s i sin α ∑ w j ρ j sin ( θ j – α ) – --------- ∑ w j ρ j cos ( θ j – α ) ρ i s i cos α
2 1 1
Σ wj Σ wj

+ --------- ∑ w j ρ j cos ( θ j – α ) ρ i s i cos α + ----------------2- ∑ w j ρ j cos ( θ j – α ) ∑ w j ρ j sin ( θ j – α )

1 1
(76)
Σ w j ( Σ wj )

= 2 ∑ w i ρ 2i c i s i ( cos2 α – sin2 α ) – ρ 2i cos α sin α ( c 2i – s 2i ) – --------

Σ wj ∑ j i j i
1
- w ρ ρ c cos α sin ( θ j – α )

– --------- ∑ w j ρ i ρ j s i sin α sin ( θ j – α ) – --------- ∑ w j ρ i ρ j s i cos α cos ( θ j – α )

1 1
Σ wj Σ wj

+ --------- ∑ w j ρ i ρ j c i sin α cos ( θ j – α ) + ----------------2- ∑ ∑ w j w k ρ j ρ k cos ( θ j – α ) sin ( θ k – α )

1 1
(77)
Σ wj ( Σ wj ) j k

15
 = 2 cos 2α ∑ w i ρ 2i c i s i – 2 cos α sin α ∑ w i ρ 2i c 2i + --------
Σ wi ∑ ∑ i j i j i
2
- w w ρ ρ ( c cos α + s i sin α ) ( s j cos α – c j sin α )

+ --------- ∑ ∑ w i w j ρ i ρ j [ – c i cos α ( s j cos α – c j sin α ) – s i sin α ( s j cos α – c j sin α )

2
Σ wi
+ s i cos α ( c j cos α + s j sin α ) + c i sin α ( c j cos α + s j sin α ) ] (78)
= cos 2α ∑ w i ρ 2i s 2i – sin 2α ∑ w i ρ 2i c 2i
+ --------- ∑ ∑ w i w j ρ i ρ j [ c i s j cos2 α – c i c j sin α cos α + s i s j sin α cos α – s i c j sin2 α – c i s j cos2 α
2
Σ wi
+ c i c j sin α cos α – s i s j sin α cos α + s i c j sin2 α – s i c j cos2 α

– s i s j sin α cos α + c i c j sin α cos α + c i s j sin2 α ] (79)

= cos 2α ∑ w i ρ 2i s 2i – sin 2α ∑ w i ρ 2i c 2i
+ --------- ∑ ∑ w i w j ρ i ρ j ( – s i c j cos2 α – s i s j sin α cos α + c i c j sin α cos α + c i s j sin2 α )
2
(80)
Σ wi
= cos 2α ∑ w i ρ 2i s 2i – sin 2α ∑ w i ρ 2i c 2i
+ --------- ∑ ∑ w i w j ρ i ρ j [ sin α cos α ( c i c j – s i s j ) + c i s j sin2 α – s i c j cos2 α ]
2
(81)
Σ wi
= cos 2α ∑ w i ρ 2i s 2i – sin 2α ∑ w i ρ 2i c 2i + --------
2
- sin α cos α ∑ ∑ w i w j ρ i ρ j c i + j
Σ wi
+ --------- sin2 α ∑ ∑ w i w j ρ i ρ j c i s j – --------- cos2 α ∑ ∑ w i w j ρ i ρ j s i c j
2 2
(82)
Σ wi Σ wi
= cos 2α ∑ w i ρ 2i s 2i – sin 2α ∑ w i ρ 2i c 2i + --------
1
- sin 2α ∑ ∑ w i w j ρ i ρ j c i + j
Σ wi
– --------- ∑ ∑ w i w j ρ i ρ j c i s j ( cos2 α – sin2 α )
2
(83)
Σ wi
= cos 2α  ∑ w i ρ 2i s 2i – -------- w w ρ ρ c s  – sin 2α  ∑ w i ρ i c 2i – --------- ∑ ∑ w i w j ρ i ρ j c i + j
Σ wi ∑ ∑ i j i j i j
2 1
-

2

(84)
Σ wi
From (84) we can obtain the result for α or tan 2α respectively.

--------- ∑ ∑ w i w j ρ i ρ j c i s j – ∑ w i ρ i s 2i
2 2
sin 2α Σ wi
---------------- = --------------------------------------------------------------------------------------------- (85)
--------- ∑ ∑ w i w j ρ i ρ j c i + j – ∑ w i ρ i c 2i
cos 2α 1 2
Σ wi

--------- ∑ ∑ w i w j ρ i ρ j cos θ i sin θ j – ∑ w i ρ i sin 2θ i

2 2
Σ wi
tan 2α = --------------------------------------------------------------------------------------------------------------------------- (86)
--------- ∑ ∑ w i w j ρ i ρ j cos ( θ i + θ j ) – ∑ w i ρ i cos 2θ i
1 2
Σ wi
Equation (86) contains double sums which may not be fully evaluated. Due to the symmetry
of trigonometric functions the corresponding off-diagonal elements can be added and thus sim-
plifies calculation. For the final result (87), the four-quadrant arc tangent has been taken.
This solution, (71) and (87), generates sometimes ( α, r )-pairs with negative r values. They
must be detected in order to change the sign of r and to add π to the corresponding α . All
α -values lie then in the interval – π ⁄ 2 < α ≤ 3π ⁄ 2 .

 Σ wi ∑
- ∑ w i w j ρ i ρ j sin ( θ i + θ j ) + --------- ∑ (w i – Σ w j )wi ρ i sin 2θ i 
2
 -------- 1 2
1 i< j Σ wi 
α = --- atan2  ---------------------------------------------------------------------------------------------------------------------------------------------------------------- (87)
 --------- - (w – Σ w j )wi ρ i cos 2θ i
 Σ wi ∑ ∑ wi w j ρi ρ j cos ( θi + θ j ) + -------- Σ wi ∑ i
2 2 1 2

i< j


16
Appendix B: Deriving the Covariance Matrix for r and α

The model parameter α and r are just half the battle. Besides these estimates of the mean
position of the line we would also like to have a measure for its uncertainty. According to the
error propagation law (1), an approximation of the output uncertainty, represented by C AR , is
subsequently determined.
At the input side, mutually independent uncertainties in radial direction only are assumed.
The 2n × 1 input random vector X = [ P Q ] consists of the random vector P
T T T

= [ P 1, P 2, …, P n ] denoting the variables of the measured radii, and the random vector Q
T

= [ Q 1, Q 2, …, Q n ] holding the corresponding angular variates. The 2n × 2n input covariance

T

matrix C X is therefore of the form

2
CP 0 diag ( σ ρ ) 0
CX = = i . (88)
0 CQ 0 0

We represent both output random variables A , R by their first-order Taylor series expan-
sion about the mean µ = [ µ ρ µ θ ] . The vector µ has dimension 2n × 1 and is composed of
T T T

the two n × 1 mean vectors µ ρ = [ µ ρ1, µ ρ2, …, µ ρn ] and µ θ = [ µ θ1, µ θ2, …, µ θn ] .

T T

∂α ∂α
A ≈ α ( µ∗ ) + ∑ [ Pi – µρ ∗ ] + ∑ [ Qi – µθ ∗ ] (89)
∂ Pi i ∂ Qi i
X = µ∗ X = µ∗

∂r ∂r
R ≈ r ( µ∗ ) + ∑ [ Pi – µρ ∗ ] + ∑ [ Qi – µθ ∗ ] (90)
∂ Pi i ∂ Qi i
X = µ∗ X = µ∗

The relationships r ( . ) and α ( . ) correspond to the results of Appendix A, equations (71) and
(87). Referring to the discussion of chapter 2.2, we do not know µ in advance and its best
available guess is the actual value of the measurements vector µ∗ .
It has been shown that under the assumption of independence of P and Q , the following
holds
∂α 2 2 ∂α  2 2
σ A ≈ ∑   σρ + ∑  (91)
2
σ
 ∂ P i i  ∂ Q i θ i

∂r 2 2 ∂r  2 2
σR ≈ ∑   σρ + ∑  (92)
2
σ
 ∂ P i i  ∂ Q i θ i

We further want to know the covariance σ AR under the abovementioned assumption of negli-
gible angular uncertainties:

COV [ A, R ] = E [ A ⋅ R ] – E [ A ]E [ R ] (93)

= E  α + ∑ ∂α [ P i – µ ρi ] ⋅  r + ∑ ∂r [ P i – µ ρi ] – αr (94)

∂ Pi ∂ Pi

= E αr + r ∑ ∂α [ P i – µ ρi ] + α ∑ ∂r [ P i – µ ρi ] + ∑ ∂α [ P i – µ ρi ] ∑ ∂r [ P i – µ ρi ] – αr (95)
∂ Pi ∂ Pi ∂ Pi ∂ Pi

∂α ∂α ∂r ∂r
= E [ αr ] + rE ∑ ∂ Pi Pi – ∑ ∂ Pi µρ i
+ αE ∑ ∂ Pi Pi – ∑ ∂ Pi µρ i

17
 ∂α ∂r
+E ∑ ∑ ∂ Pi ∂ Pi [ Pi – µρ ] [ P j – µρ ]
i j
– αr (96)

= αr + r ∑ ∂ (α)E [ P i ] – r ∑ ∂α E [ µ ρi ] + α ∑ ∂r E [ P i ] – α ∑ ∂ (r)E [ µ ρi ]
∂ Pi ∂ Pi ∂ Pi ∂ Pi
∂α ∂r ∂α ∂r
∑ ∑ ∂ Pi ∂ Pi [ Pi – µρ ] [ P j – µρ ] + ∑ ∂ Pi ∂ Pi [ Pi – µρ ] (97)
2
+E – αr
i j i
i≠ j

∂α ∂r ∂α ∂r
=E ∑ ∑ ∂ Pi ∂ Pi ( Pi P j – Pi µP ∑ ∂ Pi ∂ Pi [ Pi – µρ ] (98)
2
– P j µP + µP µP ) + E
j i i j i
i≠ j

= ∑ ∑ ∂α ∂r E [ P i P j – P i µ P j – P j µ Pi + µ Pi µ P j ] + ∑ ∂α ∂r E [ ( P i – µ ρi ) 2 ] (99)
i≠ j
∂ Pi ∂ Pi ∂ Pi ∂ Pi

= ∑ ∑ ∂α ∂r E [ P i P j – P i µ P j – P j µ Pi + µ Pi µ P j ] + ∑ ∂α ∂r σ 2ρi (100)
i≠ j
∂ Pi ∂ Pi ∂ Pi ∂ Pi

Since P i and P j are independent, the expected value of the bracketed expression disappears.
Hence
∂α ∂r
∑ ∂ Pi ∂ Pi σρ (101)
2
σ AR =
i

If, however, the input uncertainty model provides non-negligible angular variances, it is
easy to show that under the independency assumption of P and Q the expression keeping
track of Q can be simply added to yield

∂α ∂r ∂α ∂r
∑ ∂ Pi ∂ Pi σρ + ∑ ∂ Qi ∂ Qi σθ . (102)
2 2
σ AR =
i i

As demonstrated in chapter 3, the results (91), (92) and (102) can also be obtained in the
more compact but less intuitive form of equation (1). Let

∂α ∂α ∂α ∂α ∂α ∂α ∂α ∂α
… …
∂P ∂Q ∂ P1 ∂ P2 ∂ Pn ∂ Q1 ∂ Q2 ∂ Qn
F PQ = FP FQ = = (103)
∂r ∂r ∂r ∂r ∂r ∂r ∂r ∂r
… …
∂P ∂Q ∂ P1 ∂ P2 ∂ Pn ∂ Q1 ∂ Q2 ∂ Qn

the composed p × 2n Jacobian matrix containing all partial derivates of the model parameters
with respect to the input random variables about the guess µ∗ . Then, the sought covariance
matrix C AR can be rewritten as

C AR = FPQ C X FPQ
T
. (104)

Under the conditions which lead to equation (102), the right hand side can be decomposed
yielding

T
C AR = F P C P F P + F Q C Q F Q
T
(105)

18
B.1 Practical Considerations

We determine implementable expressions for the elements of covariance matrix C AR . Under

the assumption of negligible angular uncertainties, concrete expressions of σ A , σ R and σ AR
2 2

will be derived. We must furthermore keep in mind that our problem might be a real time
problem, requiring an efficient implementation. Expression are therefore sought which mini-
mize the numer of floating point operations, e.g. by reuse of already computed subexpressions.
Although calculating with weights does not add much difficulty, we will omit them in this
chapter†.
For the sake of brevity we introduce the following notation

α = --- atan2  ----

1 D
 N
(106)
2
with
N = --- ∑ ∑ P i P j cos Q i sin Q j – ∑ P i sin 2Q i ,
2 2
(107)
n

D = --- ∑ ∑ P i P j cos ( Q i + Q j ) – ∑ P i cos 2Q i .

1 2
(108)
n

We will use the result that the parameters α (equation (87)) and r (equation (71)) can also
be written in Cartesian form, where x i = ρ i cos θ i and y i = ρ i sin θ i :

1  –2 ∑ ( y – yi ) ( x – xi ) 
α = --- atan 2  --------------------------------------------------------
2
, (109)
 ∑ ( y – yi ) – ( x – xi ) 
2 2

r = x cos α + y sin α . (110)

They use the means
x = 1 ⁄ n ⋅ ∑ xi y = 1 ⁄ n ⋅ ∑ yi . (111)
From equations (91), (92) and (101) we see that the covariance matrix is defined when both
partial derivates of the parameters with respect to P i are given. Let us start with the derivate
of α .
∂D ∂N ∂D ∂N
N– D N– D
∂α 1 1 ∂ Pi ∂ Pi 1 ∂ Pi ∂ Pi
= --- ⋅ ---------------------------
- ⋅ --------------------------------- = --- ⋅ --------------------------------
- (112)
∂ Pi 2 1 + N 2 ⁄ D2 D
2 2 D +N
2 2

The partial derivates of the numerator and the denominator with respect to P i can be
obtained as follows:

∂N  
= ∂  2--- ∑ ∑ P j P k cos Q j sin Q k  – ∂ { ∑ P 2j sin 2Q j } (113)
∂ Pi ∂ Pi  n  ∂ Pi

= – 2 P i sin 2Q i + 2--- ∂ { P 21 c 1 s 1 + P 1 c 1 P 2 s 2 + P 1 c 1 P 3 s 3 + P 1 c 1 P 4 s 4 + …
n ∂ Pi
2
+ P2 c2 P1 s1 + P2 c2 s2 + P2 c2 P3 s3 + P2 c2 P4 s4 + …
2
+ P3 c3 P1 s1 + P3 c3 P2 s2 + P3 c3 s3 + P3 c3 P4 s4 + … } (114)

† See [ARRAS97] for the results with weights. Performance comparison results of three different ways to
determine α and r are also briefly given.

19
 = 2---  ∂ ∂
{ P cos Q i P j sin Q j } – 2P i sin 2Q i
n ∂ Pi ∑ ∂ Pi ∑ i
{ P j cos Q j P i sin Q i } +

(115)

= 2--- ( sin Q i ∑ P j cos Q j + cos Q i ∑ P j sin Q j ) – 2P i sin 2Q i (116)

n

= 2--- ( sin Q i nx + cos Q i ny ) – 2P i sin 2Q i (117)

n

= 2 ( x sin Q i + y cos Q i – P i sin 2Q i ) (118)

The mean values x and y are those of equation (111).

∂D  
= ∂  1--- ∑ ∑ P j P k cos ( Q i + Q j )  – ∂ { ∑ P 2j cos 2Q j } (119)
∂ Pi ∂ Pi  n  ∂ Pi

= – 2 P i cos 2 Q i + 1--- ∂ { P 21 c 1 + 1 + P 1 P 2 c 1 + 2 + P 1 P 3 c 1 + 3 + P 1 P 4 c 1 + 4 + …
n ∂ Pi
2
+ P2 P1 c2 + 1 + P2 c2 + 2 + P2 P3 c2 + 3 + P2 P4 c2 + 4 + …

+ P3 P1 c3 + 1 + P3 P2 c3 + 2 + P3 c3 + 3 + P3 P4 c3 + 4 + … }
2
(120)
∂ ∂
n ∂ Pi ∑ j i ∂ Pi ∑ i j
= 1--- { P P cos ( Q i + Q j ) } + { P P cos ( Q j + Q i ) } – 2P i cos 2 Q i (121)

= 2--- ∂ { ∑ P j P i cos ( Q i + Q j ) } – 2P i cos 2 Q i (122)

n ∂ Pi

= 2---
n ∑ P j cos ( Qi + Q j ) – 2Pi cos 2 Qi (123)

= 2--- ∑ P j cos Qi cos Q j – --n- ∑ P j sin Qi sin Q j – 2Pi cos 2 Qi

2
(124)
n

= 2--- cos Q i ∑ P j cos Q j – 2--- sin Q i ∑ P j sin Q j – 2P i cos 2 Q i (125)

n n

= 2--- cos Q i nx – 2--- sin Q i ny – 2P i cos 2 Q i (126)

n n

= 2 ( x cos Q i – y sin Q i – P i cos 2 Q i ) (127)

Substituting into equation (112) gives
∂D ∂N
N– D
∂α 1 ∂ Pi ∂ Pi
= ⋅
--
- --------------------------------
- (128)
∂ Pi 2 D +N
2 2

2 ( x cos Q – y sin Q – P cos 2 Q )N – 2 ( x sin Q + y cos Q – P sin 2Q )D

= 1--- ⋅ ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
i i i i
2 2
i i i i
- (129)
2 D +N

N ( x cos Q – y sin Q – P cos 2 Q ) – D ( x sin Q + y cos Q – P sin 2Q )

= --------------------------------------------------------------------------------------------------------------------------------------------------------------------------
i i i i
2 2
i i i i
- (130)
D +N
It remains the derivate of r :

∂r  
= ∂  1--- ∑ P j cos ( Q j – α )  (131)
∂ Pi ∂ Pi  n 

= 1--- ∑ ∂ { P j cos ( Q j – α ) } (132)

n ∂ Pi

20
 = 1--- ∑ cos ( Q j – α ) ∂ { P j } + 1--- ∑ P j ∂ { cos ( Q j – α ) } (133)
n ∂ Pi n ∂ Pi

= 1--- cos ( Q i – α ) + 1--- ∑ P j sin ( Q j – α ) ∂α (134)

n n ∂ Pi

= 1--- cos ( Q i – α ) + ∂α 1--- ∑ P j sin ( Q j – α ) (135)

n ∂ Pi n

= 1--- cos ( Q i – α ) + ∂α ⋅ ∂r (136)

n ∂ Pi ∂ α

= 1--- cos ( Q i – α ) + ∂α ( y cos α – x sin α ) (137)

n ∂ Pi

Note that we made use of the already known expression ∂α ⁄ ∂P i .

Let us summarize the results. The sought covariance matrix is

2
σ A σ AR
C AR = (138)
2
σ AR σ R

with elements

2∑
1
2
σA = --------------------------- 2 2
- [ N ( x cos θ i – y sin θ i – ρ i cos 2 θ i ) – D ( x sin θ i + y cos θ i – ρ i sin 2θ i ) ] σ ρ (139)
2 2 i
(D + N )

= ∑ 1--- cos ( θ i – α ) + ∂α ( y cos α – x sin α ) σ 2ρi

2
2
σR (140)
n ∂ Pi

σ AR = ∑ ∂α ∂r σ 2ρi (141)
∂ Pi ∂ Pi

All elements of C AR are of complexity O ( n ) and allow extensive reuse of already computed
expressions. This makes them suitable for fast calculation under real time conditions and lim-
ited computing power.

21