6.4.5 Environment Sensing

The measurement made in the environment can be used to improve the estimation of the state X (e.g., location) in this environment. Imagine that we are sleepwalking around the house in the middle of the night. When we wake up, we can figure out where we are by using our senses (sight, touch, etc.).

Mathematically, the initial knowledge about the environment can be described with probability distribution p(x) (prior). This distribution can be improved when a new measurement z (with belief p(z|x)) is available if the probability after the measurement is determined p(x|z). This can be achieved using the Bayesian rule p(x|z)=bel(x)=p(z|x)p(x)p(z)si122_e. The probability p(z|x) represents the statistical model of the sensor, and bel(x) is state-estimate belief after the measurement is made. In the process of perception, the correction step of the Bayesian filter is evaluated.

Example 6.9

Considering Example 6.8, assume that the mobile system detects a dark cell Z = dark. The mobile system detects a dark cell with probability 0.6 and makes a mistake with probability 0.2 (detects a bright cell as dark). Hence,

p(Z=dark|X=xd)=0.6,d∈{3,4}p(Z=dark|X=xb)=0.2,b∈{1,2,5}

si123_e

where index b indicated bright cells and index d indicated dark cells. In the beginning, the mobile system does not know its position. This can be described with a uniform probability distribution P(X = xi) = bel(xi) = 0.2, i ∈{1, …, 5}. Calculate the location probability distribution after a single measurement.

Solution

We would like to determine the distribution of conditional probability p(X1|Z = dark), the state-estimate belief after the measurement is made. The desired probability can be determined using the correction step of the Bayesian filter:

p(X1|Z=dark)=p(Z=dark|X1)*p(X1)P(Z=dark)=[0.2,0.2,0.6,0.6,0.2]T*[0.2,0.2,0.2,0.2,0.2]TP(Z=dark)=[0.04,0.04,0.12,0.12,0.04]TP(Z=dark)

si124_e

where the operator * represents the operation of element-wise multiplication of vector elements. We need to calculate the probability of detecting a dark cell P(Z = dark). Therefore the total probability must be evaluated, that is, the probability of detecting a dark cell considering all the cells:

P(Z=dark)=∑iP(Z=dark|X1=xi)P(X1=xi)=pT(Z=dark|X1)p(X1)=[0.2,0.2,0.6,0.6,0.2][0.2,0.2,0.2,0.2,0.2]T=0.36

si125_e

The posterior probability distribution is therefore the following:

p(X1|Z=dark)=[0.11,0.11,0.33,0.33,0.11]T

si126_e

Hence, we can conclude that the position of the mobile system is three times more likely to be in cells 3 or 4 than in the remaining three cells. The probability distributions are also shown graphically in Fig. 6.10. The solution of this example is also given in Listing 6.4.

f06-10-9780128042045
Fig. 6.10 Probability distributions from Example 6.9.

Listing 6.4

Implementation of the solution of Example 6.9

1 disp( ’Sensor measurement distribution p(Z = dark|X ) ’)

2 p_ZdX= [ 0 . 2 0.2 0.6 0.6 0 . 2 ]

3 disp( ’Sensor measurement distribution p(Z = bright|X ) ’)

4 p_ZbX=  1− p_ZdX

5 disp( ’ Initial  distribution p(X ) ’)

6 p_X=  ones (1 ,5) /5

7

8 disp( ’Probability of detecting a darkc e l l  P (Z = dark) ’)

9 P_Zd=  p_ZdX*p_X. ’

10

11 disp( ’Posterior distribution p(X |Z = dark) ’)

12 p_XZd=  p_ZdX.*p_X/P_Zd

Sensor measurement distribution p ( Z = dark | X )

p_ZdX =

0.2000 0.2000 0.6000 0.6000 0.2000

Sensor measurement distribution p ( Z = bright | X )

p_ZbX =

0.8000 0.8000 0.4000 0.4000 0.8000

Initial distribution p ( X )

p_X =

0.2000 0.2000 0.2000 0.2000 0.2000

Probability of detecting a dark c e l l P ( Z = dark )

P_Zd =

0.3600

Posterior distribution p ( X | Z = dark )

p_XZd =

0.1111 0.1111 0.3333 0.3333 0.1111

Example 6.10

Answer the following questions about Example 6.9:

1. Can multiple measurements improve the estimated mobile system position (the mobile system does not move between the measurements)?

2. What is the probability distribution of the mobile system position if the mobile system detects a tile as dark twice in a row?

3. What is the probability distribution of the mobile system position if the mobile system detects a tile as dark and then as bright?

4. What is the probability distribution of the mobile system position if the mobile system detects a tile as dark, then as bright, and then again as dark?

Solution

1. Multiple measurements can improve the estimate about the mobile system position if the probability of the correct measurement is higher than the probability of the measurement mistake.

2. The probability distribution if the sensor detects cell as dark twice in a row is the following:

p(X2|Z1=dark,Z2=dark)=p(X2|z1,z2)=p(z2|X2)*p(X2|z1)P(z2|z1)=[0.2,0.2,0.6,0.6,0.2]T*[0.11,0.11,0.33,0.33,0.11]TP(z2|z1)

si127_e

where jth element in the probability distribution p(X2|z1) is given by P(X2 = xj|z1) =pT(X2 = xj|X1)p(X1|z1) = P(X1 = xj|z1), since we have no influence on the states (see Example 6.5); we only observe the states with measurements. The conditional probability in the denominator is

P(z2|z1)=∑xiP(z2|X2=xi)P(X2=xi|z1)=pT(z2|X2)p(X2|z1)=[0.2,0.2,0.6,0.6,0.2][0.11,0.11,0.33,0.33,0.11]T=0.4667

si128_e

Finally, the solution is

p(X2|z1,z2)=[0.2,0.2,0.6,0.6,0.2]T*[0.11,0.11,0.33,0.33,0.11]T[0.2,0.2,0.6,0.6,0.2][0.11,0.11,0.33,0.33,0.11]T=[0.0476,0.0476,0.4286,0.4286,0.0476]T

si129_e

