So all we have to do is, using the known position, compute the position of the landmark and of course compute the appropriate covariance. So as for the landmark position, we know that the measurement can be computed from our current pose and landmark coordinates using our measurement function H.

So what we want now is the inverse of this. We want to get our landmark from the current pose and our measurement. So with regard to M and C, this is the inverse function. So this is already the first step. We use the inverse function, the current pose and the known range and bearing measurement to compute the landmark position M.

Now in our Python code, we'll have to compute the scanner pose because it is different from the robot's pose and we may use the lego.log file .scannerToWorld function to compute the world coordinates M of our new landmark from the X, Y coordinates in the scanner's coordinate system. So this is what we'll use in the code.

Now let's have a look at the covariance matrix. So we know that the Jacobian of H is a capital H, that's the derivative of H with respect to the landmark and we use that already to compute the likelihoods. Now we would have to take this at the current state and the landmark coordinates X and Y which we just computed.

Now this H matrix is the Jacobian of this H function which translates from our landmark coordinates to our measurement. So this H gives us the information how our landmark noise translates to a measurement noise. But now we have the inverse problem. Our robot measured the range and bearing and this noise translates to the noise of

the landmark. So we need the Jacobian of the inverse of H which is of course the inverse of this H matrix. And so in order to compute this covariance which we're looking for, all we have to do is take the measurement noise which is the variance in range and bearing and use variance propagation now with the matrix H minus one.

And after this we'll have to append our new landmark coordinates to our landmark positions list and our new covariance matrix to the landmark covariances list. And that's all there is to do. So in summary, we compute the landmark's coordinates mathematically by computing H minus one of the state and our measurement and practically by computing the scanner pose and using the

scanner to world function. Then we compute the Jacobian which we take at the current state and at the landmark coordinates which we just computed. We invert this and compute our landmark covariance by this variance propagation. Now I have prepared the SLAM-10C new landmark file for you and as usual it contains our

particle class with a constructor and a utility function. It also contains the derivative of H with respect to a landmark which we just had in our previous exercise. And then here, this is the function you'll have to implement. And here in the first part, I gave a hint how to compute the scanner pose given the particle state.

And then remember, after you computed the position and covariance matrix of the landmark, insert them into the particles list of positions and covariances. So that's all there is to do. Let's have a look at the main function. Now the main function again places the scanner at the origin of the coordinate system.

After setting up the particle, three landmarks are measured and those measurements are given in the scanner's coordinate system. The first measurement is at X equals 1000 relative to the scanner which happens to be aligned with our world coordinate system in this case. So this is at 1000, 0.

The second measurement is at 2000, 0. And the third measurement is at 1000, 1000 divided by the square root of 2 which means it will be here. And after you run this, you should see the following result. So for landmark 0, position is initialized as 1000, 0. So that's correct.

And the landmark covariance matrix contains 40,000 and 68,000 on the main diagonal which is converging to the error ellipse here. So it means 200 along the axis with degree 0 and 260 along the other axis. So it's slightly elongated. Now for the first landmark, we get 2000, 0. So that's correct too. With the covariance matrix which corresponds to an error ellipse which is 200 in one direction,

again, so it's the same. But 523 in the other direction which is just double the value that we had earlier. So this is times two. And the reason is of course that this error is dependent on the range accuracy. And so it's the same in both cases. But the error along the other axis is dependent on the error of the bearing angle measurement.

And so it increases linearly with the distance. And finally for the third landmark, landmark number two, again, we get the correct result for the position which is here. Now our covariance looks much more complicated. But as we see in terms of the error ellipse, we'll have the exactly same as in the first case.

So it's 200 along one axis and 261 along the other axis. That's exactly what we have here. The first axis is at minus 45 degrees, so the axis is like that. We have 261 along this axis and 200 along the other axis, so it's slightly elongated.

And of course we see it's the exactly same error ellipse as the red one because this point is measured at the same distance. The only difference being that this ellipse is turned by 45 degrees. So now please program the code to initialize a new landmark.