Now I want you to extend your routine to accommodate the landmark
Specifically, I want you to use a function that I coded for you that is very useful
that is called "expand."
You can run omega.expand, xi.expand to take a 3 x 3 matrix or vector
and move it to a 4 x 4 vector that you actually need when you have to include
the landmark itself.
Give that a try and see if you can modify the code
to now have additional input parameters of measurement 0, 1, and 2.
In particular, here is our new doit routine.
It now has as input parameters my 2 motion commands
and the 3 measurement commands for the 3 different poses.
Here is the code that you produced before.
That's my version of it where we have the initial 3 x 3 matrix.
Then using the expansion command
you can now increase those to a 4 x 4 matrix and a 4 x 1 vector.
When you run it what comes out is this result over here-- -3, 2, 5, 7.
I want you to do this where -3 and 2 and 5 is the robot path,
and 7, as before, is the landmark location.
Please code this and realize that I can modify the input to doit just fine,
and your code should not just produce this one vector,
but it should implement the right math.