3. The bright cell is detected correctly with probability p(Z = bright|X = bright) = 1 − p(Z = dark|X = bright) = 0.8 and incorrectly with probability p(Z = bright|X = dark) = 1 − p(Z = dark|X = dark) = 0.4. The second measurement can be made based on the probability distribution p(X2|Z1 = dark):

p(X2|Z1=dark,Z2=bright)=p(X2|z1,z2)=[0.8,0.8,0.4,0.4,0.8]T*[0.11,0.11,0.33,0.33,0.11]TP(Z2=bright|Z1=dark)

si130_e

P(Z2=bright|Z1=dark)=∑xiP(Z2=bright|X2=xi)P(X2=xi|Z1=dark)=pT(Z2=bright|X2)p(X2|Z1=dark)=[0.8,0.8,0.4,0.4,0.8][0.11,0.11,0.33,0.33,0.11]T=0.533

si131_e

p(X2|Z1=dark,Z2=bright)=[0.167,0.167,0.25,0.25,0.167]T

si132_e

4. The state probability distribution after three measurements were made is

p(X3|Z1=dark,Z2=bright,Z3=dark)=[0.083,0.083,0.375,0.375,0.083]T

si133_e

The implementation of the solution in Matlab is shown in Listing 6.5. The posterior state probability distributions for three time steps are presented graphically in Fig. 6.11.

f06-11-9780128042045
Fig. 6.11 Posterior state probability distributions in three time steps from the fourth case in Example 6.9.

Listing 6.5

Implementation of the solution of Example 6.10

1 p_ZdX =[ 0 . 2 0.2 0.6 0.6 0 . 2 ] ;

2 p_ZbX = 1−p_ZdX;

3 p_X = ones (1 ,5) /5;

4

5 disp( ’Probability of detecting a darkt i l e  P (Z1= dark) ’)

6 P_z1=  p_ZdX*p_X. ’

7 disp( ’Posterior distribution p(X1 |Z1= dark) ’)

8 p_Xz1=  p_ZdX.*p_X/P_z1

9

10 disp( ’Probability of detecting a brightt i l e  P (Z2= bright|Z1= dark) ’)

11 P_z2=  p_ZbX*p_Xz1 . ’

12 disp( ’Posterior distribution p(X2 |Z1= dark,Z2= bright) ’)

13 p_Xz2=  p_ZbX.* p_Xz1/P_z2

14

15 disp( ’Probability of detecting a darkt i l e  P (Z3= dark|Z1= dark,Z2= bright) ’)

16 P_z3=  p_ZdX*p_Xz2 . ’

17 disp( ’Posterior distribution p(X3 |Z1= dark,Z2= bright ,Z3= dark) ’)

18 p_Xz3=  p_ZdX.* p_Xz2/P_z3

Probability of detecting a dark t i l e P ( Z1 = dark )

P_z1 =

0.3600

Posterior distribution p ( X1 | Z1 = dark )

p_Xz1 =

0.1111 0.1111 0.3333 0.3333 0.1111

Probability of detecting a bright t i l e P ( Z2 = bright | Z1 = dark )

P_z2 =

0.5333

Posterior distribution p ( X2 | Z1 = dark , Z2 = bright )

p_Xz2 =

0.1667 0.1667 0.2500 0.2500 0.1667

Probability of detecting a dark t i l e P ( Z3 = dark | Z1 = dark , Z2 = bright )

P_z3 =

0.4000

Posterior distribution p ( X3 | Z1 = dark , Z2 = bright , Z3 = dark )

p_Xz3 =

0.0833 0.0833 0.3750 0.3750 0.0833

6.4.6 Motion in the Environment

Mobile systems can move in the environment using some actuators (e.g., motorized wheels) and a control system. Every movement has small or large uncertainty; therefore, the movement of the mobile system through the environment increases the uncertainty about the mobile system state (pose) in the environment.

Consider that we are standing in well-known environment. We close our eyes and make several steps. After a few steps we know approximately where we are, since we know how big are our steps and we know in which direction the steps were made. Therefore, we can imagine where we are. However, the lengths of our steps are not known precisely, and also the directions of the steps are hard to estimate; therefore, our knowledge about our pose in space decreases with time as more and more steps are made.

In the case of movement without observing the states through measurement, the relation (6.28) can be reformulated:

p(xk|u0:k−1)=∫−∞+∞p(xk|xk−1,uk−1)p(xk−1|u1:k−2)dxk−1

si134_e

The belief in the new state p(xk|u0:k−1) depends on the belief in the previous time step p(xk−1|u0:k−2) and conditional state transition probability p(xk|xk−1, uk−1). The probability distribution p(xk|, u0:k−1) can be determined with integration (or summation in the discrete case) of all possible state transition probabilities p(xk|xk−1, uk−1) from previous states xk−1 into state xk, given the known action uk−1.

Example 6.11

Consider again Example 6.8 and assume that the initial position of the mobile system is in the first cell (X0 = x1). The initial state can be written with probability distribution p(X0) = [1, 0, 0, 0, 0]. The mobile system can move between cells, the outcome of movement action is correct in 80%, in 10% the movement of the mobile system is one cell less than required, and in 10% the mobile system moves one cell more than required. This can be described with the following state transition probabilities:

P(Xk=xi|Xk−1=xj,Uk−1=u)=0.8;fori=j+uP(Xk=xi|Xk−1=xj,Uk−1=u)=0.1;fori=j+u−1P(Xk=xi|Xk−1=xj,Uk−1=u)=0.1;fori=j+u+1

si135_e

The mobile system has to make a movement for two cells in the counter-clockwise direction (U0 = 2). Determine the mobile system position belief after the movement is made.

Solution

The probability distribution (belief) after the movement is made can be determined if the probabilities of the mobile system positions in every cell are calculated (total probability). The mobile system can arrive into the first cell only from cell 3 (too long move), cell 4 (correct move), and cell 5 (too short move). This yields the probability distribution of the transition into the first cell p(X1 = x1|X0, U0 = 2) = [0, 0, 0.1, 0.8, 0.1]T. After the movement the mobile system is in the first cell with probability

