6.6 Particle Filter

Search in book...
Toggle Font Controls
Create new playlist

Name your new playlist

Playlist description (optional)
Sign In

Email address

Password

Forgot Password?

or

Continue with Facebook

Continue with Google
Sign Up

Full Name

Email address

Confirm Email Address

Password

or

Continue with Facebook

Continue with Google

$\begin{array}{l} \begin{matrix} P_{k | k} & = P_{k | k - 1} - K_{k} C P_{k | k - 1} - P_{k | k - 1} C^{T} K_{k}^{T} + K_{k} S_{k} K_{k}^{T} \\ = P_{k | k - 1} - K_{k} C P_{k | k - 1} - P_{k | k - 1} C^{T} K_{k}^{T} + P_{k | k - 1} C^{T} K_{k}^{T} \\ = P_{k | k - 1} - K_{k} C P_{k | k - 1} \end{matrix} \end{array}$ $\begin{array}{l} \begin{matrix} P_{k | k} & = P_{k | k - 1} - K_{k} C P_{k | k - 1} - P_{k | k - 1} C^{T} K_{k}^{T} + K_{k} S_{k} K_{k}^{T} \\ = P_{k | k - 1} - K_{k} C P_{k | k - 1} - P_{k | k - 1} C^{T} K_{k}^{T} + P_{k | k - 1} C^{T} K_{k}^{T} \\ = P_{k | k - 1} - K_{k} C P_{k | k - 1} \end{matrix} \end{array}$

si237_e

Example 6.19

There is a mobile robot that drives around on a plane and measures its position with a GPS sensor 10 times in a second (sampling time T_s = 0.1 s). The position measurement is disturbed with zero-mean Gaussian noise with variance 10 m². The mobile robot drives with the speed of 1 m/s in the x direction and with the speed of 0 m/s in the y direction. The variance of Gaussian noise of the velocity is 0.1 m²/s². At the beginning of observation, the mobile robot is situated in the origin x = [0, 0]^T, but our initial estimate about the mobile robot’s position is $\hat{x} = {[3, 3]}^{T}$ $\hat{x} = {[3, 3]}^{T}$ with initial variance

$\begin{array}{l} P_{0} = [\begin{matrix} 10 & 0 \\ 0 & 10 \end{matrix}] \end{array}$ $\begin{array}{l} P_{0} = [\begin{matrix} 10 & 0 \\ 0 & 10 \end{matrix}] \end{array}$

si239_e

What is the time evolution of the position estimate and the variance of the estimate?

Solution

The solution of the problem can be obtained with the aid of simulation tools provided in Matlab. Let us determine the robot’s movement model where the state estimate represents the robot’s position in a plane ${\hat{x}}_{k | k - 1} = {[x_{k}, y_{k}]}^{T}$ ${\hat{x}}_{k | k - 1} = {[x_{k}, y_{k}]}^{T}$ and input u = [v_x, v_y]^T represents the robot’s speed in x and y directions, respectively. The model for system state prediction is therefore

$\begin{array}{l} {\hat{x}}_{k | k - 1} = [\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}] {\hat{x}}_{k - 1 | k - 1} + [\begin{matrix} T_{s} & 0 \\ 0 & T_{s} \end{matrix}] u \end{array}$ $\begin{array}{l} {\hat{x}}_{k | k - 1} = [\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}] {\hat{x}}_{k - 1 | k - 1} + [\begin{matrix} T_{s} & 0 \\ 0 & T_{s} \end{matrix}] u \end{array}$

si241_e

The position measurement model of a GPS sensor is

$\begin{array}{l} {\hat{z}}_{k} = [\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}] {\hat{x}}_{k | k - 1} \end{array}$ $\begin{array}{l} {\hat{z}}_{k} = [\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}] {\hat{x}}_{k | k - 1} \end{array}$

si242_e

Listing 6.10 provides an implementation of the solution in Matlab. The simulation results are shown graphically in Figs. 6.20–6.22. The simulation results confirm that the algorithm provides a position estimate that is converging to the true robot position from the false initial estimate. This is expected since the mean of the measurement noise is zero. However, the estimate variance is decreasing with time and falls much below the measurement variance (the internal kinematic model is the trusted mode). The Kalman filter enables optimum fusion of data from different sources (internal kinematic model and external measurement model) if variances of these sources are known. The reader should feel free to experiment with system parameters and observe the estimate variance evolution and convergence rate.

Listing 6.10

Implementation of the solution of Example 6.19

1 % Linear model of the system in state − space

2 Ts = 0 . 1 ; % Sampling time

3 A = [1 0; 01 ] ;

4 B = [ Ts0; 0 Ts ] ;

5 C = [1 0; 01 ] ;

6 F= B; % Noisei s added directly to the input.

8 xTrue= [ 0 ; 0 ] ; % True Initial state

9 x= [ 3 ; 3 ] ; % Initial state estimate

10 P = diag([10 10]) ; % Variance of theInitial state estimate

11 Q = diag([1 1]/10) ; % Noise variance of the movement actuator

12 R = diag([10 10]) ; % Noise vairace of the GPS measurements

14 figures _ k f 1 _ i n i t ;

16 % Loop

17 N = 150;

18 for k= 1:N

19 u= [ 1 ; 0 ] ; % Movement command

21 % Simulation of the true mobile robot position and measurement

22 xTrue= A *xTrue+ B *u+ F*sqrt(Q)*randn(2 , 1) ;

23 zTrue= C *xTrue+ sqrt(R)*randn(2 , 1) ;

25 % Position estimate based on known inputs and measurements

26 % %% Prediction

27 xPred= A *x+ B *u ;

28 PPred= A *P* A. ’ + F* Q *F . ’ ;

30 % %% Correction

31 K = PPred* C. ’/(C *PPred* C. ’ + R) ;

32 x= xPred+ K*( zTrue − C * xPred ) ;

33 P = PPred − K * C * PPred ;

35 figures_kf1_update ;

36 end

f06-20-9780128042045 — Fig. 6.20 True (dashed line) and estimated (solid line) trajectory with measurements (dotted line) from Example 6.19. The final position of the robot is marked with a circle.

f06-21-9780128042045 — Fig. 6.21 True position (dashed line) and position estimate (solid line) of the mobile robot based on measurements (dotted line) from Example 6.19.

f06-22-9780128042045 — Fig. 6.22 Time evolution of mobile robot position variance from Example 6.19.

6.5.2 Extended Kalman Filter

The Kalman filter was developed for linear systems, and it is normally assumed that all disturbances and noises can be described with a zero-mean Gaussian distribution. Noise with a Gaussian distribution is invariant to linear transformations, or in other words, if Gaussian noise is transformed by a linear function, it remains Gaussian noise; it only has different noise parameters that can be calculated explicitly given the known linear function. This is the reason behind the computational efficiency of the Kalman filter. In the case the input Gaussian noise is transformed by a nonlinear function, the output noise is no longer Gaussian, but it can normally still be approximated with a Gaussian noise.

If any of the state transitions or output equations of the system are a nonlinear function, the conventional Kalman filter may no longer produce an optimum state estimate. To overcome the problem of nonlinearities, an extended Kalman filter (EKF) was developed where the system nonlinearities are approximated with local linear models. The local linear model can be obtained from a nonlinear system with the procedure of function linearization (first-order Taylor series expansion) around the current state estimate. In the process of linearization the sensitivity matrices (Jacobian matrices) for current values of estimated states and measurements are obtained. The obtained linear model enables approximation of the noise that is not necessarily Gaussian with a Gaussian noise distribution.

The use of noise linearization in the process of noise modeling enables a computationally efficient implementation of the EKF algorithm. Hence, the EKF algorithm is commonly used in practice. The accuracy of linear approximation depends on the noise variance (big uncertainties or noise amplitudes may lead to poor linear approximation, since the signal may be outside the linear region) and the level of nonlinearity. Due to the error that is caused by linearization the convergence of the filter can worsen or the estimation may not even converge to the true solution.

A nonlinear system may be written in a general form:

