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
This position changes as the robot drives during each time step, . The incremental travel distances, or the change in the values of , are given by
where and are the distances traveled by the right and left wheels respectfully as given by the encoder values, is the axle length, 0.052m, and is the angle of rotation between the robot and inertial frames of reference. Any new position of the robot, is given by the last position, plus the change in position
or making the substitutions for and from above
If we further substitute the values for and from our first equations, we get
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 . We need the initial start position , which we will plug into . We then need to know the distance that the robot’s wheels move each time step, and . These distances are given by the encoder using the equations
With these three pieces of information, initial position, and , 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
Many of you are more comfortable with the notation on p. 114 of the book, also given in the last lecture,
These two are essentially the same except that the first takes into account the variances of both and . To use the second version we would really need something like
where represents the covariance of the previous position and represents the covariance of the change in position. We will use the notation of the second version for directly above to find the error inherent in the new position estimate for . To do this we first need the Jacobian matrix for the previous position, , 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
Next we need the value for . In the very first iteration, in which is the initial position, the covariance matrix is all zeros since we know where we start. In all subsequent iterations, is the covariance found in the previous iteration.
Next we need the Jacobian matrix for the change in position,
We can simplify this matrix a little given
If we take the partial derivatives of and
; ; ;
and substitute them into the Jacobian matrix for above, we get
The last piece of information we need is the covariance matrix for the change in position, . This is the matrix
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, and , 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
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 , 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.
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 and 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, , at the center coordinate of each square. In this equation and 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 and 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.