P(X1=x1|U0=2)=∑xiP(X1=x1|X0=xi,U0=2)P(X0=xi)=pT(X1=x1|X0,U0=2)p(X0)=[0,0,0.1,0.8,0.1][1,0,0,0,0]T=0

si136_e

The probability that the mobile system after the movement is in the second cell is

P(X1=x2|U0=2)=∑xiP(X1=x2|X0=xi,U0=2)P(X0=xi)=pT(X1=x2|X0,U0=2)p(X0)=[0.1,0,0,0.1,0.8][1,0,0,0,0]T=0.1

si137_e

Similarly, the probabilities of all the other cells can be calculated:

P(X1=x3|U0=2)=[0.8,0.1,0,0,0.1][1,0,0,0,0]T=0.8P(X1=x4|U0=2)=[0.1,0.8,0.1,0,0][1,0,0,0,0]T=0.1P(X1=x5|U0=2)=[0,0.1,0.8,0.1,0][1,0,0,0,0]T=0

si138_e

The mobile position belief after the movement is therefore

p(X1|U0=2)=[0,0.1,0.8,0.1,0]T

si139_e

The posterior state probability distributions are presented graphically in Fig. 6.12. The implementation of the solution in Matlab is shown in Listing 6.6.

f06-12-9780128042045
Fig. 6.12 Posterior state probability distributions in two time steps from Example 6.11.

Listing 6.6

Implementation of the solution of Example 6.11

1 disp( ’ Initial belief  p(X0 ) ’)

2 p_X0= [1  0 0 00]

3

4 P_xxu_null= 0 . 8 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u

5 P_xxu_less= 0 . 1 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u−1

6 P_xxu_more = 0 . 1 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u+1

7

8 disp( ’Belief p(X1 |U0 =2) ’) ;

9 p_xXu= [0  0 P_xxu_more P_xxu_null P_xxu_less ] ; %  for U =2

10 p_Xu=  zeros(1 ,5) ;

11     fori =1:5

12     p_Xu( i )=  p_xXu*p_X0 . ’ ;

13 p_xXu=  p_xXu ( [ end1:end−1]) ;

14 end

15 p_X1 = p_Xu

Initial belief p ( X0 )

p_X0 =

1 0 0 0 0

Belief p ( X1 | U0 =2)

p_X1 =

0 0.1000 0.8000 0.1000 0

Example 6.12

What is the belief of mobile system position if after the movement made in Example 6.11 the mobile system makes another move in the counter-clockwise direction, but this time only for a single cell (U1 = 1)?

Solution

The probability distribution (belief) after the movement can again be determined if the probability that the mobile system is in a particular cell is calculated for every cell (total probability). In the case of a movement for a single cell, the first cell can be reached from cell 1 (too short move), cell 4 (too long move), and cell 5 (correct move). The mobile system can arrive to the second cell from cells 1, 2, and 5, and so on. After the movement is made, the following probabilities can be calculated:

P(X2=x1|U0=2,U1=1)=[0.1,0,0,0.1,0.8][0,0.1,0.8,0.1,0]T=0.01P(X2=x2|U0=2,U1=1)=[0.8,0.1,0,0,0.1][0,0.1,0.8,0.1,0]T=0.01P(X2=x3|U0=2,U1=1)=[0.1,0.8,0.1,0,0][0,0.1,0.8,0.1,0]T=0.16P(X2=x4|U0=2,U1=1)=[0,0.1,0.8,0.1,0][0,0.1,0.8,0.1,0]T=0.66P(X2=x5|U0=2,U1=1)=[0,0,0.1,0.8,0.1][0,0.1,0.8,0.1,0]T=0.16

si140_e

The position belief after the second movement is

p(X2|U0=2,U1=1)=[0.01,0.01,0.16,0.66,0.16]T

si141_e

and it is also shown in the bottom of Fig. 6.13. Note that the mobile system is most probably in cell 4. However, the probability distribution does not have as significant peak as it was before the second movement action was made (compare the middle with the bottom probability distribution in Fig. 6.13; the peak dropped from 80% to 66%). This observation is in accordance with the statement that every movement (action) increases the level of uncertainty about the states of the environment.

f06-13-9780128042045
Fig. 6.13 Posterior state probability distributions in three time steps from Example 6.12.

The implementation of the solution in Matlab is shown in Listing 6.7.

Listing 6.7

Implementation of the solution of Example 6.12

1 disp( ’ Initial belief  p(X0 ) ’)

2 p_X0= [1  0 0 00]

3

4 P_xxu_null= 0 . 8 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u

5 P_xxu_less= 0 . 1 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u−1

6 P_xxu_more = 0 . 1 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u+1

7

8 disp( ’Belief p(X1 |U0 =2) ’) ;

9 p_xXu= [0  0 P_xxu_more P_xxu_null P_xxu_less ] ; %  for U =2

10 p_Xu=  zeros(1 ,5) ;

11 fori =1:5

12     p_Xu( i )=  p_xXu*p_X0 . ’ ;

13     p_xXu=  p_xXu ( [ end1:end−1]) ;

14 end

15 p_X1 = p_Xu

16

17 disp ( ’Belief p(X2 |U1 =1) ’) ;

18 p_xXu= [ P_xxu_less 0 0 P_xxu_more P_xxu_null ] ; %  for U =1

19 p_Xu=  zeros(1 ,5) ;

20 fori =1:5

21     p_Xu( i )=  p_xXu*p_X1 . ’ ;

22     p_xXu=  p_xXu ( [ end1:end−1]) ;

23 end

24 p_X2 = p_Xu

Initial belief p ( X0 )

p_X0 =

1 0 0 0 0

Belief p ( X1 | U0 =2)

p_X1 =

0 0.1000 0.8000 0.1000 0

Belief p ( X2 | U1 =1)

p_X2 =

0.0100 0.0100 0.1600 0.6600 0.1600

Example 6.13

Consider that the mobile system in Example 6.11 is initially in the first cell, p(X0) = [1, 0, 0, 0, 0]. In every time step the mobile system makes one step in counter-clockwise direction.

