In this video, we're going to look at the first exercise that you have to complete for the linnear
common.
And this is to implement the prediction step of the common filter.
So this step is going to involve implementing the linnear common filter prediction equations and the
2D vehicle process model.
So the first step in the process is to open up the common filter zip file when we compile this and run
the simulation as is using the default profile, which is profile one, we can see that the car starts
at the origin.
So P p y equals zero zero and it moves at a 45 degree angle at five meters per second.
So if we break it down into the V, X and Voila components is going to give us this equation here.
So this is going to be the first profile that we're looking at for implementing the step equations and
the 2D process model.
The second step in the process is to set up the initial state and the covariance for the Linnear come
and fill it up.
So we are going to initially assume that the position is zero zero and the initial velocity is five
meters a second with the heading of 45 degrees.
We also can assume that the zero uncertainty initially.
So we're going to use a zero covariance matrix.
So to do this inside our function prediction step inside the common filter between these two lines here,
we want to start to enter the code over here for setting up the initial condition for the filter.
So to do this, what we're going to do is we're going to set up the initial state vector X being zero
zero three three point five three point five.
So inside our step vector, we have the form of the state's X Y for position and then X Y for velocity.
We also want to set up the initial covariance matrix, the P matrix, just to be an identity.
So just to be a zero matrix as shown here.
The next step in the exercise is to implement the prediction step.
This is the process model for the linear common filter.
So to do this, we're going to use the time step and define the process, model, matrix according to
the to the legal process model that we've covered in previous video.
We're also going to use the linear model equation to update the state, and we're going to do this inside
the function block as shown here.
So inside our prediction step function, there's a function down here that has this code down here.
And we want to fill in the details over here.
So the metrics that we want to implement is shown here, as we've seen before, where data is going
to be the time stamp and then the linear model equation is just simply our F X.
So F is a process.
Model X is all previous state to calculate the new state.
And you can see that we have no other input here.
So we don't have a B you.
All we're doing here is just assuming that we know the precise model.
There's zero input and then we're going to use the noise of the system, the uncertainty of the system
to update the acceleration and allow for a change in velocity.
The fourth step is then going to be to implement the prediction step for the covariance.
And so to do this, we're going to define the key metrics as a function of the variable acceleration,
standard deviation.
So this is a global concern that you can use.
And then we're going to define the metrics as a function of the timestep, as we've covered in the previous
video.
And then we want to implement the covariance prediction set equations.
We're going to assume that the process, model noise, acceleration, standard deviation is initially
zero.
So we're going to set constant value, this acceleration, standard deviation to be zero so that we
can carry out the verification without any extra noise in the system so we can understand what exactly
is going on.
So again, inside our prediction step block of code down here, we want to implement the key metrics.
So our key metrics is just going to be this metric is going to be a two by two metrics with the acceleration
noise index and acceleration noise in the Y.
And for this example, we can assume that both of these are just going to be equal to this constant
acceleration, standard deviation.
We're going to implement the EL metrics.
And again, this is just going to be based on the TimeStep data so we can define this matrix here.
And then lastly, we can implement the prediction set for the covariance, which is shown in this equation
here.
So once we've implemented the state prediction and covariance prediction, we can then go about verifying
the work that we've actually done.
So the next step is to run the simulation and we want to check that the diction follows the truth fairly
closely.
And this is going to work because we've initialized the filter with the truth, position and velocity.
And then we can see what happens if we use profiles two to four.
So profiler's no one is going to should track fairly well for two to four.
We have changing conditions so we can actually see what happens.
So on profile one, you can see that our estimate is state the red line, the Red Cross being the current
state, is going to follow the car very nicely.
We're going to get a very small position error and frostier because we've started the simulation from
the true condition and we have zero input.
The car is just traveling at a constant speed with no other noise inside the system.
So it should track the truth very well.
So this shows that the system will be working and the prediction equations are working well, the next
step is to check the position.
Covariance prediction is working.
And to do this, we want to set the initial position for the X and Y covariance to be basically at,
say, five meters squared.
We also want to run the simulation and see that a three sigma position uncertainty stays at approximately
plus or minus 15 meters.
And we can compare this with the grid size because that grid size is at twenty five meters.
So what this means is that when we run it, the simulation, we can see that the uncertainty circle,
the covariance here is going to stay at a constant size and it's going to be staying centered on the
vehicle.
Once you check the position covariance, we can look at the velocity covariance and make sure that the
prediction step is working correctly to do this.
Again, we want to set the initial state to be all zero so that the estimate stays at the origin.
Now we want to set the initial position covariance to be zero, but we want to set the initial velocity
X and Y covariance to be five, divided by three squared meters per second.
So what this is going to do is going to allow us when we run the simulation, we're going to see that
the three sigma error for position grows at the same rate as the car position changes.
So we don't run the simulation.
You can see down here the origin, we run the simulation and the car drives off at forty five degrees
at five minutes.
And second, we should see that the three sigma uncertainty basically moves at or expands at the same
rate as a car moves.
So it should stay fairly close to the car seat of origin as it expands.
And this is showing us that we can correctly model.
I've lost the uncertainty inside the system and see that it actually tracks what the true system is
doing as the position changes with a constant velocity.
The last step in the verification process is to verify that the acceleration covariance prediction is
working.
So we want to set the initial state back to the original value.
We also want to set the initial covariance or to be zero.
But now we want to set the process model acceleration to constant.
So the acceleration standard deviation to be point one.
Now, when we run the simulation, we can see that three sigma velocity, the uncertainty grows over
time and the position uncertainty is going to grow quite radically with time.
So we can see as the car moves out, uncertainty is going to start small.
But as we as time goes on, is going to grow even larger and it's going to start growing exponentially
larger as we as time goes on.