PROBABILISTIC ROBOTICS:

RECURSIVE STATE ESTIMATION

Pierre-Paul TACHER

1
Let Xn the random variable indicating the state of the range sensor at the date n. More precisely,
(
0 if the sensor is faulty,
Xn =
1 if the sensor is working correctly.

Assuming the state of the sensor does not change, we have that

∀n ∈ N, Xn = X0
P (X0 = 0) = p = 0.01
P (X0 = 1) = 1 − p = 0.99

Let Zn ∈ [0, 3] the real valued random variable giving the range measured by the sensor. By the problem
statement,

∀n ∈ N, Zn = zn ∈ [0, 1]

P (Xn = 0 | z1:n ) = ηP (zn | Xt = 0, z1:n−1 ) × P (Xn = 0 | z1:n−1 )

= ηP (zn | Xt = 0, z1:n−1 ) × P (Xn−1 = 0 | z1:n−1 ) (the update step is trivial
since the state does not change)
= η × 1 × P (Xn−1 = 0 | z1:n−1 )
= ηP (X0 = 0) (recurrence)
= ηp

P (Xn = 1 | z1:n ) = ηP (zn | Xt = 1, z1:n−1 ) × P (Xn = 1 | z1:n−1 )

= ηP (zn | Xt = 1, z1:n−1 ) × P (Xn−1 = 1 | z1:n−1 )
1
= η × × P (Xn−1 = 1 | z1:n−1 )
3
1
= η × n × P (X0 = 1) (recurrence)
3
1
= η × n × (1 − p)
3
Note that the value of the normalizer η changes along the recurrence although it is not reflected in the
notation. We can compute the normalizer:
1
η= 1
p+ 3n (1 − p)

The posterior probability of sensor fault at date n is therefore:

p
P (Xn = 0 | z1:n ) = 1
p+ − p)
3n (1
It comes as no surprise this increases to 1 as the number of observation goes to infinity. Let’s compute some
1
2 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION

numerical values:
n P (Xn = 0 | z1:n )
1 0.0294
2 0.0833
3 0.2143
4 0.4500
5 0.7105
6 0.8804
7 0.9567
8 0.9851
9 0.9950
10 0.9983

2
Let Xt be the random variable which is the wheather of day t. We define (”sunny”, ”cloudy”, ”rainy”) =
(1, 2, 3).

2.1.
P (X1 = 1 ∩ X2 = 2 ∩ X3 = 2 ∩ X4 = 3)
= P (X4 = 3 | X1 = 1 ∩ X2 = 2 ∩ X3 = 2) × P (X1 = 1 ∩ X2 = 2 ∩ X3 = 2)
= P (X4 = 3 | X3 = 2) × P (X3 = 2 | X1 = 1 ∩ X2 = 2) × P (X1 = 1 ∩ X2 = 2)
= P (X4 = 3 | X3 = 2) × P (X3 = 2 | X2 = 2) × P (X2 = 2 | X1 = 1) × P (X1 = 1)
= 0.2 × 0.4 × 0.2 × 1
= 0.016

2.2.

2.3.

2.4. Let
 
P (Xt = 1)
Yt = P (Xt = 2)
P (Xt = 3)
 
P (Xt+1 = 1 | Xt = 1) P (Xt+1 = 1 | Xt = 2) P (Xt+1 = 1 | Xt = 3)
A = P (Xt+1 = 2 | Xt = 1) P (Xt+1 = 2 | Xt = 2) P (Xt+1 = 2 | Xt = 3)
P (Xt+1 = 3 | Xt = 1) P (Xt+1 = 3 | Xt = 2) P (Xt+1 = 3 | Xt = 3)
 
0.8 0.4 0.2
= 0.2 0.4 0.6
0 0.2 0.2
We have
Yt = AYt−1
so that
∀t ∈ N, Yt = An Y0
The matrix A is diagonalizable
A = P DP −1
 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION 3

with
 √ √ 
9 2√−1 − √2 − 1
P = 4 − 2 2 
1 1 1
 1 1 1 
14 14 14
 − 2(√2−4)
√ √ √
− 2( 2+10)
√ √
2(13 2+4) 
P −1 =


 56 56 56 
√ √ √ √ √ √
− 2( 2+4) − 2( 2−10) 2(13 2−4)
56 56 56
 
1 0√ 0
1+ 2
D = 0 0√ 
 
5
1− 2
0 0 5
Since
 
1 0 0
lim Dn = 0 0 0
n→∞
0 0 0
we have
 
1 0 0
lim Y n = P 0 0 0 P −1 X0
n→∞
0 0 0
9
14
2 
=

7 
1
14
whatever the initial distribution X0 is.
The stationary distribution is
9 
14
2
Π= 7
1
14

2.5. Let’s compute the conditional probability distribution of Xt−1 given Xt :

∀(i, j) ∈ J1, 3K2 , P (Xt−1 = i | Xt = j) = ηP (Xt = j | Xt−1 = i) × P (Xt−1 = i)
do not depend on t depends on t
z }| { z }| {
P (Xt = j | Xt−1 = i) × P (Xt−1 = i)
=
P (Xt = j)
| {z }
depends on t

We can consider the limit distribution as t goes to infinity:

P (Xt = j | Xt−1 = i) × Π[i]
mi,j =
Π[j]
aj,i × Π[i]
=
Π[j]
This gives the following transition matrix, rounded to 2 decimal place:
 
0.80 0.45 0
M = 0.18 0.40 0.80
0.02 0.15 0.20

2.6. The markov property states the probability law of future state conditioned on the current state does
not depend on any other variables: the current state should be sufficient to compute the future stochastic
evolution. This implies the state transition function can not depend on the season. Therefore, in order to
restore the Markov property, we could incorporate the season into the state variable.
4 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION

3
Let (Zt )t∈N∗ denote the sequence of random variables measuring the wheather of the day.

∀t ∈ N∗ , Zt ∈ J1, 3K

3.1. We know for a fact that day 1 is sunny, and we do the following observations from day 2 to day 5:

t zt
2 2
3 2
4 3
5 1

We compute the distributions for (Xt )t∈J2,5K , being given the observations from the past until the current
point of time:

P (X2 = 1 | z2 ) = ηP (Z2 = 2 | X2 = 1) × P (X2 = 1)

= η × 0.4 × 0.8
= 0.32η
P (X2 = 2 | z2 ) = ηP (Z2 = 2 | X2 = 2) × P (X2 = 2)
= η × 0.7 × 0.2
= 0.14η
P (X2 = 3 | z2 ) = ηP (Z2 = 2 | X2 = 3) × P (X2 = 3)
=0

1
η=
0.32 + 0.14
1
=
0.46

i P (X2 = i | z2 )
1 0.70
2 0.30
3 0
 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION 5

P (X3 = 1 | z2:3 ) = ηP (Z3 = 2 | X3 = 1, z2 ) × P (X3 = 1 | z2 )

X3
= ηP (Z3 = 2 | X3 = 1) × P (X3 = 1 | X2 = i, z2 ) × P (X2 = i | z2 )
i=1
3
X
= ηP (Z3 = 2 | X3 = 1) × P (X3 = 1 | X2 = i) × P (X2 = i | z2 )
i=1
= η × 0.4 × 0.6783
= 0.271η
P (X3 = 2 | z2:3 ) = ηP (Z3 = 2 | X3 = 2, z2 ) × P (X3 = 2 | z2 )
X3
= ηP (Z3 = 2 | X3 = 2) × P (X3 = 2 | X2 = i, z2 ) × P (X2 = i | z2 )
i=1
3
X
= ηP (Z3 = 2 | X3 = 2) × P (X3 = 2 | X2 = i) × P (X2 = i | z2 )
i=1
= η × 0.7 × 0.2609
= 0.183η
P (X3 = 3 | z2 ) = ηP (Z2 = 2 | X3 = 3) × P (X3 = 3 | z2 )
=0

1
η=
0.183 + 0.271
1
=
0.4539

i P (X3 = i | z2:3 )
1 0.60
2 0.40
3 0

P (X4 = 1 | z2:4 ) = ηP (Z4 = 3 | X4 = 1, z2:3 ) × P (X4 = 1 | z2:3 )

=0
P (X4 = 2 | z2:4 ) = ηP (Z4 = 3 | X4 = 2, z2:3 ) × P (X4 = 2 | z2:3 )
=0
P (X4 = 3 | z2:4 ) = 1

i P (X4 = i | z2:4 )
1 0
2 0
3 1
6 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION

P (X5 = 1 | z2:5 ) = ηP (Z5 = 1 | X5 = 1, z2:4 ) × P (X5 = 1 | z2:4 )

X 3
= ηP (Z5 = 1 | X5 = 1) × P (X5 = 1 | X4 = i, z2:4 ) × P (X4 = i | z2:4 )
i=1
= ηP (Z5 = 1 | X5 = 1) × P (X5 = 1 | X4 = 3) × 1
= η × 0.6 × 0.2
= 0.12η
P (X5 = 2 | z2:5 ) = ηP (Z5 = 1 | X5 = 2, z2:4 ) × P (X5 = 2 | z2:4 )
X 3
= ηP (Z5 = 1 | X5 = 2) × P (X5 = 2 | X4 = i, z2:4 ) × P (X4 = i | z2:4 )
i=1
= ηP (Z5 = 1 | X5 = 2) × P (X5 = 2 | X4 = 3) × 1
= η × 0.3 × 0.6
= 0.18η
P (X5 = 3 | z2:5 ) = 0

1
η=
0.18 + 0.12
1
=
0.30

i P (X5 = i | z2:5 )
1 0.40
2 0.60
3 0

3.2. We know for a fact that day 1 is sunny, and we do the following observations from day 2 to day 5:

t zt
2 1
3 1
4 3

We compute the distributions for (Xt )t∈J2,5K , being given the observations from the past until the current
point of time:

P (X2 = 1 | z2 ) = ηP (Z2 = 1 | X2 = 1) × P (X2 = 1)

= η × 0.6 × 0.8
= 0.48η
P (X2 = 2 | z2 ) = ηP (Z2 = 1 | X2 = 2) × P (X2 = 2)
= η × 0.3 × 0.2
= 0.06η
P (X2 = 3 | z2 ) = ηP (Z2 = 1 | X2 = 3) × P (X2 = 3)
=0

1
η=
0.48 + 0.06
1
=
0.54
 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION 7

i P (X2 = i | z2 )
1 0.89
2 0.11
3 0

P (X3 = 1 | z2:3 ) = ηP (Z3 = 1 | X3 = 1, z2 ) × P (X3 = 1 | z2 )

X3
= ηP (Z3 = 1 | X3 = 1) × P (X3 = 1 | X2 = i, z2 ) × P (X2 = i | z2 )
i=1
3
X
= ηP (Z3 = 1 | X3 = 1) × P (X3 = 1 | X2 = i) × P (X2 = i | z2 )
i=1
= η × 0.6 × 0.7560
= 0.4536η
P (X3 = 2 | z2:3 ) = ηP (Z3 = 1 | X3 = 2, z2 ) × P (X3 = 2 | z2 )
X3
= ηP (Z3 = 1 | X3 = 2) × P (X3 = 2 | X2 = i, z2 ) × P (X2 = i | z2 )
i=1
3
X
= ηP (Z3 = 1 | X3 = 2) × P (X3 = 2 | X2 = i) × P (X2 = i | z2 )
i=1
= η × 0.3 × 0.222
= 0.0666η
P (X3 = 3 | z2:3 ) = ηP (Z3 = 1 | X3 = 3, z2 ) × P (X3 = 3 | z2 )
X3
= ηP (Z3 = 1 | X3 = 3) × P (X3 = 3 | X2 = i, z2 ) × P (X2 = i | z2 )
i=1
3
X
= ηP (Z3 = 1 | X3 = 3) × P (X3 = 3 | X2 = i) × P (X2 = i | z2 )
i=1
=0
1
η=
0.4536 + 0.0666
1
=
0.5202

i P (X3 = i | z2:3 )
1 0.87
2 0.13
3 0

P (X4 = 1 | z2:4 ) = ηP (Z4 = 3 | X4 = 1, z2:3 ) × P (X4 = 1 | z2:3 )

=0
P (X4 = 2 | z2:4 ) = ηP (Z4 = 3 | X4 = 2, z2:3 ) × P (X4 = 2 | z2:3 )
=0
P (X4 = 3 | z2:4 ) = 1

i P (X4 = i | z2:4 )
1 0
2 0
3 1
8 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION

We will now compute the distribution of random variables (Xt )t∈J2,4K , conditionned on both past and future
observations of the sensor. Let’s define the transition matrix
 
P (Zt = 1 | Xt = 1) P (Zt = 1 | Xt = 2) P (Zt = 1 | Xt = 3)
C = P (Zt = 2 | Xt = 1) P (Zt = 2 | Xt = 2) P (Zt = 2 | Xt = 3)
P (Zt = 3 | Xt = 1) P (Zt = 3 | Xt = 2) P (Zt = 3 | Xt = 3)
 
0.6 0.3 0
= 0.4 0.7 0
0 0 1

The sensor gives us the measurement:

t zt
2 1
3 1
4 3

P (X2 = 1 | z2:4 )
= ηP (Z2 = 1 | X2 = 1, z3:4 ) × P (X2 = 1 | z3:4 )
= ηP (Z2 = 1 | X2 = 1) × P (Z3 = 1 | X2 = 1, z4 ) × P (X2 = 1 | z4 )
X3
= ηP (Z2 = 1 | X2 = 1) × ( P (Z3 = 1 | X3 = i, X2 = 1, z4 ) × P (X3 = i | X2 = 1, z4 ))
i=1
× P (X2 = 1 | z4 )
X3
= ηP (Z2 = 1 | X2 = 1) × ( P (Z3 = 1 | X3 = i) × P (X3 = i | X2 = 1))
i=1
× P (Z4 = 3 | X2 = 1) × P (X2 = 1)
X3
= ηP (Z2 = 1 | X2 = 1) × ( P (Z3 = 1 | X3 = i) × P (X3 = i | X2 = 1))
i=1
| {z }
À
X3
×( P (z4 = 3 | X4 = j, X2 = 1) × P (X4 = j | X2 = 1)) × P (X2 = 1)
j=1
| {z }
Á
À = 0.6 × 0.8 + 0.3 × 0.2
= 0.54
X3 3
X
Á= P (Z4 = 3 | X4 = j) × ( P (X4 = j | X3 = k) × P (X3 = k | X2 = 1))
j=1 k=1
3 X
X 3
= P (Z4 = 3 | X4 = j) ×P (X4 = j | X3 = k) × P (X3 = k | X2 = 1)
| {z }
j=1 k=1
δj,3
3
X
= P (X4 = 3 | X3 = k) × P (X3 = k | X2 = 1)
k=1
= 0.2 × 0.2 + 0.2 × 0 + 0
= 0.04
P (X2 = 1 | z2:4 ) = η × 0.6 × 0.54 × 0.04 × 0.8
= 0.0104η
 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION 9

We observe that

À = P (Z3 = 1 | X2 = 1, z4 )
= (CA)1,1
Á = P (Z4 = 3 | X2 = 1)
= (CAA)3,1

 
0.54 0.36 0.30
CA = 0.46 0.44 0.50
0 0.20 0.20
 
0.5040 0.42 0.3840
CAA =  0.456 0.46 456 
0.04 0.12 0.16

Then,

P (X2 = 2 | z2:4 )
= ηP (Z2 = 1 | X2 = 2) × P (Z3 = 1 | X2 = 2, z4 ) × P (Z4 = 3 | X2 = 2) × P (X2 = 2)
= ηP (Z2 = 1 | X2 = 2) × (CA)1,2 × (CAA)3,2 × P (X2 = 2)
= η0.3 × 0.36 × 0.12 × 0.2
= 0.0026η
P (X2 = 3 | z2:4 )
= ηP (Z2 = 1 | X2 = 3) × P (Z3 = 1 | X2 = 3, z4 ) × P (Z4 = 3 | X2 = 3) × P (X2 = 2)
=0
1
η=
0.0104 + 0.0026
1
=
0.013

i P (X2 = i | z2:4 )
1 0.8
2 0.2
3 0
10 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION

P (X3 = 1 ∩ Z4 = 3 | z2:3 )
P (X3 = 1 | z2:4 ) =
P (Z4 = 3 | z2:3 )
= ηP (Z4 | X3 = 1, z2:3 ) × P (X3 = 1 | z2:3 )
X 3
=η P (Z4 = 3 ∩ X4 = i | X3 = 1, z2:3 ) × P (X3 = 1 | z2:3 )
i=1
3
X
=η P (X4 = i | X3 = 1, z2:3 ) × P (Z4 = 3 | X4 = i, z2:3 ) × P (X3 = 1 | z2:3 )
i=1
X3
=η P (X4 = i | X3 = 1) × P (Z4 = 3 | X4 = i) ×P (X3 = 1 | z2:3 )
| {z }
i=1
δ3,i
= ηP (X4 = 3 | X3 = 1) × P (Z4 = 3 | X4 = 3) × P (X3 = 1 | z2:3 )
=0
P (X3 = 2 | z2:4 ) = ηP (X4 = 3 | X3 = 2) × P (Z4 = 3 | X4 = 3) × P (X3 = 2 | z2:3 )
= η × 0.2 × 1 × 0.21
= 0.042η
P (X3 = 3 | z2:4 ) = ηP (X4 = 3 | X3 = 3) × P (Z4 = 3 | X4 = 3) × P (X3 = 3 | z2:3 )
=0

i P (X3 = i | z2:4 )
1 0
2 1
3 0

For day 4 this is trivial:

i P (X4 = i | z2:4 )
1 0
2 0
3 1

4
4.1. Using the notation of the book for probability density functions,
2
(x−xinit )
1 −
2σ 2
p(x) = √ e init
2πσinit
(z−x)2
1 −
2σ 2
p(z | x) = √ e GP S
2πσGP S

4.2. We have the following property for conditional probability density functions:

p(z | x) × p(x)
p(x | z) =
p(z)
 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION 11

We first compute the probability density of Z:

Z ∞
p(z) = p(z | x) × p(x) dx
−∞
Z∞ (z−x) 2 (x−xinit ) 2
1 − 2 1 −
2σ 2
= √ e 2σGP S √ e init dx
−∞ 2πσGP S 2πσinit
Z ∞ (z−x)2 (x−xinit )2
1 −
2σ 2
1 − 2
= √ e GP S √ e 2×3σGP S dx
−∞ 2πσGP S 2π × 9σGP S
Z ∞ (z−x)2 (x−xinit )2
1 −
2σ 2
−
2×3σ 2
= 2 e GP S GP S dx
9 × 2πσGP S −∞

(z − x)2 (x − xinit )2 9(z 2 − 2xz + x2 ) + x2 − 2xxinit + x2init

2 + 2 = 2
2σGP S 2 × 9σGP S 18σGP S
9z + 10x − 2x(9z + xinit ) + x2init
2 2
= 2
18σGP S
2
9z 2 + x2init − (9z+x10init ) (x − (9z+x
10
init ) 2
)
= 2 + 9 2
18σGP 2 × 10 σGP S
| {z S }
does not depend on x

Thus,
(9z+xinit )2
− 9z 2 −x2
√
r
init +
1 18σ 2
10
9
p(z) = 2 e GP S × 2π σGP S
3 × 2πσGP S 10
− 9 z 2 − 9 x2 +2× 9 xinit
1 10 10 init
2×9σ 2
10
=√ √ e GP S
2π 10σGP S
(z−xinit )2
1 −
2×10σ 2
=√ √ e GP S
2π 10σGP S
2
This shows Z has a normal distribution Z ,→ N (xinit , 10σGP S ). It follows that
(9z+xinit ) 2
(z−xinit )2 (x−
10
)
− −
1 2×10σ 2 2× 9 σ 2
2
3×2πσGP
e GP S e 10 GP S

p(x | z) = S
(z−xinit )2
−
2×10σ 2
√ √1 e GP S
2π 10σGP S
(9z+xinit ) 2
(x− )
1 − 10
2× 9 σ 2
=√ e 10 GP S
2π √310 σGP S

From which we conclude

The distribution of random variable X conditioned on (Z = z) is N ( 9z+x

10
init 9 2
, 10 σGP S ).

5
Being sloppy on technical details,
p(x, y | z)
p(x | z, y) =
p(y | z)
p(x | z) × p(y | z)
=
p(y | z)
= p(x | z)
12 PROBABILISTIC ROBOTICS: RECURSIVE STATE ESTIMATION

6
Classical derivation. Z
E[X − E[X]]2 = (X − E[X])2 dP
ZΩ
= (X 2 − 2XE[X] + (E[X])2 ) dP
ZΩ Z Z
= X 2 dP − 2E[X] X dP + (E[X])2 1 dP
Ω Ω Ω
= E[X 2 ] − 2(E[X])2 + (E[X])2
E[X − E[X]]2 = E[X 2 ] − (E[X])2