1. What is the belief into mobile system position after 10 time steps?

2. To which value does the belief converge after an infinite number of time steps?

Solution

1. The state belief after 10 time steps is

p(X10|U0:9)=[0.29,0.22,0.13,0.13,0.22]T

si142_e

2. After an infinite number of time steps a uniform distribution is obtained, since all the cells become equally possible to be occupied by the mobile system:

p(X∞|U0:∞)=[0.2,0.2,0.2,0.2,0.2]T

si143_e

These results were also validated in Matlab (Listing 6.8) and are shown graphically in Fig. 6.14.

f06-14-9780128042045
Fig. 6.14 Posterior state probability distributions in three time steps from Example 6.13.

Listing 6.8

Implementation of the solution of Example 6.13

1 disp( ’ Initial belief  p(X0 ) ’)

2 p_X0= [1  0 0 00]

3

4 P_xxu_null= 0 . 8 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u

5 P_xxu_less= 0 . 1 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u−1

6 P_xxu_more = 0 . 1 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u+1

7

8 p_X=  p_X0 ;

9 for k=1:1000

10     p_xXu= [ P_xxu_less 0 0 P_xxu_more P_xxu_null ] ; %  for U =1

11     p_Xu=  zeros(1 ,5) ;

12     fori =1:5

13       p_Xu( i )=  p_xXu*p_X . ’ ;

14       p_xXu=  p_xXu ( [ end1:end−1]) ;

15     end

16     p_X = p_Xu ;

17     i f  k==10

18       disp( ’Belief p(X10|U9 =1) ’) ;

19       p_X10=  p_X

20      else i f  k==1000

21       disp( ’Belief p(X1000|U999=1) ’) ;

22       p_X1000=  p_X

23     end

24 end

Initial belief p ( X0 )

p_X0 =

1 0 0 0 0

Belief p ( X10 | U9 =1)

p_X10 =

0.2949 0.2243 0.1283 0.1283 0.2243

Belief p ( X1000 | U999 =1)

p_X1000 =

0.2000 0.2000 0.2000 0.2000 0.2000

6.4.7 Localization in the Environment

The mobile system can estimate its location in the environment even if it does not know its initial location but has a map of the environment. The location of the mobile system can be determined precisely with probability distribution. The process of determining the location in the environment is known as localization. Localization combines the process of observation (measurement) and action (movement). As already mentioned, measurements made in the environment increase the knowledge about the location, but the movement of the mobile system through the environment decreases this information.

Localization is a process in which the mobile system repeatedly updates the probability distribution that represents the knowledge about the mobile system’s location in the environment. The peak in the probability distribution (if it exists) represents the most probable mobile system location.

The localization process is a realization of the Bayesian filter (Algorithm 3), which combines the processes of movement and perception.

Example 6.14

Consider the mobile system that moves around the environment as shown in Example 6.8. The mobile system first makes a move and then observes the environment. The initial pose of the mobile system is not known. This can be described with uniform probability distribution p(X0) = bel(X0) = [0.2, 0.2, 0.2, 0.2, 0.2].

The movement action for uk cells in the counter-clockwise direction is accurate in 80%; in 10% the movement is either a cell shorter or longer than required:

p(Xk=xi|Xk−1=xj,Uk−1=uk−1)=0.8;fori=j+uk−1p(Xk=xi|Xk−1=xj,Uk−1=uk−1)=0.1;fori=j+uk−1−1p(Xk=xi|Xk−1=xj,Uk−1=uk−1)=0.1;fori=j+uk−1+1

si144_e

The mobile system detects a dark cell correctly with probability 0.6, and the probability of detecting a bright cell correctly is 0.8. This can be written down in mathematical form as

P(Z=dark|X=dark)=0.6,P(Z=bright|X=dark)=0.4P(Z=bright|X=bright)=0.8,P(Z=dark|X=bright)=0.2

si145_e

In every time step, the mobile system receives a command of moving for a single cell in the counter-clockwise direction (uk−1 = 1). The sequence of the first three measurements is z1:3 = [bright, dark, dark].

1. What is the belief in the first time step k = 1?

2. What is the belief in the second time step k = 2?

3. What is the belief in the third time step k = 3?

4. In which cell is the mobile system most likely after the third step?

Solution

After every movement is made a prediction step of the Bayesian filter (Algorithm 3) is evaluated, and a correction step after the measurement is made.

1. The prediction step is evaluated based on the known movement action. The small symbol xi, i ∈{1, …, 5} denotes that the location (state) of the mobile system is in cell i, and big symbol Xk denotes the vector of all possible states in time step k.

belp(X1=x1)=∑xiP(X1=x1|X0=xi,u0)bel(X0=xi)=pT(X1=x1|X0,u0)bel(X0)=[0.1,0,0,0.1,0.8][0.2,0.2,0.2,0.2,0.2]T=0.2belp(X1=x2)=[0.8,0.1,0,0,0.1][0.2,0.2,0.2,0.2,0.2]T=0.2belp(X1=x3)=[0.1,0.8,0.1,0,0][0.2,0.2,0.2,0.2,0.2]T=0.2belp(X1=x4)=[0,0.1,0.8,0.1,0][0.2,0.2,0.2,0.2,0.2]T=0.2belp(X1=x5)=[0,0,0.1,0.8,0.1][0.2,0.2,0.2,0.2,0.2]T=0.2

si146_e

Therefore, the complete probability distribution (belief) of the prediction step is

belp(X1)=[0.2,0.2,0.2,0.2,0.2]T

si147_e

After the measurement is obtained the correction step of the Bayesian filter is evaluated:

bel(X1=x1)=ηp(Z1=bright|x1)belp(X1=x1)=η0.8⋅0.2=η0.16bel(X1=x2)=ηp(Z1=bright|x2)belp(X1=x2)=η0.8⋅0.2=η0.16bel(X1=x3)=ηp(Z1=bright|x3)belp(X1=x3)=η0.4⋅0.2=η0.08bel(X1=x4)=ηp(Z1=bright|x4)belp(X1=x4)=η0.4⋅0.2=η0.08bel(X1=x5)=ηp(Z1=bright|x5)belp(X1=x5)=η0.8⋅0.2=η0.16