$\begin{array}{l} \begin{matrix} x_{k} & = f (x_{k - 1}, u_{k - 1}, w_{k - 1}) \\ z_{k} & = h (x_{k}) + v_{k} \end{matrix} \end{array}$ $\begin{array}{l} \begin{matrix} x_{k} & = f (x_{k - 1}, u_{k - 1}, w_{k - 1}) \\ z_{k} & = h (x_{k}) + v_{k} \end{matrix} \end{array}$

si243_e (6.37)

where the noise w_k can occur at the system input or influence the states directly.

The EKF for a nonlinear system (6.37) has a prediction part:

$\begin{array}{l} \begin{matrix} {\hat{x}}_{k | k - 1} & = f ({\hat{x}}_{k - 1 | k - 1}, u_{k - 1}) \\ P_{k | k - 1} & = A P_{k - 1 | k - 1} A^{T} + F Q_{k - 1} F^{T} \end{matrix} \end{array}$ $\begin{array}{l} \begin{matrix} {\hat{x}}_{k | k - 1} & = f ({\hat{x}}_{k - 1 | k - 1}, u_{k - 1}) \\ P_{k | k - 1} & = A P_{k - 1 | k - 1} A^{T} + F Q_{k - 1} F^{T} \end{matrix} \end{array}$

si244_e (6.38)

and a correction part:

$\begin{array}{l} \begin{matrix} K_{k} & = P_{k | k - 1} C^{T} {(C P_{k | k - 1} C^{T} + R_{k})}^{- 1} \\ {\hat{x}}_{k | k} & = {\hat{x}}_{k | k - 1} + K_{k} (z_{k} - {\hat{z}}_{k}) \\ P_{k | k} & = P_{k | k - 1} - K_{k} C P_{k | k - 1} \end{matrix} \end{array}$ $\begin{array}{l} \begin{matrix} K_{k} & = P_{k | k - 1} C^{T} {(C P_{k | k - 1} C^{T} + R_{k})}^{- 1} \\ {\hat{x}}_{k | k} & = {\hat{x}}_{k | k - 1} + K_{k} (z_{k} - {\hat{z}}_{k}) \\ P_{k | k} & = P_{k | k - 1} - K_{k} C P_{k | k - 1} \end{matrix} \end{array}$

si245_e (6.39)

In the prediction part (6.38) the nonlinear state transition system is used for calculation of state prediction estimate. To calculate the noise covariance matrix, the Jacobian matrix A that describes the noise propagation from previous to current states and the Jacobian matrix F that describes the noise propagation from inputs to the states need to be determined from the state model (6.37):

$\begin{array}{l} A & = \frac{\partial f}{\partial \hat{x}}|_{({\hat{x}}_{k - 1 | k - 1}, u_{k - 1})} \end{array}$ $\begin{array}{l} A & = \frac{\partial f}{\partial \hat{x}}|_{({\hat{x}}_{k - 1 | k - 1}, u_{k - 1})} \end{array}$

si246_e (6.40)

$\begin{array}{l} F & = \frac{\partial f}{\partial w}|_{({\hat{x}}_{k - 1 | k - 1}, u_{k - 1})} \end{array}$ $\begin{array}{l} F & = \frac{\partial f}{\partial w}|_{({\hat{x}}_{k - 1 | k - 1}, u_{k - 1})} \end{array}$

si247_e (6.41)

In the correction part (6.39) the measurement estimate is made based on predicted state estimate ${\hat{z}}_{k} = h ({\hat{x}}_{k | k - 1})$ ${\hat{z}}_{k} = h ({\hat{x}}_{k | k - 1})$ . The Jacobian matrix C that describes the noise propagation from states to outputs also needs to be determined:

$\begin{array}{l} C = \frac{\partial h}{\partial x}|_{({\hat{x}}_{k | k - 1})} \end{array}$ $\begin{array}{l} C = \frac{\partial h}{\partial x}|_{({\hat{x}}_{k | k - 1})} \end{array}$

si249_e (6.42)

The noise covariance matrices are $Q_{k} = E \{w (k) w^{T} (k)\}$ $Q_{k} = E \{w (k) w^{T} (k)\}$ and $R_{k} = E \{v (k) v^{T} (k)\}$ $R_{k} = E \{v (k) v^{T} (k)\}$ . Among many applications, the EKF has been successfully applied for solving the wheeled mobile robot localization problem [7, 8] and map building [9].

Example 6.20

Awheeled mobile robot with a differential drive moves on a plane. The robot input commands are the translational velocity v_k and angular velocity ω_k that are both disturbed independently by Gaussian noise with zero mean and variances $var \{v_{k}\} = 0.1 m^{2} / s^{2}$ $var \{v_{k}\} = 0.1 m^{2} / s^{2}$ and $var \{ω_{k}\} = 0.1 {rad}^{2} / s^{2}$ $var \{ω_{k}\} = 0.1 {rad}^{2} / s^{2}$ , respectively.

The mobile robot has a sensor that enables measurement of the distance to the marker that is positioned in the origin of the global coordinate frame. The robot is also equipped with a compass that enables measurement of the mobile robot’s orientation (heading). The considered setup is presented in Fig. 6.23. The distance measurement is disturbed by a Gaussian noise with zero mean and variance 0.5 m², and the angle measurement is also disturbed by Gaussian noise with zero mean and variance 0.3 rad².

f06-23-9780128042045 — Fig. 6.23 Setup considered in Example 6.20. The mobile robot has a sensor for measuring the distance to marker M (positioned in the origin of the global frame) and a compass to determine the robot orientation in the global frame (heading).

At the beginning of observation the true initial pose of the robot is x₀ = [1, 2, π/6]^T; however, the initial estimated pose is ${\hat{x}}_{0} = {[3, 0, 0]}^{T}$ ${\hat{x}}_{0} = {[3, 0, 0]}^{T}$ with initial variance:

$\begin{array}{l} P_{0} = [\begin{matrix} 9 & 0 & 0 \\ 0 & 9 & 0 \\ 0 & 0 & 0.6 \end{matrix}] \end{array}$ $\begin{array}{l} P_{0} = [\begin{matrix} 9 & 0 & 0 \\ 0 & 9 & 0 \\ 0 & 0 & 0.6 \end{matrix}] \end{array}$

si255_e

What is the time evolution of the mobile robot pose estimate ( ${\hat{x}}_{k | k - 1} = {[x_{k}, y_{k}, φ_{k}]}^{T}$ ${\hat{x}}_{k | k - 1} = {[x_{k}, y_{k}, φ_{k}]}^{T}$ ) and the variance of the estimation if the command $u = {[v_{k}, ω_{k}]}^{T} = {[0.5, 0.5]}^{T}$ $u = {[v_{k}, ω_{k}]}^{T} = {[0.5, 0.5]}^{T}$ is sent to the robot in every sampling time step T_s = 0.1 s?

Solution

The solution can be obtained with the aid of simulation tools provided in Matlab. Let us determine the kinematic motion model of the mobile robot. In this case the kinematic model is nonlinear:

$\begin{array}{l} \begin{matrix} {\hat{x}}_{k | k - 1} & = {\hat{x}}_{k - 1 | k - 1} + [\begin{matrix} T_{s} v_{k - 1} cos (φ_{k - 1}) \\ T_{s} v_{k - 1} sin (φ_{k - 1}) \\ T_{s} ω_{k - 1} \end{matrix}] \end{matrix} \end{array}$ $\begin{array}{l} \begin{matrix} {\hat{x}}_{k | k - 1} & = {\hat{x}}_{k - 1 | k - 1} + [\begin{matrix} T_{s} v_{k - 1} cos (φ_{k - 1}) \\ T_{s} v_{k - 1} sin (φ_{k - 1}) \\ T_{s} ω_{k - 1} \end{matrix}] \end{matrix} \end{array}$

si258_e

The distance and angle measurement model is

$\begin{array}{l} {\hat{z}}_{k} = [\begin{matrix} \sqrt{x_{k}^{2} + y_{k}^{2}} \\ φ_{k} \end{matrix}] \end{array}$ $\begin{array}{l} {\hat{z}}_{k} = [\begin{matrix} \sqrt{x_{k}^{2} + y_{k}^{2}} \\ φ_{k} \end{matrix}] \end{array}$

si259_e

