Welcome to the first exercise for the extent and filter in this exercise, we want to implement the
extended common field of prediction step equations and the nonlinear 2D vehicle process model.
So the first step in this fall is the open the C++ follow the common field step.
We want to review the code that has already been written as part of this exercise.
Then we want to compile and run the simulation as is, and we want to use the motion profile.
Number three, we can see that the car starts at the origin and then the car performs a series of turns,
all while maintaining the current speed of five meters a second.
We can also note that the GPS measurement model and the state initialization is copied from the linear
computer as the filter equations and logic remain the same between the linear common filter and the
extended column for the steps.
So the filter will start to run, but is using GPS only and it's without a process model.
Now, at the top of the file, you should be able to see a number of these constants and these are some
constants you can play around with and you can modify and use them in the code to help you out in your
programming.
We can also see that inside the handle, GPS measurement or the GPS measurement code is already done.
And this is exactly the same as a code that's been used inside the linear coming filter.
We can do this because the GPS model is a linear model.
Therefore we don't need any changes to implement it inside the extended common field on.
We can also have a look at the end of this function, and we also see that the state initialization
is already done, so the state initialization, as happens on the first measurement.
And if we have a look at the state and the covariance that gets initialized, we can see that the state
vector gets initialized with the X and Y locations directly from the of measurement.
And we initialize the heading and velocity to be zero.
We can look at the covariance matrix and we use the covariance from the GPS measurements for the precision
and we set an initial heading variance and initial velocity variance just to be large so that we have
some just to be large as we have no idea about what the true state of these parameters are going to
be directly at the initialization because we don't have this information available.
We should also note that in this example, the code uses the state vector.
In this filing form, it has a position in the X, in a position in the Y, then the heading, then
the velocity.
So is it important to note that all the code uses this vector definition and this order of the state
parameters?
The second step is to start to implement the prediction step.
So this is where we put the process model into the code.
So what we want to do is we want to modify the prediction step function inside the code so you can see
this block of code here.
This is where we want to implement the prediction step equations.
So we want to implement the process model, which is shown here.
So this is just the two vehicle process model that we have covered before.
And we can use some of these inputs from the function so we can see that we have the gyroscope measurement
here and the data parameters inside this function.
So we want to put them into this motion model here.
So our data here is this parameter here, the Delta team, and this is going to be our Jarawa.
You're right.
So we can reference that inside the code as Jarro cyborg.
So this is the you're right of the vehicle.
Now, we also want to set the initial sorry.
We also want to set the acceleration at each time set to be zero.
And we're just going to assume that this is a random variable from a normal distribution zero mean and
a variance of our sigma squared acceleration parameter.
So just like in the linear computer, we are going to use our unknown acceleration.
So we're going to increase this inside.
So we're going to include this term inside the process, model noise.
Now, once we do the prediction step, we also want to make sure we normalize the hitting angles or
any angles inside the filter to be plus or minus pi.
So plus or minus 180 degrees.
So if we were to just to integrate this plainly using this equation here, outside value here might
eventually get larger than, what, 180 degrees or smaller than negative hundreds of degrees if we keep
turning in the same direction.
But we always want to normalize the setting output, A.J. And to do this, we can use this equation
here.
We can use the angle function.
So this basically wraps an angle here to within this range and returns the result here.
So this line of code here is pretty much what you're going to need to do for the heading angle integration.
The next step is to implement the Keverian step for the British step, so to do this, we need to calculate
the Jicarilla matrix, the F matrix for the process model.
We want to define the key metrics.
So this is the process model noise matrix for the JavaScript noise and the acceleration noise.
So this is where we assume that we don't know the acceleration, but we assume it's a random variable.
So this is going to be a sigma squared acceleration.
And we also want to implement the extended field covariance prediction step so that you can your metrics
that we want to implement is going to be this set of equations here.
So we've covered this in the measurement model.
We also want to implement the key metrics for the process model noise.
And we're not going to be adding any noise for the X and Y position.
We're going to be adding the gyroscope sensor noise.
So this is this term here.
So this is going to implement the the noise from the uncertainty inside our gyroscope measurements.
And then we want to implement our acceleration, uncertainty.
This allows our process, model, acceleration and velocity to change as we run.
And lastly, we want to implement the common fieldworker variance prediction set.
So this set of equations here.
Once we've done that, then we can run the simulation and we can run it with the profiles one to four,
so we can run the simulation and check that everything's working and we can check out how accurate our
simulation is with profiles one before so we can check out the root mean squared error that we get for
the whole simulations.
So we can can quickly compare the linnear Cominco, the solution and the extended calm and feel the
solution that we've just done, so now we basically have an extended common and a version of the linnear
common filter.
All we have done is just changed the process model to be a non-linear model.
So we should be able to compare the results with the with compared to the extent Mattilda, they're
using the same inputs except for the extended coming up, which is now including the nonlinear model
and gyroscope measurements.
So we can see if this improves the situation at all.
So we should end up with a few numbers that look fairly close to this and we should be able to see that
the hopefully the extended common midfielder is performing very well.
So now that you've implemented the predictions that play around with the code and make sure everything's
working as expected, is this filter performing better than the Lynnae Common Filter is are expected
to be?
So is it more robust?
Can you think of a better way to nationalize the state, i.e. how to initialize velocity and the heading
without assuming them to be zero or unknown or known constant?
So this is going to be one problem with initializing it from a single GPS measurement.
All we can do is just measure the position, but we can't infer any information about the velocity of
the heading.
Can you think about any ways or techniques that you can overcome this problem or you could improve on
the situation?