In this video, we're going to have a look at the initial conditions for the Linnear and Fulda, so
we're going to see different ways of setting up this initial conditions and what the responses is going
to be.
And then we're going to look at you doing an exercise, setting up the initial conditions, using the
first measurement, rather than assuming initial state for the simulation that we've been working on.
So you should have seen from the simulations that the best performance obtained, which means the minimum
remains squared error for the whole profile is when the initial conditions being the state and the covariance
are as close to the truth as possible.
And this makes sense from the simulations that you've already run.
You should have seen that when we start the initial state below zero.
So zero for the origin and zero for the velocity of the coming for us to perform fairly well on the
first profile.
So if we run the first profile, so you can see that the estimate starts at the origin and which is
where the car starts as well.
And you can see that the further quickly converges down to track the true position of the car.
However, when we run with the second profile for the same configurations, the car does not start at
the origin anymore.
However, the filter still starts at the origin.
And you can see it takes quite a while for the filter to try to catch up with the true position.
This is because the field starts very inconsistently.
The state estimates thinks is at zero, whereas the car is not at zero.
So it takes a long time for the uncertainty to grow such large enough that the filter can shift its
position and catch up with the true state of the system.
And it can also say that once we run this profile, we end up with a very large maze with error for
the whole profile.
And this is because the filter did not start consistently.
The initial state of the system was nowhere near the true state of the system.
However, this raises a question, how do we actually know what the initial conditions are when the
system is actually initialized from a non-zero value is an example of this is when we use profile to
inside the simulator and the car starts at a different position rather than the origin.
So how do you know what the initial conditions are going to be in events?
And obviously in most conditions, we don't know what the initial conditions are going to be in events.
So then we're going to have two options.
We're either going to have to assume zero or some other concern like we've done already, or we're going
to wait until we get the first measurement or we know enough about the current state to actually start
the initialization process.
And this is what we're going to do in this exercise.
Now, luckily, as we've seen, the linnear common filter has nice property that it will not diverge
even if the air is too large.
So if we start with really large era because we have a non-zero initial condition, the update step
will always be updating to minimize the estimation error.
The filter is not going to start to diverge.
The air is always going to be decreasing.
So inside this exercise, we want to implement the filter state and covariance initialization on the
first measurement.
So to this first, we want to open up, become a filter file from the previous exercise in which we've
had the prediction and the update steps completed.
Now we want to run the simulation with profile number two.
So this is a profile of the non-zero initial conditions and you should see some issues.
When we actually run this simulation profile, you should see that the initial step and covariance are
inconsistent.
So the true state is not within the uncertainty of the filter estimate.
So the red circle and if we look at the estimation error for the whole profile, you can see that it's
quite large compared to the other profiles.
So what you want to do is that we want to change the way the filter gets initialized instead of initializing
on the first prediction, as we've done for the last two examples, using a constant state and covariance
set by these parameters here, we are going to change this flag up here to be false.
So the filter is no longer going to initialize on.
The first prediction is going to wait until it gets initialized in the update step.
So inside the handle GPS measurement function, there's going to be a bit of suction down here.
And this is where we're going to write the initialization for the state vector and covariance matrix.
And we're going to use the current GPS measurement to do this.
So what we're doing in the past, we've been setting the initial state to bring all zeros, always said
it to be five meters per second at 45 degrees because we actually knew and what the profile was going
to be.
And we've set the covariance for the initial state just to be a diagonal matrix with the first two terms
being an initial position uncertainty and then the last two terms being initial velocity, uncertainty.
And we've set these large enough.
That encapsulates what the truth is going to be.
Another way is to instead of initializing at time equals zero.
We want to initialize on the first update and this first update is going to be the GPS position measurement.
So basically we want to do is we want to set Vector X, not to be the GPS measurement.
So this is the position in the X and Y.
And then since we don't have any information about the velocity, all we're going to do the same thing.
We're going to assume it starts off at zero, but we're going to set a initial velocity variance such
that this is going to be large enough that it will actually encapsulate what the true velocity is.
And then for the position states and the position uncertainty, we're just going to set the uncertainty
to be the R matrix from the GPS measurement.
So we want to change the variable in it on first prediction to be false.
So this is going to not initialize the state vector and it's going to wait until we get the first measurement.
Then inside the measurement function for the GPS, we want to initialize the field of state and covariance
on the first update, and we want to do it according to these equations here.
So we want to set the position states to be the GPS measurements.
We will assume zero velocity and then we're going to set the position uncertainty inside the covariance
to be the measurement, uncertainty and then aid and uncertainty for the velocity, which is going to
be based on the constant again, which allows us to set a large variance for velocity and let the field
converge on the true velocity.
Once we've done that, then we can rerun the simulation with profile too, and we should be able to
notice.
And the response and overall the whole profile should have a smaller profile error compared to if we
didn't actually use this delayed initialization and we just initialize it from a zero condition.