si148_e

After considering the normalization factor,

η=10.16+0.16+0.08+0.08+0.16=1.56

si149_e

the updated probability distribution (belief) is obtained:

bel(X1)=[0.25,0.25,0.125,0.125,0.25]T

si150_e

The same result can be obtained from the following:

bel(X1)=pT(Z1=bright|X1)*belpT(X1)pT(Z1=bright|X1)belp(X1)=[0.8,0.8,0.4,0.4,0.8]*[0.2,0.2,0.2,0.2,0.2][0.8,0.8,0.4,0.4,0.8][0.2,0.2,0.2,0.2,0.2]T=[0.25,0.25,0.125,0.125,0.25]T

si151_e

2. The procedure from the first case can be repeated again on the last result to obtain state belief in time step k = 1. First, a prediction step is repeated:

belp(X2=x1)=[0.1,0,0,0.1,0.8][0.25,0.25,0.125,0.125,0.25]T=0.237belp(X2=x2)=[0.8,0.1,0,0,0.1][0.25,0.25,0.125,0.125,0.25]T=0.25belp(X2=x3)=[0.1,0.8,0.1,0,0][0.25,0.25,0.125,0.125,0.25]T=0.237belp(X2=x4)=[0,0.1,0.8,0.1,0][0.25,0.25,0.125,0.125,0.25]T=0.138belp(X2=x5)=[0,0,0.1,0.8,0.1][0.25,0.25,0.125,0.125,0.25]T=0.138

si152_e

The complete probability distribution of prediction is

belp(X2)=[0.237,0.25,0.237,0.138,0.138]T

si153_e

The correction step yields

bel(X2)=[0.2,0.2,0.6,0.6,0.2]T*[0.237,0.25,0.237,0.138,0.138]T[0.2,0.2,0.6,0.6,0.2][0.237,0.25,0.237,0.138,0.138]T=[0.136,0.143,0.407,0.236,0.079]T

si154_e

3. Similarly as in the previous two cases, the belief distribution can be obtained for time step k = 3:

belp(X3)=[0.1,0.131,0.167,0.363,0.237]Tbel(X3)=[0.048,0.063,0.245,0.528,0.115]T

si155_e

4. After the third time step, the mobile system is most likely in the fourth cell, with probability 52.8%. The second most likely cell is the third cell, with probability 24.5%.

The state beliefs for all three time steps are presented graphically in Fig. 6.15. The implementation of the solution in Matlab is shown in Listing 6.9.

f06-15-9780128042045
Fig. 6.15 Posterior state probability distributions in three time steps from Example 6.14.

Listing 6.9

Implementation of the solution of Example 6.14

1 disp( ’ Initial belief  p(X0 ) ’)

2 bel_X0=  ones (1 ,5) /5

3

4 P_xxu_null= 0 . 8 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u

5 P_xxu_less= 0 . 1 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u−1

6 P_xxu_more = 0 . 1 ; %  P (X = i|X ’=j ,U ’=u) , i= j+ u+1

7

8 p_ZdX= [ 0 . 2 0.2 0.6 0.6 0 . 2 ] ; %  p(Z = dark|X )

9 p_ZbX=  1− p_ZdX ;   %  p(Z = bright|X )

10

11 bel_X=  bel_X0 ;

12 for k=1:3

13    %  Prediction step

14    p_xXu= [ P_xxu_less 0 0 P_xxu_more P_xxu_null ] ; %  for U =1

15    belp_X=  zeros(1 ,5) ;

16    fori =1:5

17       belp_X ( i )=  p_xXu*bel_X . ’ ;

18       p_xXu=  p_xXu ( [ end1:end−1]) ;

19    end

20

21    %  Correction step

22    if  k ==1

23       bel_X=  p_ZbX.* belp_X ;

24    else

25       bel_X=  p_ZdX.* belp_X ;

26    end

27    bel_X=  bel_X/sum( bel_X ) ;

28

29    if  k ==1

30       disp( ’ belief s  belp_X1 and bel_X1 ’)

31       belp_X1=  belp_X

32       bel_X1=  bel_X

33    else i f  k ==2

34       disp( ’ belief s  belp_X2 and bel_X2 ’)

35       belp_X2=  belp_X

36       bel_X2=  bel_X

37    else i f  k ==3

38       disp( ’ belief s  belp_X3 and bel_X3 ’)

39       belp_X3=  belp_X

40       bel_X3=  bel_X

41       disp( ’Lessl i k e l y  to mostl i k e l y  position ’)

42       [m, mi ] =  sort( bel_X )

43    end

44 end

Initial belief p ( X0 )

bel_X0 =

0.2000 0.2000 0.2000 0.2000 0.2000

belief s belp_X1 and bel_X1

belp_X1 =

0.2000 0.2000 0.2000 0.2000 0.2000

bel_X1 =

0.2500 0.2500 0.1250 0.1250 0.2500

belief s belp_X2 and bel_X2

belp_X2 =

0.2375 0.2500 0.2375 0.1375 0.1375

bel_X2 =

0.1357 0.1429 0.4071 0.2357 0.0786

belief s belp_X3 and bel_X3

belp_X3 =

0.1000 0.1307 0.1686 0.3636 0.2371

bel_X3 =

0.0484 0.0633 0.2450 0.5284 0.1149

Less l i k e l y to most l i k e l y position

m =

0.0484 0.0633 0.1149 0.2450 0.5284

mi =

1 2 5 3 4

6.5 Kalman Filter

Kalman filter [6] is one of the most important state estimation and prediction algorithms, which has been applied to a diverse range of applications in various engineering fields, and autonomous mobile systems are no exception. The Kalman filter is designed for state estimation of linear systems where the system signals may be corrupted by noise. The algorithm has a typical two-step structure that consists of a prediction and a correction step that are evaluated in every time step. In the prediction step, the latest system state along with state uncertainties are predicted. Once a new measurement is available, the correction step is evaluated where the stochastic measurement is joined with the predicted state estimate as a weighted average, in a way that less uncertain values are given a greater weight. The algorithm is recursive and allows an online estimation of the current system state taking into account system and measurement uncertainties.

