We are now going to cover it, the first exercise you have to do for the unscented, come and fill it
up, and that is to implement the prediction step.
So as part of this exercise, we want to implement the unscented in common field of prediction and the
2D non-linear vehicle process model.
So the first step is to open up the C++ for Common Filter Seipp, and we want to review the code that
already been written as part of the student Ukhov exercise.
We want to compile and run the simulation as is, and we can test it with profile three, we should
be able to see that the cost starts at the Argin.
And then the car performs a series of turns while maintaining the current speed of five meters per second.
We should also note that the GPS measurement model and the state initialization is already copied from
the linear common filtering exercises as the filter equation.
So these steps and the logic is all exactly the same.
So the filter in this example will start to run, but it will only be using GPS and it will be running
without a process model until we've implemented it inside the step throughout the exercise.
There's a few helper functions that you can use inside your code, and this is to normalize the state
and normalize the line of measurements.
So any angular values inside the state or the measurement, we can put them in and wrap them to you
plus or minus pi.
So this is going to be handy when they want to normalize the state or normalize the light relative bearing
measurements.
We can also have a look at the top of the file and you'll find that there's a number of constants just
as before that you can use.
So I would encourage you to use these constants as they give you a single point of access to changes
constants rather than having to change your code later on.
And as we said before, the GPS management model, the linear model is already being done because it's
the same as linear algebra.
So if you look at the code, this has already been filled in, so you don't have to do it for you.
And then a very similar thing for the state initialization.
This part of the code is already being done.
So on the first GPS measurement, you'll take the X and Y position forward into the state.
It will assume that the other velocity components are all going to be zero, is going to set the velocity
uncertainty to be this initial constant defined in the type of file.
And it's going to be setting the position uncertainty just be the uncertainty of the GPS measurements.
So basically, just as we've done before for the extended common filter, it's just going to set up
the initial state and initial covariance as shown here.
We also just want to remind you that inside this example, the whole code assumes that the state vector
has a following form.
So it has a positions and then the heading and then the velocity is the position in the X Y and then
the heading of the vehicle and then the velocity of the vehicle.
So all the code here assumes that we have the state vector inside this form.
So the second step in the exercise is to implement the generate signal points and generate sigma weighting
functions.
And to do this we can use the Koskie decomposition inside the Reagan Library.
And to do that, all we have to do is just take the covariance matrix and use the dot lety function
on it to actually return the lower triangle decomposition.
So inside the function that we have generated signal points, we just want to implement these series
of equations here.
So we want to generate a vector of the sigma points where each of the segments follows these equations
here.
Now, since we're using a common filter, we're going to assume that the noise is relatively Gaussian.
So for our scaling time, kapa, we're just going to assume it's going to be equal to three minus NP,
where N is the size of the state vector.
So I just like in the unscented transform.
We're just going to be using these equations here to generate sigma weights inside this function.
We just want to generate these weights here based on these equations.
And again, we're going to assume that this cappa scaling function is just going to be this function
up here.
So it's going to be a function of the number of states inside the state vector.
The third step in the exercise is to implement the 2D vehicle process, motor function, and this is
going to be slightly different than the extended computer.
We're going to be using the same mathematical model.
However, we want to take in the augmented state vector and return the new state.
And we also do not want to normalize heading inside this vehicle process model because the discontinuities
when we do this will interfere with the results.
So we have this vehicle process model function that takes in an augmented state.
It takes in the current rotation rate and timestep so we can assume that this augmented state vector
is going to be looking like this.
So it's going to be the initial vector that we have for the system.
And then the last two terms in this vector are going to be the noise components or the random variable
components.
So it's going to be the noise for the rotation.
Right.
So this is going to be so basically the noise and the gyroscope and this is going to be the acceleration
noise.
So the actual state model that we want to implement is shown here.
So this is the process model for the system and is all exactly the same as Akef, except now we're going
to add on our noise component for the gyro onto the rotation.
Right.
And we're going to set the acceleration noise to be the noise turn here.
So we're going to take in the augmented state.
However, we're just going to return the new state, which is just the normal state vector.
So the main step then for the unscented coming for the prediction step is to implement the unsent and
transform prediction step.
So we're going to implement this inside the car method of prediction step equation.
So you're going to decode between these two lines here.
The first step in the process is to augment the state in the covariance matrix with the process model
noise.
So we're going to have we're going to generate to augment the state is going to consist of the current
state of the vehicle and then zero zero.
So this is going to be zero for the gyroscope noise and zero for the process model acceleration noise.
And we're going to do this because we're going to assume that the main value of the noise is going to
be zero.
We're going to augment the covariance matrix to find this augmented variance matrix.
And again, it's going to be the original covariance matrix.
And then on the diagonal times are going to put the jarra noise and then the acceleration noise.
So once we have the augmented vector and the augmented covariance vector, we're going to put these
into the generate sigma points functions.
So this is the function that we've just written on the previous slide.
And we're going to calculate the sigma points and the weights.
Once we've done that, then we want to transform all the signal points through the vehicle process model.
So this is why we needed the presence model to take in our augmented step vector so that we can take
we can directly put in the augmented single points in this vector.
Once we have the transformed signal points, we then calculate the main of the transform points.
So this is basically the step here.
We can use a weighting vector that we've calculated and I'll transform Sigma points and calculate the
main.
And this will give us the new main for the updated prediction step.
We can do the same thing for the covariance.
So we evaluate this component down here and this will give us the covariance for the predicted state.
And as we've come before with the unscented transform prediction step, since we've augmented the state
vector with the process model noise, we don't have to add the process model noise matrix onto this
covariance matrix here.
So by the time you've done that, you should have a working version that we can run the simulation with.
So if you compare it and run the simulation, you can check out how the simulation runs with profiles
one to four.
So that's all the profiles that don't include the line measurements.
We can then quickly compare the X solutions to the UK solutions for the error statistics.
Now that we some slight differences maybe between the results that you get and what I get.
However, you can see that results from the akef compared to the UK are going to be very similar for
most of profiles so artfully done.
This is a good idea to play around with code and make sure it's working as expected.
And you can sort of think about a few of these answers.
So is the field performing better than the AKF and is that to be expected?
You can also think about is the field a method easier or harder to implement compared to implementing
the EAF as what you've done before?
And then do you think the filter is computationally more or less expensive than the IQF?