Listing 6.11 provides an implementation of the solution in Matlab. The simulation results are shown in Figs. 6.24–6.28. Simulation results confirm that the algorithm can provide an estimate of the pose that converges to the true pose of the mobile robot although the initial estimate was biased. The innovation terms settle around zero.

Listing 6.11

Implementation of the solution of Example 6.20

1 Ts =0 . 1 ; % Sampling time

2 xTrue= [ 1 ; 2; pi / 6 ] ; % True Initial pose

3 x= [ 3 ; 0; 0 ] ; % Initial pose estimate

4 P = diag([9 90 . 6 ] ) ; % Initial covariance matrix of the pose estimate

5 Q = diag ( [ 0 . 1 0 . 1 ] ) ; % Noise covariance matrix of movement actuator

6 R = diag ( [ 0 . 5 0 . 3 ] ) ; % Noise covariance matrix of distance and

7 % angle measurement

8 enableNoise= 1; % Enable noise: 0 or 1

9 N = 300;% Number of simulation sample times

10 figures_ekf1_init ;

12 % Loop

13 for k= 1:N

14 u= [ 0 . 5 ; 0 . 5 ] ; % Movement command (translational and angular velocity)

15 uNoisy= u+ sqrt(Q)*randn(2 , 1)* enableNoise ;

17 % Simulation of the true mobile robot state (pose)

18 xTrue= xTrue+ Ts *[ uNoisy (1) *cos(xTrue (3) ) ; …

19 uNoisy (1) *sin(xTrue (3) ) ; …

20 uNoisy (2) ] ;

21 xTrue (3) = wrapToPi(xTrue (3) ) ;

23 % Simulation of the true noisy measurements

24 zTrue= [sqrt(xTrue (1) ˆ2+ xTrue (2) ˆ2 ) ; …

25 xTrue (3) ] + sqrt(R)*randn(2 , 1)* enableNoise ;

26 zTrue (1) = abs( zTrue (1) ) ;

27 zTrue (2) = wrapToPi( zTrue (2) ) ;

29 % %% Prediction (pose and speed estimation based on known inputs)

30 xPred= x+ Ts *[u(1) *cos(x (3) ) ; …

31 u(1) *sin(x (3) ) ; …

32 u(2) ] ;

33 xPred (3) = wrapToPi( xPred (3) ) ;

35 % Jacobian matrices

36 A = [1 0 − Ts * u (1) * sin(x (3) ) ; …

37 0 1 Ts*u(1) *cos(x (3) ) ; …

38 0 0 1 ] ;

39 F= [ Ts*cos(x (3) )0; …

40 Ts*sin(x (3) )0; …

41 0 Ts ] ;

42 PPred= A *P* A. ’ + F* Q *F . ’ ;

44 % Estimated measurements

45 z= [sqrt( xPred (1) ˆ2+ xPred (2) ˆ2) ; …

46 xPred (3) ] ;

48 % %% Correction

49 d= sqrt( xPred (1) ˆ2+ xPred (2) ˆ2) ;

50 C = [ xPred (1) /d xPred (2) /d0 ; …

51 0 0 1 ] ;

52 K = PPred* C. ’/(C *PPred* C. ’ + R) ;

53 inov= zTrue − z ;

54 inov (2) = wrapToPi( inov (2) ) ;

55 x= xPred+ K *inov ;

56 P = PPred − K * C * PPred ;

58 figures_ekf1_update ;

59 end

f06-24-9780128042045 — Fig. 6.24 True (dashed line) and estimated (solid line) trajectory from Example 6.20.

f06-25-9780128042045 — Fig. 6.25 Pose estimate (solid line) and true state (dashed line) of the mobile robot with a nonzero initial estimation error from Example 6.20.

f06-26-9780128042045 — Fig. 6.26 Distance and angle measurements from Example 6.20.

f06-27-9780128042045 — Fig. 6.27 Robot pose estimate variances from Example 6.20.

f06-28-9780128042045 — Fig. 6.28 Time evolution of innovation from Example 6.20.

When the states of the system are not measurable directly (as in this example), the question of system state observability arises. Observability analysis approaches can normally provide only sufficient conditions for system observability. However, observability analysis should be performed before designing a state estimation, since it can aid the designer choosing the appropriate set of measurement signals that make the estimation feasible. Although advanced mathematical tools could be used to check system observability, the observability analysis can also be performed with a simple graphical check that is based on the definition of indistinguishable states (see Section 6.3.3). The observability analysis of the system from this example is made in Fig. 6.29. The results in Fig. 6.29 demonstrate that the states of the system are normally distinguishable, except for some special cases (Fig. 6.29D). However, if the system states are observed long enough and mobile robot control inputs are adequately excited, the system is observable.

f06-29-9780128042045 — Fig. 6.29 Observability analysis of the system from Example 6.20. (A) Three particular initial robot poses that all have the same heading and distance from the marker. (B) Updated situation from Fig. (A) after the robots have traveled the same distance in a forward direction. From the measurement point of view, the robot poses are distinguishable. (C) Three particular initial robot poses that all have the same heading and distance from the marker: a special case. (D) Updated situation from Fig. (C) after the robots have traveled the same distance in a forward direction. From the measurement point of view, the robot in pose 1 is indistinguishable from the robot in pose 2, but the robot in pose 3 is distinguishable from the other two poses. (E) Updated situation from Fig. (C) after the robots have traveled the same not-straight path. From the measurement point of view, the robot poses are all distinguishable.

Example 6.21

Let us use the same mobile robot as in Example 6.20, but let us use a slightly different sensor. Assume that the mobile robot has s sensor for measuring the distance and angle to the marker that is in the origin of the global coordinate frame (Fig. 6.30). The angle measurements are in the range α ∈ [−π, π]. The distance measurement is disturbed by a Gaussian noise with zero mean and variance 0.5 m², and the angle measurement is disturbed by the Gaussian noise with zero mean and variance 0.3 rad².

f06-30-9780128042045 — Fig. 6.30 Setup considered in Example 6.21. The mobile robot has a sensor for measuring the distance and angle to marker M (positioned in the origin of the global frame).

What is the time evolution of the robot pose estimate ( ${\hat{x}}_{k | k - 1} = {[x_{k}, y_{k}, φ_{k}]}^{T}$ ${\hat{x}}_{k | k - 1} = {[x_{k}, y_{k}, φ_{k}]}^{T}$ ) and estimate variance if the input commands $u = {[v_{k}, ω_{k}]}^{T} = {[0.5, 0.5]}^{T}$ $u = {[v_{k}, ω_{k}]}^{T} = {[0.5, 0.5]}^{T}$ are sent to the robot in every sampling time step T_s = 0.1 s?

Solution

The solution can be obtained with the aid of simulation in Matlab. Let us determine the mobile robot motion model. The kinematic model of the wheeled mobile robot is the same as in Example 6.20:

$\begin{array}{l} {\hat{x}}_{k | k - 1} = {\hat{x}}_{k - 1 | k - 1} + [\begin{matrix} T_{s} v_{k - 1} cos (φ_{k - 1}) \\ T_{s} v_{k - 1} sin (φ_{k - 1}) \\ T_{s} ω_{k - 1} \end{matrix}] \end{array}$ $\begin{array}{l} {\hat{x}}_{k | k - 1} = {\hat{x}}_{k - 1 | k - 1} + [\begin{matrix} T_{s} v_{k - 1} cos (φ_{k - 1}) \\ T_{s} v_{k - 1} sin (φ_{k - 1}) \\ T_{s} ω_{k - 1} \end{matrix}] \end{array}$

si262_e

In this case, the distance and angle measurement model is

$\begin{array}{l} {\hat{z}}_{k} = [\begin{matrix} \sqrt{x_{k}^{2} + y_{k}^{2}} \\ atan2 (0 - y_{k}, 0 - x_{k}) - φ_{k} \end{matrix}] \end{array}$ $\begin{array}{l} {\hat{z}}_{k} = [\begin{matrix} \sqrt{x_{k}^{2} + y_{k}^{2}} \\ atan2 (0 - y_{k}, 0 - x_{k}) - φ_{k} \end{matrix}] \end{array}$

si263_e