A classical Kalman filter assumes normally distributed noises, that is, the probability distribution of noise is a Gaussian function:

p(x)=12πσ2e−12(x−μ)2σ2

si26_e  (6.30)

where μ is the mean value (mathematical expectation) and σ2 is the variance. The Gaussian function is a unimodal (left and right from the single peak the function monotonically decreases toward zero)—more general distributions are normally multimodal (there are several local peaks). If the probability distributions of continuous variables are assumed to be unimodal, the Kalman filter can be used for optimum state estimation. In the case that the variables are not all unimodal, the state estimation is suboptimal; furthermore, the convergence of the estimate to the true value is questionable. The Bayesian filter does not have the aforementioned problems, but its applicability is limited to simple continuous problems and to discrete problems with a finite countable number of states.

In Fig. 6.16 an example of continuous probability distribution, which is not unimodal, is shown. The continuous distribution is approximated with a Gaussian function and with a histogram (domain is divided into discrete intervals). The approximation with a Gaussian function is used in the Kalman filter, and the histogram is used in the Bayesian filter.

f06-16-9780128042045
Fig. 6.16 An example of probability distribution of a continuous variable x (solid line), approximation with Gaussian function (dashed line), and approximation with a histogram (dotted line).

The essence of the correction step (see Bayesian filter (6.29)) is information fusion from two independent sources, that is, sensor measurements and state predictions based on the previous state estimations. Let us use Example 6.15 again to demonstrate how two independent estimates of the same variable x can be jointed optimally if the value and variance (belief) of each source is known.

Example 6.15

There are two independent estimates of the variable x. The value of the first estimate is x1 and has a variance σ12si157_e, and the value of the second estimate is x2 with a variance σ22si158_e. What is the optimal linear combination of these two estimates that represent the state estimate x^si159_e with minimal variance?

Solution

The estimation of optimal value of the variable x is assumed to be a linear combination of two measurements:

x^=ω1x1+ω2x2

si160_e

where the parameters ω1 and ω2 are the unknown weights that satisfy the following condition: ω1 + ω2 = 1. The optimum values of the weights should minimize the variance σ2 of the optimal estimate x^si159_e. Hence, the variance is as follows:

σ2=E(x^−Ex^)2=E(ω1x1+ω2x2−Eω1x1+ω2x2)2=E(ω1x1+ω2x2−ω1Ex1−ω2Ex2)2=Eω1x1−Ex1+ω2x2−Ex22=Eω12x1−Ex12+ω22x2−Ex22+2ω1ω2(x1−Ex1)(x2−Ex2)=ω12E(x1−Ex1)2+ω22E(x2−Ex2)2+2ω1ω2E(x1−Ex1)(x2−Ex2)=ω12σ12+ω22σ22+2ω1ω2E(x1−Ex1)(x2−Ex2)

si162_e

Since the variables x1 and x2 are independent, the differences x1−Ex1si163_e and x2−Ex2si164_e are also independent, and therefore E(x1−Ex1)(x2−Ex2)=0si165_e. Hence,

σ2=ω12σ12+ω22σ22

si166_e

or after introducing ω2 = ω and ω1 = 1 − ω,

σ2=(1−ω)2σ12+ω2σ22

si167_e

We are seeking the value of the weight ω that minimizes the variance that can be obtained from variance derivative:

∂∂ωσ2=−2(1−ω)σ12+2ωσ22=0

si168_e

which yields the solution

ω=σ12σ12+σ22

si169_e

The minimum-variance estimate is therefore

x^=σ22x1+σ12x2σ12+σ22

si170_e  (6.31)

and the minimum variance is

σ2=σ12σ22σ12+σ22=1σ12+1σ22−1

si171_e  (6.32)

The obtained results confirm that the source with lower variance (higher belief) contributes more to the final estimate, and vice-versa.

Example 6.16

In a particular moment in time, an initial state estimate is given x = 2 with variance σ2 = 4. Then a sensor is used to measure the value of the state, which is z = 4 with sensor variance σz2=1si172_e. The Gaussian probability distributions of the state and measurement are shown in Fig. 6.17.

f06-17-9780128042045
Fig. 6.17 Probability distribution of the state (dashed line) and the measurement (dash-dotted line).

What is the value of optimum state estimate that includes information from previous state estimate and current measurement? What is the probability distribution of the updated optimum state estimate?

Solution

Based on Fig. 6.17 we can foreknow that the mean value x′ of the updated state will be closer to the measurement mean value, since the measurement variance (uncertainty) is lower than previous estimate variance. Using Eq. (6.31) the updated state estimate is obtained:

x′=σz2x+σ2zσ2+σz2=3.6

si173_e

Variance of the updated estimate σ′2 is lower than both previous variances, since the integration of the previous estimate and measurement information lower the uncertainty of the updated estimate. The variance of the updated estimate is obtained from Eq. (6.32):

σ′2=1σ2+1σz2−1=0.8

si174_e

and the standard deviation is

σ′=σ′2=0.894

si175_e

The updated probability distribution p(x|z) of the state after the measurement-based correction is shown in Fig. 6.18.

f06-18-9780128042045
Fig. 6.18 Probability distribution of the initial state (dashed line), measurement (dash-dotted line), and updated state (solid line).

Let us use the findings from Example 6.15 in the derivation of the recursive state estimation algorithm. In every time step a new state measurement z(k) = x(k) + n(k) is obtained by the sensor, where n(k) is the measurement noise. The measurement variance σz2(k)si176_e is assumed to be known. The updated optimum state estimate is a combination of the previous estimate x^(k)si177_e and the current measurement z(k), as follows:

x^(k+1)=1−ωx^(k)+ω(k)z(k)=x^(k)+ω(z(k)−x^(k))

si178_e

The updated state variance is

σ2(k+1)=σ2(k)σz2(k)σ2(k)+σz2(k)=(1−ω)σ2(k)

