Welcome to the second exercise for the extended common shoulder in this exercise.
We want to implement the extended coming update equations and the non-linear light on measurement model.
The first step in this process is to open the last common file from the previous exercise, which had
the prediction step completed, and we want to review the code that has already been written for the
update step as part of this exercise.
So looking at the file, you can see that we have this handle light measurements function.
And this function is called every time we get a series of the measurements, it automatically takes
in the data set of the line of measurement.
So a vector of measurements that are made at that time step and it takes in the landmark map.
And all of this function does is that recursively calls a lot of measurement for each of the measurements
inside the data set.
So this function here is this larger function that's expanded out of here.
And this is where we're going to be modifying the code for the Lider measurement model inside the external
filter.
It is important to know that a lot of measurements contains the measurement from the light, a sensor
for that landmark, so it contains a range and relative bearing for that measurement.
It also contains the associated landmarked.
So this set of functions here, it gets the measurement ID.
So this is landmark ID that the measurement is made against and then it looks at this ID inside the
map and that returns.
Bakan data for the associated bacon of this relies on the fact that the measurement ID does not equal
negative one and the Beacon ID does not equal the negative one.
So if these two conditions are met, then we have matched up the light and measurement to a beacon that
we know inside the map.
Once we've done that, then we can access the Bacons X and Y locations using the map back into X and
map Y.
So the light of measurement model that we want to implement is shown here.
So this is a set of equations.
Now, the X and Y are the landmark locations and they can be read from the map beacons or X and that
bacon Y that we've already just said.
And then the state vectors for the position of the X position Y as well as the heading of the vehicle
can all be extracted from the state vector.
And it's also important to remember that the state vector follows this convention here.
So the state that Texas or the position in the X Y and then the heading and then the velocity.
So this is a form of the state variables inside the state vector.
The next step is to implement the innovation calculations, so we want to calculate the measurement
innovation and remember any of the angle measurements inside, the innovation has to be normalized with
the angle function like we did in the prediction step.
We also want to calculate the measurement model Jacobean.
So The Matrix, so we can do that from this set of Jacobean matrices here.
And then we want to calculate the innovation covariance matrix, the matrix.
And again, that is done through this equation here.
And for this equation, we will assume that the uncertainty in the measurements is given by a range
variance and a relative bearing variance which can be accessed by the constants inside the top of the
file.
So they lied our range standard deviation and the light theta standard deviation.
Then the next step is to implement the actual EAF update step equations themselves, so we want to calculate
the come again and using the common gane and innovation, update the state vector.
And we also want to update the covariance matrix as well.
Once we've done that, we want to run the simulation and we want to run the simulation for all the different
profiles.
So propose one to eight.
Now, the profiles, five, six, seven, eight are pretty much just profiles.
One, two, three, four.
But we also include the Leida measurements inside the simulation so we can see how, including the light
and measurements inside the simulation improves the performance of the filter.
And then the last step is to compare the simulation results between the linear computer and the extended
computer with and without the Leida.
So that's comparing profiles, wonderful and profiles five to eight for the extended common theorem.
And then the profile is one to four for the linear common filter.