theta = atan2(2*b, a-c)/2.0;(This "theta" is the angle of the major axis of the ellipse with respect to the x axis.)
If (a*c - b*b) is zero (you should actually check that its absolute value is less than some small number such as 1e-8 instead of testing for equality with zero), then the distribution represented by the covariance matrix is degenerate --- it is a one dimensional distribution, not a two dimensional distribution. Your uncertainty bounds should then be a degenerate ellipse, i.e. a line. The angle "theta" from above is still the correct angle of this line with respect to the x axis. However, to compute the length of the line, you need to know the variance of the 1D Gaussian distribution. It is:sigma^2 = a + cIf you want a 95.4% uncertaintly limit, then the radius of the major axis of this degenerate ellipse would be 2*sigma.For the regular (nondegerate case) you should use equations 5.28-30 to compute the "radii" of the ellipse.
The range sensor calculates derivatives, but right now they are not correct. I wanted to get this out now, so I'll have to release a new version of simrobot.cc when I get this fixed.
Still to come is a handout and/or a new section on this page with more information on the assignment, including matrix manipulation.
Are the aspect ratio and the minor radius the same thing for an ellipse? If not, then how do you calculate the aspect ratio/where does the minor radius come into play for calculating an ellipse?
Answer: I've defined the aspect ratio of the ellipse to be the radius on the minor axis divided by the radius on the major axis. (This is mentioned in graphics.h. Generally speaking, I will comment things in the .h file since that defines the interface which you will include in another file that makes use of it.)
Is it correct that the kalman filter is only used in the second half of the assignment (when we have a measurement)?
Answer: Not really, though the part of the Kalman filter that is used when there is no measurement is something that was known before Kalman came along.
How do you calculate the "cones" of 95.4 certainty? I assume it is some calculation using the value of sigma theta squared. How are we meant to display the "cones"? With lines?
Answer: You get the variance of theta from the covariance matrix. This characterizes a 1D Gaussian distribution. See the course notes on the Gaussian distribution and confidence limits for further information.
Yes, you should display the cones with two lines, one along each edge.
In the equations for the ellipse, k is the constant that defines the certainty. Is this a straight percentage or is there some way we have to calculate it? I am currently just using the value .954, but I do not know if this is correct.
Answer: Read the course notes. (See the section on confidence limits.)
i dont understand what the vector u(k) means exactly. it is supposed to be the odometry measurement from state x(k). now, suppose that at state x(k) the robot is in position (x,y,A). the input file command tells it to move by vector (d,B). so the robot moves by vector (d,B).
Answer: The vector u(k) is the odometry measurement from the motion command that was executed from the configuration x(k). In that sense, it is actually not a measurement at time step k but rather after the motion has been completed.
The way I've set up this problem, the sequence of events goes like this:
It is also possible to approach this problem in a slightly different way --- by treating the commanded motion as the input to the system and then by using both the odometry and the laser rangefinder as "measurements" for the extended Kalman filter. Since this is a little more involved, I've suggested the first approach above.
Answer: As the assignment handout states, the sensor returns the range (i.e. distance) to the nearest obstacle. In the handout, it says it will be either a SONAR sensor or a laser rangefinder. I've decided that it will be a laser rangefinder. This means that it sends out a beam of light in the requested direction and measures the distance to the point where it hits and obstacle (or the world boundary). (Note that the requested direction is an angle with respect to the robot's orientation.) The value it returns is a scalar, the measured distance plus some Gaussian noise. This is what the H function computes in equation 6 (and 7) in the assignment handout.
Does eq. 2 of the assignment handout have the matrix form/sizes [rows x columns]: [3x1] = [3x1] + [3x3] + [3x2]? If so, how are these matrices added together? I thought matrix addition required the matrices be of the same size?
Answer: As Equation 3 shows, J_x is a 3x3 matrix and J_u is a 3x2 matrix. However, these are multiplied on the right (respectively) by (x - \hat x(k-1)) which is a 3x1 column vector and by (u - u(k-1)) which is a 2x1 column vector. Perhaps this is just because the notation for function arguments versus time step "arguments" might not be consistent? Also see the announcement above regarding the correction to Equation 3.
with regards to the assignment2, equation 6 you state z = H[x] + v(k) Is the H[x] in eqn 6 H[x^(k)] or H[k] or H[x^(k|k-1)] or can this equation be rewritten as z(k) = H[k] + v(k)
Answer: Equation 6 in the assignment handout is only supposed to show that the measurement is a function of the state and that there is some noise in the measurement. The "k index" for the noise term v(k) indicates that the distribution of the noise can be different at each time step.
Strictly speaking, I should have written:
z(k) = H(k)[ x(k) ] + v(k)This would properly indicate (in addition to the above) that the measurement z at step k is a function of the state x at step k and that this function H[] can change from step to step. Since I'm providing code that computes H[] and the corresponding derivatives, you don't need to worry about this fact. It should not make any difference in your answer to the written questions where you can just treat it as a function.
One somewhat unintuitive thing is that you must declare the size of the matrix. The constructor for the general matrix takes two integers as the first two arguments which specify the size. The remaining arguments are the elements of the matrix, given row by row, and they must not be integers. (So add a ".0" where needed.) You can access elements of matrices using regular C/C++ style array access (with brackets). The matrices are 0 indexed.
See the new assign2.cc file for some examples. You can also look at the documentation.
I haven't yet tried it out myself, but it should be useful (and work).
The README file that comes with this package is not quite accurate about the install. Here are the commands I used to compile, install, and test the package:
make linux_RH ; to create configuration files make ; to compile the library make DEST=/usr/local/svl install ; after creating this directory, etc. cd test make test