Now, let's have a look at the second exercise for the uncynical Matura, so this is to implement the
update step.
So this exercise is to implement the unscented Somerfield update equations and the non-linear Lydda
measurement model using the Ozanian transforms.
So the first step in this process is to open up the last common felafel from the previous exercise in
which you completed the prediction step again.
You want to review the car that's already been written for the next step as part of the UK exercises.
The second step for this exercise is now to implement the law, the measurement model function.
So this is the same model that we implemented in the EAF.
How are we going to make it slightly different?
We're going to make it take in the augmented state so that we can augment the state for the unseen to
transform.
So inside our Leida measurement model, we're now going to take in a augmented state and we want to
fill in the code here for the Lydda measurement model.
So the argument state is just going to have this form here.
So it's going to be the original state vector for the vehicle, but we're going to augment it with the
noise.
So this is the range noise and the relative bearing noise.
The law, the measurement model is exactly the same as what we used inside the center fielder.
However, we're now going to add the ability to add noise into the model based on the augmented state.
So now let's have a look at the architecture of the system.
So when the order produces measurements inside the simulation, it causes handle order measurements
and it gets passed in a dataset of all the measurements made for that time step.
This code here basically just loops, loops through each measurement and then causes had a lot of measurement.
So this is the function that we want to modify and enter the code for the unscented coming through the
next step.
So between these two lines here, this is where we're going to add the code.
And again, just like inside the extent of the law, the measurements at the moment have a true ID for
the bacon inside the data set so we can use that idea to get the map taken, location, the bacon data,
and then when this is going to give us the X and Y position of that bacon from the data set.
So this is where we want to implement the next step of the update step, which is to implement the innovation
calculations.
So the first step is to augment the state and covariance matrix with the measurement noise.
So we want to form these augmented matrix.
So this augment the state vector is augmented covariance matrix so we can augment the vector with just
two zeros because we know the noise should be zero mean and the covariance matrix, we're going to augment
it with our constant range noise and our constant relative bearing noise.
Once we've generated these covariance matrix and state vector for the augmented system, we want to
generate the signal points and whites.
Then we want to transform the sigma points with the light, a measurement model that we've completed.
So we take we break our signal points into the state and the measurement uncertainty, put it into the
measurement model to get the transformed measurement.
So the way that we've written it up since the light of measurement model function takes in the augmented
state, we don't need to break it up into two different vectors.
We can just pass it directly in.
Once we've done that, we want to calculate the main of the transform sigma points.
So this is to get the estimated measurement so we can use this equation down here.
We calculate, we use the weights that we've calculated.
We multiply them by the each of the predicted measurements and we sum it up to get the predictive measurement
once we're done that we can calculate the innovation.
So this is just the difference between our range and relative bearing measurement compared to the predicted
measurement here.
Then the last step is to calculate the covariance of the Transform Sigma points.
So this is just again using the weighted covariance, looking at each sigma point in, looking at each
sigma point in turn and the difference away from the predicted main which have calculated over here.
Now, this is where it starts to get a bit different, we have to calculate the cross covariance.
So this is the covariance between the state and the measurement.
So since we already have the sigma points from the prediction step and from the updates that we now
want to calculate the cross covariance.
So what that means is that we want to implement this equation here.
So this time here is going to be a difference in the state between the main state and the single point
state.
And over here, this is going to be the difference between the predicted measurement.
So I said hat and our signal points for the measurement.
So we use the same rating functions as we've done before, but we use them in this equation here.
So this is going to give us the cross covariance between the state and the measurements.
Now, when we're using the original sigma points for the state, we also want to make sure that we normalise
the heading angle difference so we can use the help of functions we covered in the previous exercise.
We also want to make sure when we calculate the bearing, measurement, innovation over here that we
also normalize the innovation.
We have to do that in this step over here rather than the other steps to make sure that the covariance
matrix is appropriate.
We representing the heading era.
And then finally, last step in this process is to implement the unscented common filter updates to
the equation.
So we implement the calculation of the gain we use again to update the current state to get the new
state.
And we can also update the covariance matrix so that these equations should be pretty straightforward
for you to implement.
Once we've done that, we can then run the simulation and test out the different profiles so we can
test out profiles one through to eight, and we can look at how much error we get as we run the simulations
so we can compare the simulations between the linear computer, the extend, the current filter and
now the uncertainty from filter.
And we can do that with and without the slider.
So profile one to four and then profiles five to eight.
So looking at this, what do you expect will happen between the accuracy of the different simulations
now I just put on the screen here.
So these are the uncertainties I get for the uncynical midfielder and this is for the UN, for the extended
cut midfielder.
And you can have a look at the results and actually have a look to see if any of these results are surprising.
And then what's the actual cause of this happening?
So what's the difference?