si179_e

where

ω=σ2(k)σ2(k)+σz2(k)

si180_e

Therefore, given known initial state estimate x, x^(0)si181_e and the corresponding variance σ2(0) the measurements z(1), z(2), … can be integrated optimally, in a way that the current state and state variance are estimated. This is the basic idea behind the correction step of the Kalman filter.

The prediction step of the Kalman filter provides the given state prediction a known input action. The initial state estimate x^(k)si177_e has a probability distribution with variance σ2(k). In the same way, the action u(k), which is responsible for transition of the state x(k) to x(k + 1), has probability distribution (transition uncertainty) σu2(k)si183_e. Using Example 6.17 let us examine the value of the state and the variance after the action is executed (after state transition).

Example 6.17

The initial state estimate x^(k)si177_e with variance σ2(k) is known. Then an action u(k) is executed, which represents the direct transition of the state with uncertainty (variance) σu2(k)si183_e. What is the value of the state estimate and the state uncertainty after the transition?

Solution

The updated state estimate after the transition is

x^(k+1)=x^(k)+u(k)

si186_e

and the uncertainty of this estimation is

σ2(k+1)=Ex^(k+1)−Ex^(k+1)2=Ex^(k)+u(k)−Ex^(k)+u(k)2=E(x^(k)−Ex^(k))+(u(k)−Eu(k))2=E(x^(k)−Ex^(k))2+(u(k)−Eu(k))2+E2(x^(k)−Ex^(k))(u(k)−Eu(k))=σ2(k)+σu2(k)+E2x^(k)−Ex^(k)(u(k)−Eu(k))

si187_e  (6.33)

Since x^si159_e and u are independent, it holds E2x^(k)−Ex^(k)u(k)−si189_eEu(k)=0si190_e, and therefore Eq. (6.33) simplifies to

σ2(k+1)=σ2(k)+σu2(k)

si191_e

Simplified Implementation of the Kalman Filter Algorithm

The Kalman filter algorithm for a simple case with only a single state is given in Algorithm 4, where the variables with a subscript (⋅)k|k−1 represent the estimated values in the prediction step, and variables with subscript (⋅)k|k represent the values from the correction step. For improved readability the following notation is used: u(k − 1) = uk−1 and z(k) = zk.

Algorithm 4

Kalman Filter for a Single State

functionKalman_filter(x^k−1|k−1si192_e, uk−1, zk, σk−1|k−12si193_e, σuk−12si194_e, σzk2si195_e)

Prediction step:

x^k|k−1←x^k−1|k−1+uk−1si196_e

σk|k−12←σk−1|k−12+σuk−12si197_e

Correction step:

ωk←σk|k−12σk|k−12+σzk2si198_e

x^k|k←x^k|k−1+ωk(zk−x^k|k−1)si199_e

σk|k2←(1−ωk)σk|k−12si200_e

returnx^k|ksi201_e, σk|k2si202_e

end function

The Kalman filter has two steps (prediction and correction step) that are executed one after another in the loop. In the prediction step only the known action is used in a way that enables prediction of the state in the next time step. From the initial belief a new belief is evaluated, and the uncertainty of the new belief is higher that the initial uncertainty. In the correction step the measurement is used to improve the predicted belief in a way that the new (corrected) state estimate has lower uncertainty than the previous belief. In both steps only two inputs are required: in the prediction step the value of previous belief x^k−1|k−1si192_e and executed action uk−1 need to be known, and in the correction step the previous belief x^k|k−1si204_e and measurement zk are required. The state transition variance σk−1|k−12si193_e, input action variance σuk−12si194_e, and measurement variance σzk2si195_e also need to be given.

Example 6.18

There is a mobile robot that can move in only one dimension. The initial position of the robot is unknown (Fig. 6.19). Let us therefore assume that the initial position is x^0=3si208_e with large variance σ02=100si209_e (the true position x0 = 0 is not known).

f06-19-9780128042045
Fig. 6.19 Localization of a mobile robot in one-dimensional space with unknown initial position.

Then, in every time moment k − 1 = 0, …, 4 the mobile robot is moved for u0:4 = (2, 3, 2, 1, 1) units and the measurements of the robot positions in time moments k = 1, …, 5 are taken, z1:5 = (2, 5, 7, 8, 9). The movement action and measurement are disturbed with white normally distributed zero-mean noise that can be described with a constant uncertainty for movement σu2=2si210_e and measurement uncertainty σz2=4si211_e.

What is the estimated robot position and uncertainty of this estimate?

Solution

Let us apply Algorithm 4 to solve the given mobile robot localization problem. In the initial time step (k = 1) the predicted state and variance can be calculated first:

x^1|0=x^0|0+u0=3+2=5σ1|02=σ0|02+σu2=100+2=102

si212_e

and then the correction step of the Kalman filter in the first time step k = 1 can be evaluated:

ω1=σ1|02σ1|02+σz2=102102+4=0.962x^1|1=x^1|0+ω1(z1−x^1|0)=5+0.962(2−5)=2.113σ1|12=(1−ω1)σ1|02=(1−0.962)102=3.849

si213_e

These prediction and correction steps can be evaluated for all the other time steps. The prediction results are therefore

x^1:5|0:4=(5.00,5.11,7.05,8.02,9.01)σ1:5|0:42=(102,5.85,4.38,4.09,4.02)

si214_e

and the correction results are

x^1:5|1:5=(2.11,5.05,7.02,8.01,9.01)σ1:5|1:52=(3.85,2.38,2.09,2.02,2.01)

si215_e

The obtained results show that the position of the mobile robot can be determined in a few time steps with uncertainty 2, which agrees with the prediction uncertainty and measurement uncertainty 1σ5|42+1σz2−1=2.01si216_e. The uncertainty of the predicted position estimate converges toward 4, which is in accordance with the correction uncertainty from the previous time step and measurement σ4|42+σu2=4.02si217_e.

6.5.1 Kalman Filter in Matrix Form

Multiple input, multiple state, and multiple output systems can be represented in a matrix form for improved readability. A general linear system can be expressed in a state-space form as