where $atan2 (y, x)$ $atan2 (y, x)$ is a four-quadrant extension of $arctan \frac{y}{x}$ $arctan \frac{y}{x}$ :

$\begin{array}{l} atan2 (y, x) = \{\begin{matrix} arctan \frac{y}{x} & if x > 0 \\ arctan \frac{y}{x} + π & if x < 0 and y \geq 0 \\ arctan \frac{y}{x} - π & if x < 0 and y < 0 \\ \frac{π}{2} & if x = 0 and y > 0 \\ - \frac{π}{2} & if x = 0 and y < 0 \\ undefined & if x = 0 and y = 0 \end{matrix} \end{array}$ $\begin{array}{l} atan2 (y, x) = \{\begin{matrix} arctan \frac{y}{x} & if x > 0 \\ arctan \frac{y}{x} + π & if x < 0 and y \geq 0 \\ arctan \frac{y}{x} - π & if x < 0 and y < 0 \\ \frac{π}{2} & if x = 0 and y > 0 \\ - \frac{π}{2} & if x = 0 and y < 0 \\ undefined & if x = 0 and y = 0 \end{matrix} \end{array}$

si266_e (6.43)

Listing 6.12 provides an implementation of the solution in Matlab. The simulation results in Figs. 6.31–6.35 show that the estimated states converge to the true robot pose. Although this example is similar to Example 6.20, the estimate may not converge to the true robot pose in the presence of only slightly different environmental conditions. The case of convergence to the wrong solution is shown in Figs. 6.36–6.40. Since both sensor outputs (distance and angle) are relative measurements, the estimated states may not converge to the true solution, even though the innovation (difference between measurement and measurement prediction) is in the vicinity of a zero value (Fig. 6.40).

f06-31-9780128042045 — Fig. 6.31 True (dashed line) and estimated (solid line) trajectory from Example 6.21.

f06-32-9780128042045 — Fig. 6.32 Pose estimate (solid line) and true state (dashed line) of the mobile robot with a nonzero initial estimation error from Example 6.21.

f06-33-9780128042045 — Fig. 6.33 Distance and angle measurements from Example 6.21.

f06-34-9780128042045 — Fig. 6.34 Robot pose estimate variance from Example 6.21.

f06-35-9780128042045 — Fig. 6.35 Time evolution of innovation from Example 6.21.

f06-36-9780128042045 — Fig. 6.36 True (dashed line) and estimated (solid line) trajectory from Example 6.21 (representative case).

f06-37-9780128042045 — Fig. 6.37 Pose estimate (solid line) and true state (dashed line) of the mobile robot with a nonzero initial estimation error from Example 6.21 (representative case).

f06-38-9780128042045 — Fig. 6.38 Distance and angle measurements from Example 6.21 (representative case).

f06-39-9780128042045 — Fig. 6.39 Robot pose estimate variance from Example 6.21 (representative case).

f06-40-9780128042045 — Fig. 6.40 Time evolution of innovation from Example 6.21 (representative case).

Listing 6.12

Implementation of the solution of Example 6.21

1 Ts =0 . 1 ; % Sampling time

2 xTrue= [ 1 ; 2; pi / 6 ] ; % True Initial pose

3 x= [ 3 ; 0; 0 ] ; % Initial pose estimate

4 P = diag([9 90 . 6 ] ) ; % Initial covariance matrix of the pose estimate

5 Q = diag ( [ 0 . 1 0 . 1 ] ) ; % Noise covariance matrix of movement actuator

6 R = diag ( [ 0 . 5 0 . 3 ] ) ; % Noise covariance matrix of distance and

7 % angle measurement

8 enableNoise= 1; % Enable noise: 0 or 1

9 N = 300;% Number of simulation sample times

10 figures_ekf2_init ;

12 % Loop

13 for k= 1:N

14 u= [ 0 . 5 ; 0 . 5 ] ; % Movement command (translational and angular velocity)

15 uNoisy= u+ sqrt(Q)*randn(2 , 1)* enableNoise ;

17 % Simulation of the true mobile robot state (pose)

18 xTrue= xTrue+ Ts *[ uNoisy (1) *cos(xTrue (3) ) ; …

19 uNoisy (1) *sin(xTrue (3) ) ; …

20 uNoisy (2) ] ;

21 xTrue (3) = wrapToPi(xTrue (3) ) ;

23 % Simulation of the true noisy measurements (distance and angle)

24 zTrue= [sqrt(xTrue (1) ˆ2+ xTrue (2) ˆ2) ; …

25 atan2(0− xTrue (2) , 0− xTrue (1) )− xTrue (3) ] + …

26 sqrt(R)*randn(2 , 1)* enableNoise ;

27 zTrue (1) = abs( zTrue (1) ) ;

28 zTrue (2) = wrapToPi( zTrue (2) ) ;

30 %%% Prediction (pose and speed estimation based on known inputs)

31 xPred= x+ Ts *[u(1) *cos(x (3) ) ; …

32 u(1) *sin(x (3) ) ; …

33 u(2) ] ;

34 xPred (3) = wrapToPi( xPred (3) ) ;

36 % Jacobian matrices

37 A = [1 0 − Ts * u (1) * sin(x (3) ) ; …

38 0 1 Ts*u(1) *cos(x (3) ) ; …

39 0 0 1 ] ;

40 F= [ Ts*cos(x (3) )0; …

41 Ts*sin(x (3) )0; …

42 0 Ts ] ;

43 PPred= A *P* A. ’ + F* Q *F . ’ ;

46 % Estimated measurements

47 z = [sqrt( xPred (1) ˆ2+ xPred (2) ˆ2) ; …

48 atan2(0− xPred (2) , 0− xPred (1) ) − xPred (3) ] ;

49 z (2) = wrapToPi( z (2) ) ;

1 %%% Correction

2 d= sqrt( xPred (1) ˆ2+ xPred (2) ˆ2) ;

3 C = [ xPred (1) /d xPred (2) /d 0; …

4 − xPred (2) / d ˆ2 xPred (1) /dˆ2−1];

5 K = PPred * C . ’ / ( C * PPred * C . ’ + R) ;

6 inov= zTrue − z ;

8 % Selection of appropriate innovation due to noise and angle wrapping

9 inov (2) = wrapToPi( inov (2) ) ;

11 x= xPred+ K *inov ;

12 P = PPred − K * C * PPred ;

14 figures_ekf2_update ;

15 end

A graphical check of observability made in Fig. 6.41 can provide additional insight into the reasons for estimation bias. The analyses show that there are states that are clearly indistinguishable from the measurement point of view, regardless of control inputs. Moreover, for every measurement there is an infinite number of states that agree with the measurement.

f06-41-9780128042045 — Fig. 6.41 Observability analysis of the system from Example 6.21. (A) Three particular initial robot poses with same distance from the marker. (B) Updated situation from Fig. (A) after the robots have traveled the same distance in a forward direction. From the measurement point of view, all three robot poses are distinguishable. (C) Three particular initial robot poses that all have the same angle to and distance from the marker: a special case. (D) Updated situation from Fig. (C) after the robots have traveled the same distance in a forward direction. From the measurement point of view, all three robot poses are indistinguishable. (E) Updated situation from Fig. (C) after the robots have traveled the same not-straight path. From the measurement point of view, all three robot poses are indistinguishable.

Therefore, the system is not observable. The estimation bias can be eliminated if multiple markers are observed simultaneously, since this would provide enough information to select the appropriate solution, as it is shown in Example 6.22.

Example 6.22

This is an upgraded Example 6.21, in a way that now two spatially separated markers, instead of only one, are observed simultaneously. In this case the sensors measure the distances and angles to two markers (with respect to the mobile robot pose), which are positioned at x_M1 = 0, y_M1 = 0 and at x_M2 = 5, y_M2 = 5 in the plane (Fig. 6.42). Otherwise, all the data are identical to Example 6.21.

f06-42-9780128042045 — Fig. 6.42 Setup considered in Example 6.22. The mobile robot has a sensor for measuring the distance and angle to markers M1 and M2.

Solution

In this case only the correction part of the algorithm needs to be modified. The measurement vector now contains four elements, the distance and angle to the first marker, and also the distance and angle to the second marker:

$\begin{array}{l} {\hat{z}}_{k} = [\begin{matrix} \sqrt{{(x_{M 1} - x_{k})}^{2} + {(y_{M 1} - y_{k})}^{2}} \\ atan2 (y_{M 1} - y_{k}, x_{M 1} - x_{k}) - φ_{k} \\ \sqrt{{(x_{M 2} - x_{k})}^{2} + {(y_{M 2} - y_{k})}^{2}} \\ atan2 (y_{M 2} - y_{k}, x_{M 2} - x_{k}) - φ_{k} \end{matrix}] \end{array}$ $\begin{array}{l} {\hat{z}}_{k} = [\begin{matrix} \sqrt{{(x_{M 1} - x_{k})}^{2} + {(y_{M 1} - y_{k})}^{2}} \\ atan2 (y_{M 1} - y_{k}, x_{M 1} - x_{k}) - φ_{k} \\ \sqrt{{(x_{M 2} - x_{k})}^{2} + {(y_{M 2} - y_{k})}^{2}} \\ atan2 (y_{M 2} - y_{k}, x_{M 2} - x_{k}) - φ_{k} \end{matrix}] \end{array}$

si267_e

Let us determine the output matrix C (see Eq. 6.42) with linearization around the current predicted state estimate (x_k, y_k):

$\begin{array}{l} C = [\begin{matrix} \frac{x_{k}}{d_{1}} & \frac{y_{k}}{d_{1}} & 0 \\ - \frac{y_{k}}{d_{1}^{2}} & \frac{x_{k}}{d_{1}^{2}} & - 1 \\ \frac{x_{k}}{d_{2}} & \frac{y_{k}}{d_{2}} & 0 \\ - \frac{y_{k}}{d_{2}^{2}} & \frac{x_{k}}{d_{2}^{2}} & - 1 \end{matrix}] \end{array}$ $\begin{array}{l} C = [\begin{matrix} \frac{x_{k}}{d_{1}} & \frac{y_{k}}{d_{1}} & 0 \\ - \frac{y_{k}}{d_{1}^{2}} & \frac{x_{k}}{d_{1}^{2}} & - 1 \\ \frac{x_{k}}{d_{2}} & \frac{y_{k}}{d_{2}} & 0 \\ - \frac{y_{k}}{d_{2}^{2}} & \frac{x_{k}}{d_{2}^{2}} & - 1 \end{matrix}] \end{array}$

si268_e

where $d_{1} = \sqrt{{(x_{M 1} - x_{k})}^{2} + {(y_{M 1} - y_{k})}^{2}}$ $d_{1} = \sqrt{{(x_{M 1} - x_{k})}^{2} + {(y_{M 1} - y_{k})}^{2}}$ and $d_{2} = \sqrt{{(x_{M 2} - x_{k})}^{2} + {(y_{M 2} - y_{k})}^{2}}$ $d_{2} = \sqrt{{(x_{M 2} - x_{k})}^{2} + {(y_{M 2} - y_{k})}^{2}}$ . Let us also determine the measurement noise covariance matrix that is

$\begin{array}{l} R = [\begin{matrix} 0.5 & 0 & 0 & 0 \\ 0 & 0.3 & 0 & 0 \\ 0 & 0 & 0.5 & 0 \\ 0 & 0 & 0 & 0.3 \end{matrix}] \end{array}$ $\begin{array}{l} R = [\begin{matrix} 0.5 & 0 & 0 & 0 \\ 0 & 0.3 & 0 & 0 \\ 0 & 0 & 0.5 & 0 \\ 0 & 0 & 0 & 0.3 \end{matrix}] \end{array}$

si271_e

Listing 6.13 provides an implementation of the solution in Matlab. The simulation results are shown in Figs. 6.43–6.47. The obtained results confirm that the estimated states converge to the true robot pose.

f06-43-9780128042045 — Fig. 6.43 True (dashed line) and estimated (solid line) trajectory from Example 6.22.

f06-44-9780128042045 — Fig. 6.44 Pose estimate (solid line) and true state (dashed line) of the mobile robot with a nonzero initial estimation error from Example 6.22.

f06-45-9780128042045 — Fig. 6.45 Distance and angle measurements from Example 6.22.

f06-46-9780128042045 — Fig. 6.46 Robot pose estimate variance from Example 6.22.

f06-47-9780128042045 — Fig. 6.47 Time evolution of innovation from Example 6.22.

Listing 6.13

Implementation of the solution of Example 6.22

1 Ts = 0 . 1 ; % Sampling time

2 xTrue = [ 1 ; 2; pi / 6 ] ; % True Initial pose

3 x = [ 3 ; 0; 0 ] ; % Initial pose estimate

4 P = diag([9 90 . 6 ] ) ; % Initial covariance matrix of the pose estimate

5 Q = diag ( [ 0 . 1 0 . 1 ] ) ; % Noise covariance matrix of movement actuator

6 R = diag ( [ 0 . 5 0 . 3 ] ) ; % Noise covariance matrix of distance and

7 % angle measurement

8 enableNoise = 1; % Enable noise: 0 or 1

9 N = 300;% Number of simulation sample times

10 marker = [0 0; 55 ] ; % Positions of markers

12 figures_ekf3_init ;

14 % Loop

15 for k = 1:N

16 u = [ 0 . 5 ; 0 . 5 ] ; % Movement command (translational and angular velocity)

17 uNoisy = u+ sqrt(Q)*randn(2 , 1)* enableNoise ;

19 % Simulation of the true mobile robot state (pose)

20 xTrue = xTrue+ Ts *[ uNoisy (1) *cos(xTrue (3) ) ; …

21 uNoisy (1) *sin(xTrue (3) ) ; …

22 uNoisy (2) ] ;

23 xTrue (3) = wrapToPi(xTrue (3) ) ;

25 % Simulation of the true noisy measurements (distance and angle)

26 zTrue = [ ] ;

27 for m = 1: s i z e ( marker , 1)

28 dist = sqrt(( marker (m,1)− xTrue (1) ) ˆ2 + ( marker (m,2) − xTrue (2) ) ˆ2) ;

29 alpha = atan2( marker (m,2)− xTrue (2) , marker ( m,1)− xTrue (1) ) − xTrue (3) ;

30 zz= [ dist ; alpha ] + sqrt(R)*randn(2 , 1)* enableNoise ;

31 zz (1) = abs( zz (1) ) ;

32 zz (2) = wrapToPi( zz (2) ) ;

33 zTrue = [ zTrue ; zz ] ;

34 end

36 %%% Prediction (pose and speed estimation based on known inputs)

37 xPred = x+ Ts *[u(1) *cos(x (3) ) ; …

38 u(1) *sin(x (3) ) ; …

39 u(2) ] ;

40 xPred (3) = wrapToPi( xPred (3) ) ;

42 % Jacobian matrices

43 A = [1 0 − Ts * u (1) * sin(x (3) ) ; …

44 0 1 Ts*u(1) *cos(x (3) ) ; …

45 0 0 1 ] ;

46 F = [ Ts*cos(x (3) )0; …

47 Ts*sin(x (3) )0; …

48 0 Ts ] ;

49 PPred = A *P* A. ’ + F* Q *F . ’ ;

51 %%% Correction , measurement estimation

52 z = [ ] ;

53 C = [ ] ;

54 for m = 1: s i z e ( marker ,1)

55 dist = sqrt(( marker (m,1)− xPred (1) ) ˆ2 + ( marker (m,2) − xPred (2) ) ˆ2) ;

56 alpha = atan2( marker (m,2)− xPred (2) , marker ( m,1)− xPred (1) ) − xPred (3) ;

57 zz = [ dist ; alpha ] ;

58 zz (2) = wrapToPi( zz (2) ) ;

59 z = [ z ; zz ] ;

61 % Create matrix C for correction

62 c = [ xPred (1) / dist xPred (2) / dist 0; …

63 − xPred (2) / d i s t ˆ2 xPred (1) / dist ˆ2−1];

64 C = [ C ; c ] ;

65 end

67 % Measurement covariance matrix

68 RR = diag( repmat ( [R(1 ,1) R(2 ,2) ] , 1 ,s i z e ( marker , 1) ) ) ;

69 K = PPred* C. ’/(C *PPred* C. ’ + RR) ;

71 inov = zTrue − z ;

72 % Selection of appropriate innovation due to noise and angle wrapping

73 for m = 1: s i z e ( marker , 1)

74 inov (2* m )= wrapToPi( inov (2* m ) ) ;

75 end

77 x = xPred+ K*( inov ) ;

78 P = PPred − K * C * PPred ;

80 figures_ekf3_update ;

81 end

Again a simple graphical observability check can be performed. In this case one particular situation that has indistinguishable states can again be found, as shown in Fig. 6.48. At first glance the considered states are indistinguishable from the measurement point of view. But since we assume that the measurement data also contains marker IDs, this particular special situation has distinguishable states, and therefore the system is observable.

f06-48-9780128042045 — Fig. 6.48 Observability analysis of the system from Example 6.22. (A) Two particular poses that are symmetrical from the measurement point of view. (B) Updated situation from Fig. (A) after the robots have traveled the same relative path. From the measurement point of view, the both robot poses are distinguishable if measurement data contains IDs of the markers.

6.5.3 Kalman Filter Derivatives

Alongside the Kalman filter for linear systems and the EKF for nonlinear systems [10], many other extensions of the above filters have been proposed. Let us take a look at some of the most common and important approaches.

Unscented Kalman filter [11] is normally used in systems with significant nonlinearities where the EKF may not provide satisfactory results. In this case, the covariance matrices are estimated statistically based on a small subset of points that are transformed over the nonlinear function; the transformed points are used to estimate the mean value and the covariance matrix. These points are known as sigma points that are spread around the estimated value by some algorithm (normally there are 2n + 1 points for n dimensions).

Information filter uses the information matrix and information vector instead of the covariance matrix and state estimate. The information matrix represents the inverse of the covariance matrix, and the information vector is the product of the information matrix and estimated state vector. The information filter is dual to the Kalman filter where the correction step is computationally much simpler to evaluate (no matrix summation). However, the prediction step becomes more computationally demanding.

The Kalman-Bucy filter is a form of the Kalman filter for continuous systems.

6.6 Particle Filter

Up until now the Bayesian filter was considered to be normally used for discrete state-space systems with a finite number of values that the state-space variables can take. If we want to apply the Bayesian filter for continuous variables, these variables can be quantified to a finite number of values. This kind of Bayesian filter implementation for continuous state variables is commonly known as the histogram filter.

For continuous state-space variables an explicit solution of the Bayesian filter (6.29) would need to be determined:

$\begin{array}{l} \begin{matrix} p (x_{k} | z_{1 : k}, u_{0 : k - 1}) \\ = \frac{p (z_{k} | x_{k})}{p (z_{k} | z_{1 : k - 1}, u_{0 : k - 1})} \int p (x_{k} | x_{k - 1}, u_{k - 1}) p (x_{k - 1} | z_{1 : k - 1}, u_{0 : k - 2}) d x_{k - 1} \end{matrix} \end{array}$ $\begin{array}{l} \begin{matrix} p (x_{k} | z_{1 : k}, u_{0 : k - 1}) \\ = \frac{p (z_{k} | x_{k})}{p (z_{k} | z_{1 : k - 1}, u_{0 : k - 1})} \int p (x_{k} | x_{k - 1}, u_{k - 1}) p (x_{k - 1} | z_{1 : k - 1}, u_{0 : k - 2}) d x_{k - 1} \end{matrix} \end{array}$

si272_e (6.44)

where we took into account the complete state assumption and that the system is a Markov process (see Section 6.2). The current state probability distribution (6.44) is needed in calculation of the most probable state estimate (mathematical expectation):

$\begin{array}{l} E \{{\hat{x}}_{k | k}\} = \int x_{k | k} \cdot p (x_{k} | z_{1 : k}, u_{0 : k - 1}) d x_{k | k} \end{array}$ $\begin{array}{l} E \{{\hat{x}}_{k | k}\} = \int x_{k | k} \cdot p (x_{k} | z_{1 : k}, u_{0 : k - 1}) d x_{k | k} \end{array}$

si273_e

An explicit solution of Eq. (6.44) is possible only for a limited class of problems where Gaussian noise distribution and system linearity are assumed; the result is a Kalman filter. In the case of nonlinear systems, the system nonlinearity (in the motion model, actuator model, and/or sensor model) can be linearized, and this leads to the EKF.

A particle filter is a more general approach where noise distribution need not be Gaussian and the system may be nonlinear. The basic idea behind the particle filter is that the a posteriori state estimate probability distribution (6.29), after the measurement was made, is approximated with a set of N particles. Every particle in the set presents the value of the estimated state $x_{k}^{i}$ $x_{k}^{i}$ that is randomly sampled from the probability distribution (Monte Carlo simulation approach). Every particle represents a hypothesis about the system state. The representation of the probability distribution with a set of randomly generated particles is a nonparametric description of probability distribution, which is therefore not limited to only Gaussian distributions. The description of probability distribution with particles enables modeling of nonlinear transformation of noise (actuator and/or sensor model). The particles can therefore be used to describe the noise distributions that are transformed over nonlinear functions from inputs or outputs to the system states.

Algorithm 5 presents the basic implementation of the particle filter.

Algorithm 5

Particle Filter Algorithm. The Particle_filter Function Is Called in Every Time Step With Current Values of Inputs and Measurement and the Previous State Estimate

functionParticle_filter( ${\hat{x}}_{k - 1 | k - 1}$ ${\hat{x}}_{k - 1 | k - 1}$ , u_k−1, z_k)

Initialization:

if k > 0 then

Initialize a set of N particles $x_{k}^{i}$ $x_{k}^{i}$ based on a random sampling of the probability distribution p(x₀).

end if

Prediction:

Transform every particle ${\hat{x}}_{k - 1 | k - 1}^{i}$ ${\hat{x}}_{k - 1 | k - 1}^{i}$ based on the known movement model and known input u_k−1, to which add a randomly generated value based on the noise properties that are part of the movement model. The movement model is given with p(x_k|x_k−1, u_k−1). The obtained prediction is given as a set of particles ${\hat{x}}_{k | k - 1}^{i}$ ${\hat{x}}_{k | k - 1}^{i}$ .

Correction:

For every particle ${\hat{x}}_{k | k - 1}^{i}$ ${\hat{x}}_{k | k - 1}^{i}$ estimate measurement output that would be measured if the system state would correspond to the particle state.

Based on the measurement made and comparison against estimated measurements with particles, evaluate the particle importance.

This is followed by importance sampling, a sampling approach based on the particle importance with random selection of the particles in a way that the particle selection probability is proportional to the particle importance (i.e., $p (z_{k} | {\hat{x}}_{k | k - 1}^{i})$ $p (z_{k} | {\hat{x}}_{k | k - 1}^{i})$ ). More important particles should therefore be selected more times, and less important particles fewer times.

Filtered state estimate ${\hat{x}}_{k | k}$ ${\hat{x}}_{k | k}$ can be selected as the average value of all the particles.

end function

In the initial step of the particle filter an initial population of the particles needs to be selected, ${\hat{x}}_{0}^{i}$ ${\hat{x}}_{0}^{i}$ for i ∈ 1, …, N, which is distributed based on the belief (probability distribution) p(x₀) of the system states at the beginning of observation. In the case the initial state is not known a uniform distribution should be used; therefore, the particles should be spread uniformly over the entire state-space.

In the prediction part a new state for each particle is evaluated based on the known system input. To the state estimate of every particle a randomly generated noise is added, which is expected at the system input. In this way state predictions for every particle are obtained, ${\hat{x}}_{k | k - 1}^{i}$ ${\hat{x}}_{k | k - 1}^{i}$ . The added noise ensures that the particles are scattered, since this enables estimation of true value in the presence of different disturbances.

In the correction part of particle filter the importance of the particles is evaluated, in a way that the difference between the obtained measurement z_k and all the estimated particle measurements ( ${\hat{z}}_{k}^{i}$ ${\hat{z}}_{k}^{i}$ ) is calculated from estimated particle states. The difference between the measurement and estimated measurement is commonly known as innovation or measurement residual, and it can be evaluated for every particle as

$\begin{array}{l} {innov}_{k}^{i} = z_{k} - {\hat{z}}_{k}^{i} \end{array}$ $\begin{array}{l} {innov}_{k}^{i} = z_{k} - {\hat{z}}_{k}^{i} \end{array}$

which is low for most probable particles.

Based on measurement residual the importance of every particle, that is, the probability $p (z_{k} | {\hat{x}}_{k | k - 1}^{i})$ $p (z_{k} | {\hat{x}}_{k | k - 1}^{i})$ can be determined, which represents the weight $w_{k}^{i}$ $w_{k}^{i}$ for particle i. The weight can be determined with Gaussian probability distribution as

$\begin{array}{l} w_{k}^{i} = det {(2 π R)}^{- \frac{1}{2}} e^{- \frac{1}{2} {({innov}_{k}^{i})}^{T} R^{- 1} ({innov}_{k}^{i})} \end{array}$ $\begin{array}{l} w_{k}^{i} = det {(2 π R)}^{- \frac{1}{2}} e^{- \frac{1}{2} {({innov}_{k}^{i})}^{T} R^{- 1} ({innov}_{k}^{i})} \end{array}$

where R is the measurement covariance matrix.

A very important step in the particle filter is importance sampling based on the weights $w_{k}^{i}$ $w_{k}^{i}$ . The set of N particles is randomly sampled in a way that the probability of selection a particular particle from a set is proportional of the particle weight $w_{k}^{i}$ $w_{k}^{i}$ . Therefore, the particles with larger value of the weight should be selected more times than the particles with lower value of the weight; the particles with the smallest value of the weight should not even be selected. The approach of generating a new set of particles can be achieved in various ways, one of the possible approaches is given below:

• The particle weights $w_{k}^{i}$ $w_{k}^{i}$ are normed with the sum of all the weights $\sum_{i = 1}^{N} w_{k}^{i}$ $\sum_{i = 1}^{N} w_{k}^{i}$ ; in this way the new weights $w n_{k}^{i} = \frac{w_{k}^{i}}{\sum_{i = 1}^{N} w_{k}^{i}}$ $w n_{k}^{i} = \frac{w_{k}^{i}}{\sum_{i = 1}^{N} w_{k}^{i}}$ si293_e are obtained.

• The cumulative sum of the normed weights is calculated in order to obtain the cumulative weights $w c_{k}^{i} = \sum_{j = 1}^{i} w n_{k}^{i}$ $w c_{k}^{i} = \sum_{j = 1}^{i} w n_{k}^{i}$ , as shown in Fig. 6.49.

f06-49-9780128042045 — Fig. 6.49 Importance sampling in the correction part of the particle filter. The particle weights are arranged in a sequence one after another and then normed in a way that the sum of all the particles is one. More probable particles take more space on the unity interval, and vice-versa. The new population of particles is determined in a way that N numbers from the unity interval are selected randomly (with uniform distribution), and the particles that correspond to these numbers are selected.

• N numbers between 0 and 1 are selected randomly (from a uniform and 1 are selected randomly (from a uniform distribution) and then it is determined to which weights these random numbers correspond. The comparison between the cumulative weights $w c_{k}^{i}$ $w c_{k}^{i}$ and the randomly generated numbers is therefore made. As seen from Fig. 6.49, the particles with larger weights are more probable to be selected (the weights in Fig. 6.49 occupy more space).

• The selected particles are used in the correction part to determine the a posteriori state estimate ${\hat{x}}_{k | k} = \sum_{i = 1}^{N} w_{k}^{i} {\hat{x}}_{k | k - 1}^{i}$ ${\hat{x}}_{k | k} = \sum_{i = 1}^{N} w_{k}^{i} {\hat{x}}_{k | k - 1}^{i}$ .

In the case the system is not moving (the current state is equal to the previous state) it is recommended that the importance sampling is not made, but the previous particles are selected as the current ones and only the weights are modified, as shown in [12].

The application of particle filter is shown in Example 6.23.

Example 6.23

Use a particle filter for estimation of the most likely state in Example 6.22. In the implementation of particle filter use N = 300 particles. All the other data are the same as in Example 6.22.

Solution

Listing 6.14 provides an implementation of the solution in Matlab. The simulation results are shown in Figs. 6.50 and 6.51.

f06-50-9780128042045 — Fig. 6.50 True (dashed line), estimated (solid line) trajectory, and generated particles in the final step of simulation from Example 6.23.

f06-51-9780128042045 — Fig. 6.51 Pose estimate (solid line) and true state (dashed line) of the mobile robot with a nonzero initial estimation error from Example 6.23.

Listing 6.14

Implementation of the solution of Example 6.23

1 Ts = 0 . 1 ; % Sampling time

2 xTrue = [ 1 ; 2; pi / 6 ] ; % True Initial pose

3 x = [ 3 ; 0; 0 ] ; % Initial pose estimate

4 P = diag([9 90 . 6 ] ) ; % Initial covariance matrix of the pose estimate

5 Q = diag ( [ 0 . 1 0 . 1 ] ) ; % Noise covariance matrix of movement actuator

6 R = diag ( [ 0 . 5 0 . 3 ] ) ; % Noise covariance matrix of distance and

7 % angle measurement

8 enableNoise = 1; % Enable noise: 0 or 1

9 N = 300;% Number of simulation sample times

10 marker = [0 0; 55 ] ; % Positions of markers

12 % ParticleInitial i z a t i o n

13 nParticles = 300;

14 xP = repmat (xTrue , 1 ,nParticles )+ diag([4 41]) *randn(3 ,nParticles ) ;

15 W = ones ( nParticles , 1)/ nParticles ; % Allp a r t i c l e s have equal probability

17 figures_pf1_init ;

19 % Loop

20 for k = 1:N

21 u = [ 0 . 5 ; 0 . 5 ] ; % Movement command (translational and angular velocity)

22 u_sum = u+ sqrt(Q)*randn(2 , 1)* enableNoise ;

24 % Simulation of the true mobile robot state (pose)

25 xTrue = xTrue+ Ts *[u_sum(1) *cos(xTrue (3) ) ; …

26 u_sum(1) *sin(xTrue (3) ) ; …

27 u_sum(2) ] ;

28 xTrue (3) = wrapToPi(xTrue (3) ) ;

30 % Simulation of the true noisy measurements (distance and angle)

31 zTrue = [ ] ;

32 for m = 1: s i z e ( marker , 1)

33 dist = sqrt(( marker (m,1)− xTrue (1) ) ˆ2 + ( marker (m,2) − xTrue (2) ) ˆ2) ;

34 alpha = atan2( marker (m,2)− xTrue (2) , marker ( m,1)− xTrue (1) ) − xTrue (3) ;

35 zz = [ dist ; alpha ] + sqrt(R)*randn(2 , 1)* enableNoise ;

36 zz (1) = abs( zz (1) ) ;

37 zz (2) = wrapToPi( zz (2) ) ;

38 zTrue= [ zTrue ; zz ] ;

39 end

41 % Prediction

42 for p= 1: nParticles

43 % Particles are moved according to the noise model

44 un = u+ sqrt(Q)*randn(2 , 1) *1;

45 xP ( : , p)= xP ( : , p)+ Ts *[ un(1) *cos(xP(3 ,p) ) ; …

46 un(1) *sin(xP(3 ,p) ) ; …

47 un(2) ] ;

48 xP(3 ,p)= wrapToPi(xP(3 ,p) ) ;

49 end

51 % Correction

52 for p= 1: nParticles

53 % Estimated measurement for everyp a r t i c l e

54 z = [ ] ;

55 for m = 1: s i z e ( marker , 1)

56 dist = sqrt(( marker (m,1)− xP (1 , p ) ) ˆ2 + ( marker (m,2) − xP (2 , p ) ) ˆ2) ;

57 alpha = atan2( marker (m,2)− xP (2 , p ) , marker ( m,1)− xP (1 , p ) ) − xP (3 , p ) ;

58 zz = [ dist ; alpha ] ;

59 zz (1) = abs( zz (1) ) ;

60 zz (2) = wrapToPi( zz (2) ) ;

61 z = [ z ; zz ] ;

62 end

2 Innov = zTrue − z;% Determine innovation

4 % Selection of appropriate innovation due to

5 % noise and angle wrapping

6 for m = 1: s i z e ( marker , 1)

7 i i i = zTrue (2* m ) − ( z (2* m ) + [ 0 ; 2* pi; −2* pi ] ) ;

8 [ tmp , index ] = min(abs( i i i ) ) ;

9 Innov (2* m )= i i i ( index ) ;

10 end

12 % Determinep a r t i c l e weights ( p a r t i c l e probability)

13 % Measurement covariance matrix

14 RR = diag( repmat (diag(R) ,s i z e ( marker , 1) , 1) ) ;

15 W (p)= exp(−0.5* Innov . ’* inv ( RR ) * Innov ) + 0.0001;

16 end

18 iNextGeneration= obtainNextGenerationOfParticles ( W , nParticles ) ;

19 xP= xP ( : , iNextGeneration ) ;

21 % The new state estimatei s the average of all thep a r t i c l e s

22 x= mean(xP, 2) ;

23 x (3) = wrapToPi(x (3) ) ;

24 % For robot orientation use the mostl i k e l y p a r t i c l e instead of the

25 % average angle of all thep a r t i c l e s .

26 [ gg , ggi ] = max ( W ) ;

27 x (3) = xP(3 , ggi ) ;

29 figures_pf1_update ;

30 end

Listing 6.15

Function used in Listings 6.14 and 6.16

1 function iNextGeneration= obtainNextGenerationOfParticles ( W , nParticles )

2 % Selection based on thep a r t i c l e weights

3 CDF = cumsum ( W )/sum( W ) ;

4 i S e l e c t = rand( nParticles , 1) ; % Random numbers

5 % Indices of the newp a r t i c l e s

6 CDFg = [ 0 ; CDF] ;

7 indg= [ 1 ; (1: nParticles ) . ’ ] ;

8 iNextGeneration_float= interp1(CDFg, indg , iSelect , ’linear ’) ;

9 iNextGeneration= round( iNextGeneration_float+ 0.5) ; % Round the indices

10 end

In Example 6.23 the distances and angles to the markers are measured, so the angle wrapping must be taken into account. The example of localization where only distances to the markers are measured in order to estimate the mobile robot pose is shown in Example 6.24.

Example 6.24

Use a particle filter for estimation of the most likely state in Example 6.23, but consider only measurement of the distance to the markers. In the implementation of the particle filter use N = 500 particles. All the other data are the same as in Example 6.23.

Solution

Listing 6.16 provides an implementation of the solution in Matlab. The simulation results are shown in Figs. 6.52 and 6.53, where it is clear that the estimated value converges to the true value, as in Example 6.23.

f06-52-9780128042045 — Fig. 6.52 True (dashed line), estimated (solid line) trajectory, and generated particles in the final step of simulation from Example 6.24.

f06-53-9780128042045 — Fig. 6.53 Pose estimate (solid line) and true state (dashed line) of the mobile robot with a nonzero initial estimation error from Example 6.24.

Listing 6.16

Implementation of the solution of Example 6.24

1 Ts =0 . 1 ; % Sampling time

2 xTrue= [ 1 ; 2; pi / 6 ] ; % True Initial pose

3 x= [ 3 ; 0; 0 ] ; % Initial pose estimate

4 P = diag([9 90 . 6 ] ) ; % Initial covariance matrix of the pose estimate

5 Q = diag ( [ 0 . 1 0 . 1 ] ) ; % Noise covariance matrix of movement actuator

6 R = diag ( [ 0 . 5 0 . 3 ] ) ; % Noise covariance matrix of distance and

7 % angle measurement

8 enableNoise= 1; % Enable noise: 0 or 1

9 N = 300;% Number of simulation sample times

10 marker= [0 0; 55 ] ; % Positions of markers

11 R = R(1 ,1) ; % Only distance measurement

13 % ParticleInitial i z a t i o n

14 nParticles = 500;

15 xP= repmat (xTrue , 1 ,nParticles )+ diag([4 41]) *randn(3 ,nParticles ) ;

16 W = ones ( nParticles , 1)/ nParticles ; % Allp a r t i c l e s have equal probability

18 figures_pf2_init ;

20 % Loop

21 for k= 1:N

22 u= [ 0 . 5 ; 0 . 5 ] ; % Movement command (translational and angular velocity)

23 u_sum= u+ sqrt(Q)*randn(2 , 1)* enableNoise ;

25 % Simulation of the true mobile robot state (pose)

26 xTrue= xTrue+ Ts *[u_sum(1) *cos(xTrue (3) ) ; …

27 u_sum(1) *sin(xTrue (3) ) ; …

28 u_sum(2) ] ;

29 xTrue (3) = wrapToPi(xTrue (3) ) ;

31 % Simulation of the true noisy measurements (distance)

32 zTrue= [ ] ;

33 for m = 1: s i z e ( marker , 1)

34 dist = sqrt(( marker (m,1)− xTrue (1) ) ˆ2 + ( marker (m,2) − xTrue (2) ) ˆ2) ;

1 zz =[ dist ] + sqrt(R)*randn(1 , 1)* enableNoise ;

2 zz (1) = abs( zz (1) ) ;

3 zTrue= [ zTrue ; zz ] ;

4 end

6 % Prediction

7 for p= 1: nParticles

8 % Particles are moved according to the noise model

9 un= u+ sqrt(Q)*randn(2 , 1) *1;

10 xP ( : , p)= xP ( : , p)+ Ts *[ un(1) *cos(xP(3 ,p) ) ; …

11 un(1) *sin(xP(3 ,p) ) ; …

12 un(2) ] ;

13 xP(3 ,p)= wrapToPi(xP(3 ,p) ) ;

14 end

16 % Correction

17 for p= 1: nParticles

18 % Estimated measurement for everyp a r t i c l e

19 z= [ ] ;

20 for m = 1: s i z e ( marker , 1)

21 dist = sqrt(( marker (m,1)− xP (1 , p ) ) ˆ2 + ( marker (m,2) − xP (2 , p ) ) ˆ2) ;

22 zz= [ dist ] ;

23 zz (1) = abs( zz (1) ) ;

24 z= [ z ; zz ] ;

25 end

27 Innov= zTrue − z ; % Determine innovation

29 % Determinep a r t i c l e weights ( p a r t i c l e probability)

30 % Measurement covariance matrix

31 RR = diag( repmat (diag(R) ,s i z e ( marker , 1) , 1) ) ;

32 W (p)= exp(−0.5* Innov . ’* inv ( RR ) * Innov ) + 0.0001;

33 end

35 iNextGeneration= obtainNextGenerationOfParticles ( W , nParticles ) ;

36 xP= xP ( : , iNextGeneration ) ;

38 % The new state estimatei s the average of all thep a r t i c l e s

39 x= mean(xP, 2) ;

40 x (3) = wrapToPi(x (3) ) ;

41 % For robot orientation use the mostl i k e l y p a r t i c l e instead of the

42 % average angle of all thep a r t i c l e s .

43 [ gg , ggi ] = max ( W ) ;

44 x (3) = xP(3 , ggi ) ;

46 figures_pf2_update ;

47 end

A particle filter is an implementation of the Bayesian filter for continuous systems (continuous state space); it enables description of nonlinear systems and can take into account arbitrary distribution of noise. Applicability of the particle filter is computationally intensive for high-dimensional systems, since many particles are required to achieve filter to converge. The number of required particles increases with space dimensionality.

The particle filter is robust and is able to solve the problem of global localization and the problem of robot kidnapping. In the problem of global localization the initial pose (the value of the states) is not known. Therefore, the mobile robot can be anywhere in the space. The kidnapping problem occurs when the mobile robot is moved (kidnapped) to an arbitrary new location. Robust localization algorithms must be able to solve these problems.

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

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

Table of Contents for 6.6 Particle Filter

Create new playlist

Sign In

Sign Up

6.5.2 Extended Kalman Filter

6.5.3 Kalman Filter Derivatives

6.6 Particle Filter

Table of Contents for
6.6 Particle Filter