Review (pp. 270-275 of the book):

In general the position of a differential drive robot, like the E-Puck, is given by the vector

$p = \left[\begin{array}{c}x\\y\\\theta\end{array}\right]$

This position changes as the robot drives during each time step, $\Delta t$. The incremental travel distances, or the change in the values of $(x, y, \theta)$, are given by

$\Delta x = \Delta s cos(\theta + \frac{\Delta \theta}{2})$,

$\Delta y = \Delta s sin(\theta + \frac{\Delta \theta}{2})$,

$\Delta \theta = \frac{\Delta s_r - \Delta s_l}{b}$,

$\Delta s = \frac{\Delta s_r + \Delta s_l}{2}$,

where $\Delta s_r$ and $\Delta s_l$ are the distances traveled by the right and left wheels respectfully as given by the encoder values, $b$ is the axle length, 0.052m, and $\theta$ is the angle of rotation between the robot and inertial frames of reference. Any new position of the robot, $p'$ is given by the last position, $p$ plus the change in position $(\Delta x, \Delta y, \Delta \theta)$

$p' = p + \left[\begin{array}{c}\Delta x\\\Delta y\\\Delta \theta\end{array}\right]$

or making the substitutions for $p, \Delta x,$ and $\Delta y$ from above

p' = \left[\begin{array}{c}x\\y\\\theta\end{array}\right] + \left[\begin{array}{c}\Delta s cos(\theta + \frac{\Delta \theta}{2})\\\noalign{\medskip}\Delta s sin(\theta + \frac{\Delta \theta}{2})\\\noalign{\medskip}\Delta \theta\end{array}\right]           (equation 1)

If we further substitute the values for $\Delta s$ and $\Delta \theta$ from our first equations, we get

p' = \left[\begin{array}{c}x\\y\\\theta\end{array}\right] + \left[\begin{array}{c}\frac{\Delta s_r + \Delta s_l}{2}cos(\theta + \frac{\Delta s_r - \Delta s_l}{2b})\\\noalign{\medskip}\frac{\Delta s_r + \Delta s_l}{2}sin(\theta + \frac{\Delta s_r - \Delta s_l}{2b})\\\noalign{\medskip}\frac{\Delta s_r - \Delta s_l}{b}\end{array}\right]        (equation 2)

This last equation, equation 2, is the equation we will focus on. If we want to actually get numbers from all this mess, we only need a couple pieces of information. We basically have $p' = p + \Delta p$. We need the initial start position $(x, y, \theta)$, which we will plug into $p$. We then need to know the distance that the robot’s wheels move each time step, $\Delta s_r$ and $\Delta s_l$. These distances are given by the encoder using the equations

$\Delta s_r = \frac{0.0205getRightEncoder()}{159.23}$

$\Delta s_l = \frac{0.0205getLeftEncoder()}{159.23}$             (equation 3)

With these three pieces of information, initial position, $\Delta s_r$ and $\Delta s_l$, we can keep track of the position of the robot at any given time. This is very similar to the odometry we did in Lab #4. One thing that you all noticed in Lab #4 was that the results you were getting for position were always slightly off from the “real” position of the robot as given by the omniscient translation field in the Webots Scene Tree. This is due to the error inherent in the encoder values. In this lab we will take this process one step further and incorporate error into our calculations.

Error Propogation (pp. 270-275 in the book):

Remember from the lecture on error propagation, or from the book p. 273, that the covariance matrix for the integrated position p’ is