x(k+1)=Ax(k)+Bu(k)+Fw(k)z(k)=Cx(k)+v(k)

si218_e  (6.34)

where x is the state vector, u is the input (action) vector, and z is the output (measurement); A is the state matrix, B is the input matrix, F is the input noise matrix, C is the output matrix; w(k) is the process noise and v is the output (measurement) noise. In the case the noise w is added to the system input u, the following relation holds: F = B. The process noise w(k) and measurement noise v(k) are assumed to be independent (uncorrelated) white noises with zero mean value and covariance matrices Qk=Ew(k)wT(k)si219_e and Rk=Ev(k)vT(k)si220_e.

The probability distribution of the states x that are disturbed by a white Gaussian noise can be written in a matrix form as follows:

p(x)=det2πP−12e−12(x−μ)TP−1(x−μ)

si221_e

where P is the state-error covariance matrix.

The Kalman filter is an approach for filtering and estimation of linear systems with continuous state space, which are disturbed with normal noise. The noise distribution is represented with a Gaussian function (Gaussian noise). The input and measurement noises influence the internal system states that we would like to estimate. In the case that the model of the system is linear, the Gaussian noise propagated through the model (e.g., from inputs to the states) is also a Gaussian noise. The system must therefore be linear, since this requirement ensures Gaussian distribution of the noise on the states, an assumption used in the derivation of the Kalman filter. The Kalman filter state estimate converges to the true value only in the case of linear systems that are disturbed with Gaussian noise.

The Kalman filter for a linear system (6.34) has a prediction step:

x^k|k−1=Ax^k−1|k−1+Buk−1Pk|k−1=APk−1|k−1AT+FQk−1FT

si222_e  (6.35)

and a correction step:

Kk=Pk|k−1CTCPk|k−1CT+Rk−1x^k|k=x^k|k−1+Kk(zk−Cx^k|k−1)Pk|k=Pk|k−1−KkCPk|k−1

si223_e  (6.36)

In the prediction part of the algorithm the a priori estimate x^k|k−1si224_e is determined, which is based on the previous estimate x^k−1|k−1si225_e (obtained from measurements up to the time moment k − 1) and input u(k − 1). In the correction part of the Kalman filter the posteriori estimate x^k|ksi226_e is calculated, which is based on the measurements up to time step k. The state correction is made in a way that the difference between the true and estimated measurement is calculated (zk−Cx^k|k−1si227_e); this difference is also known as innovation or measurement residual. The state correction is calculated as a product of Kalman gain Kk and innovation. The prediction part can be evaluated in advance, in the time while we are waiting for the new measurement in the time step k. Notice the similarity of the matrix notation in Eqs. (6.35), (6.36) with the notation used in Algorithm 4.

Let us derive the equation for the calculation of the state error covariance matrix in the prediction part of the Kalman filter:

Pk|k−1=E(xk−x^k|k−1)(xk−x^k|k−1)T=covxk−x^k|k−1=covAxk−1+Buk−1+Fwk−1−Ax^k−1|k−1−Buk−1=covAxk−1+Fwk−1−Ax^k−1|k−1=covA(xk−1−x^k−1|k−1)+Fwk−1=covA(xk−1−x^k−1|k−1)+covFwk−1=E(A(xk−1−x^k−1|k−1))(A(xk−1−x^k−1|k−1))T+E(Fwk−1)(Fwk−1)T=E(A(xk−1−x^k−1|k−1))(xk−1−x^k−1|k−1)TAT+EFwk−1wk−1TFT=APk−1|k−1AT+FQk−1FT

si228_e

where in line six of the derivation we took into account that the process noise wk in time step k is independent of the state estimate error in the previous time step (xk−1−x^k−1|k−1si229_e).

Let us also derive the equation for computation of the state error covariance matrix in the correction part of the Kalman filter:

Pk|k=E(xk−x^k|k)(xk−x^k|k)T=covxk−x^k|k=covxk−x^k|k−1−Kk(zk−Cx^k|k−1)=covxk−x^k|k−1−Kk(Cxk+vk−Cx^k|k−1)=cov(I−KkC)(xk−x^k|k−1)−Kkvk=cov(I−KkC)(xk−x^k|k−1)+covKkvk=(I−KkC)Pk|k−1(I−KkC)T+KkRkKkT,

si230_e

where in line six of the derivation we took into account that the measurement noise vk is uncorrelated with the other terms. The obtained relation for calculation of the covariance matrix Pk|k is general and can be used for arbitrary gain Kk. However, the expression for Pk|k in Eq. (6.36) is valid only for optimum gain (Kalman gain) that minimizes the mean squared correction error Exk−x^k|k2si231_e; this is equivalent to the minimization of the sum of all the diagonal elements in the correction covariance matrix Pk|k.

The general equation for Pk|k can be extended and the terms rearranged:

Pk|k=(I−KkC)Pk|k−1(I−KkC)T+KkRkKkT=Pk|k−1−KkCPk|k−1−Pk|k−1CTKkT+KkCPk|k−1CTKkT+KkRkKkT=Pk|k−1−KkCPk|k−1−Pk|k−1CTKkT+Kk(CPk|k−1CT+Rk)KkT=Pk|k−1−KkCPk|k−1−Pk|k−1CTKkT+KkSkKkT

si232_e

where Sk = CPk|k−1CT +Rk represents the innovation covariance matrix (Sk=covzk−Cx^k|k−1si233_e). The sum of the diagonal terms of Pk|k is minimal when the derivative of Pk|k with respect to Kk is zero:

∂Pk|k∂Kk=−2(CPk|k−1)T+2KkSk=0

si234_e

which leads to the optimum gain in Eq. (6.36):

Kk=Pk|k−1CTSk−1=Pk|k−1CT(CPk|k−1CT+Rk)−1

si235_e

The correction covariance matrix at optimum gain can be derived if the optimum gain is postmultiplied with SkKkTsi236_e and inserted into the equation for Pk|k:

..................Content has been hidden....................

You can't read the all page of ebook, please click here login for view all page.
Reset
18.117.196.184