$\Sigma_{p'}=\nabla_p f \Sigma_p \nabla_p f^t + \nabla_{\Delta_{r,l}}f \Sigma_{\Delta}\nabla_{\Delta_{r,l}}f^T$

Many of you are more comfortable with the notation on p. 114 of the book, also given in the last lecture,

$C_y = F_x C_x F_x^T$

These two are essentially the same except that the first takes into account the variances of both $p$ and $\Delta p$.  To use the second version we would really need something like

$C_y = F_x C_x F_x^T + F_{x1} C_{x1} F_{x1}^T$            (equation 4)

where $C_x$ represents the covariance of the previous position and $C_{x1}$ represents the covariance of the change in position.  We will use the notation of the second version for $C_y$ directly above to find the error inherent in the new position estimate for $p'$.  To do this we first need the Jacobian matrix for the previous position, $F_x$, where the function we are using is the function in (equation 2) above, although looking at (equation 1) may make it clearer.  The Jacobian matrix is

F_x = \left[\begin{array}{ccc} \frac{\partial f}{\partial x} & \frac{\partial f}{\partial y} & \frac{\partial f}{\partial \theta}\end{array}\right] = \left[\begin{array}{ccc} 1 & 0 & -\Delta s sin(\theta + \frac{\Delta \theta}{2})\\ \noalign{\medskip} 0 & 1 & \Delta s cos(\theta + \frac{\Delta \theta}{2})\\ \noalign{\medskip} 0 & 0 & 1\end{array}\right]     (equation 5)

Next we need the value for $C_x$.  In the very first iteration, in which $p$ is the initial position, the covariance matrix is all zeros since we know where we start.  In all subsequent iterations, $C_x$ is the covariance $C_y$ found in the previous iteration.

Next we need the Jacobian matrix for the change in position, $F_{x1}$

F_{x1} = \left[\begin{array}{cc} \frac{\partial f}{\partial \Delta s_r} & \frac{\partial f}{\partial \Delta s_l}\end{array}\right] = \left[\begin{array}{cc} \frac{\partial \Delta s}{\partial \Delta s_r}cos(\theta + \frac{\Delta \theta}{2}) - \frac{\Delta s}{2}sin(\theta + \frac{\Delta \theta}{2})\frac{\partial \Delta \theta}{\partial \Delta s_r}\hspace{0.25in} & \frac{\partial \Delta s}{\partial \Delta s_l}cos(\theta + \frac{\Delta \theta}{2}) - \frac{\Delta s}{2}sin(\theta + \frac{\Delta \theta}{2})\frac{\partial \Delta \theta}{\partial \Delta s_l}\\ \noalign{\medskip} \noalign{\medskip} \frac{\partial \Delta s}{\partial \Delta s_r}sin(\theta + \frac{\Delta \theta}{2}) + \frac{\Delta s}{2}cos(\theta + \frac{\Delta \theta}{2})\frac{\partial \Delta \theta}{\partial \Delta s_r}\hspace{0.25in} & \frac{\partial \Delta s}{\partial \Delta s_l}sin(\theta + \frac{\Delta \theta}{2}) + \frac{\Delta s}{2}cos(\theta + \frac{\Delta \theta}{2})\frac{\partial \Delta \theta}{\partial \Delta s_l}\\ \noalign{\medskip} \noalign{\medskip} \frac{\partial \Delta \theta}{\partial \Delta s_r}\hspace{0.25in} & \frac{\partial \Delta \theta}{\partial \Delta s_l}\end{array}\right]

We can simplify this matrix a little given

$\Delta s = \frac{\Delta s_r + \Delta s_l}{2}$;             $\Delta \theta = \frac{\Delta s_r - \Delta s_l}{b}$

If we take the partial derivatives of $\Delta s$ and $\Delta \theta$

$\frac{\partial \Delta s}{\partial \Delta s_r} = \frac{1}{2}$     ;      $\frac{\partial \Delta s}{\partial \Delta s_l} = \frac{1}{2}$      ;      $\frac{\partial \Delta \theta}{\partial \Delta s_r} = \frac{1}{b}$      ;      $\frac{\partial \Delta \theta}{\partial \Delta s_r} = -\frac{1}{b}$

and substitute them into the Jacobian matrix for $F_{x1}$ above, we get

F_{x1} = \left[\begin{array}{cc} \frac{\partial f}{\partial \Delta s_r} & \frac{\partial f}{\partial \Delta s_l}\end{array}\right] = \left[\begin{array}{cc} \frac{1}{2}cos(\theta + \frac{\Delta \theta}{2}) - \frac{\Delta s}{2b}sin(\theta + \frac{\Delta \theta}{2})\hspace{0.25in} & \frac{1}{2}cos(\theta + \frac{\Delta \theta}{2}) + \frac{\Delta s}{2b}sin(\theta + \frac{\Delta \theta}{2})\\ \noalign{\medskip} \noalign{\medskip} \frac{1}{2}sin(\theta + \frac{\Delta \theta}{2}) + \frac{\Delta s}{2b}cos(\theta + \frac{\Delta \theta}{2})\hspace{0.25in} & \frac{1}{2}sin(\theta + \frac{\Delta \theta}{2}) - \frac{\Delta s}{2b}cos(\theta + \frac{\Delta \theta}{2})\\ \noalign{\medskip} \noalign{\medskip} \frac{1}{b} & -\frac{1}{b}\end{array}\right]   (equation 6)

The last piece of information we need is the covariance matrix for the change in position, $C_{x1}$.  This is the matrix

$C_{x1} = \left[\begin{array}{cc} \sigma_{\Delta s_r}^2 & 0\\0 & \sigma_{\Delta s_l}^2\end{array}\right]$        (equation 7)

The zeros indicate that the error from the right wheel and the error from the left wheel are independent of one another.  The values along the diagonal are the variances of the error from the right and left wheel respectively.  These values are found experimentally.  To find these values drive the E-Puck for one time-step a number of times and gather data on the distance measured by the odometers and the actual distance traveled as given by the Scene Tree.  Again the distances traveled, $\Delta s_r$ and $\Delta s_l$, can be found using (equation 3) above.For simplicity, just drive the robot in a straight line.  Remember from the lecture that the variance is calculated as follows

$\sigma^2=\int_{-\infty}^{\infty}(x-\mu)^2f(x)dx$
where $x$ is the observed data point, $\mu$ is the mean value, and $f(x)$ is the probability distribution function.  Of course in our experiments we won’t have the benefit of an infinite number of trials, but the variance can be found in a similar fashion from a sample.
As the variance of $\sigma_{\Delta s_r}$ and $\sigma_{\Delta s_l}$ should vary with the actual amount the robot traveled, it is customary to assume $\sigma_{\Delta s_r}=k|\Delta s_r|$ and $\sigma_{\Delta s_l}=k|\Delta s_l|$. If you perform your calibration experiments with constant, known speed, you can calculate the constant $k$.

Summary:

To find the position of the E-Puck at any time step, use (equation 2).  You will need to seed the initial position and rotation of the robot.  To find the error associated with your position p’, use (equation 4) and plug in the results from (equation 5), (equation 6), and (equation 7).  This will give you a 3×3 matrix.  To get the error in the x and y position we only need the first two values along the diagonal respectively.  Ultimately the position (x, y) and the associated error $(\sigma_x^2, \sigma_y^2)$, will be used in a 2D Gaussian to find the position of the E-Puck in the maze as a probability distribution over all the possible cells.

Exercise:

1)  Implement the error propagation model above to print the position and associated error of the E-Puck as it drives.  You will want a relatively open world.  You can either create one from scratch (see note below) or use the world from the odometry lab. To start, find the variance of $\Delta s_r$ and $\Delta s_l$ experimentally as described right above the summary.

2) (Advanced)  Calculate the probability that the robot is in any of the 10×10 squares by calculating the value of the 2D Gaussian, $f(x, y) = e^{-(\frac{(x-x_o)^2}{2\sigma_x^2} + \frac{(y-y_o)^2}{2\sigma_y^2})}$, at the center coordinate of each square.  In this equation $x_o$ and $y_o$ are the actual coordinates of the center of the square the E-Puck is in, x and y are the position you have based on odometry, and $\sigma_x^2$ and $\sigma_y^2$ are the variances associated with your position which you found using the error propagation model.  Since technically you would need to integrate the probability over each square (instead of just taking the center point), you will need to normalize the field with the sum of all probabilities so that the whole adds up to 1.

Note: If you want to create a totally open world from scratch, open a new world and add physics to the floor.  Select the floor node which should be the first Transform node in the scene tree, just after the PointLight nodes. Turn that Transform into a solid node using the Transform button. Now it is possible to define a bounding object for the floor. Create a Transform node under the boundingObject field in the floor node. Under the children of the Transform you just created under the boundingObject create a Box node and set the size to [1  0.02  1]. Set the translation of the boundingObject Transform to [0.5  -0.01  0.5]. Now we can introduce an E-Puck. Select the last solid from the list and click Add New proto and choose e-puck. This will give you a blank world with an E-Puck to work with.