In this video, we're going to go over a few hints to help you out with the challenges inside the capstone
project, and the first one is to deal with the non-zero initial conditions.
So the first sort of hints that I can give you is that we want to use the GPS position measurements
to initialize the X and Y position states within, then want to use a GPS position and LEIDA measurements
to landmarks when they're available to estimate the heading of the vehicle.
Now, once we have these two pieces of information, we can make an initial estimate of the velocity
and jarra bias so we can ever assume them to be zero or we could use some other process to estimate
them.
Once we have these three bits of information, we can combine the outputs of the three processes to
generate the full initial condition for the filter and then start running the common filter.
Now, to deal with the gyroscope sense of bias, what we can do is a gyroscope by state to the state
vector and vehicle model.
And what that looks like in terms of the mathematics is that we end up with a vehicle process model
that looks like this.
So the top section is just the normal process model.
However, we've now added the JavaScript bias states to the filter, so we have a constant bias with
zero derivative.
So you can see that the bias term for the current timestep is just a call to the bottom of the previous
timestep.
However, inside the JavaScript model, when we are calculating the rate of the vehicle, we're also
subtracting this bias term here.
So this is how we get the bias term inside the vehicle dynamics next to deal with the faulty GPS measurements
that we're getting.
What we want to do is not fuse any faulty GPS measurements so we can first evaluate each measurement,
such as using our innovation checking process, and then we only fuse the measurements that pass the
test.
And more mathematically, concretely, what we can do is we can calculate the normalized innovation
squared.
So if we have the GPS innovation as the music to here and we have the covariance matrix for the innovation,
we can use this equation to calculate the Naess, the normalized innovation squared, and then we can
test the normalize innovation squared against a preset threshold.
And if this test pass, then we confuse the measurement into the filter.
If the NS is larger than that threshold, then it must be a 40 cents A measurement so we can just ignore
it and not fuse it into the filter.
And then lastly, to deal with a lot of data association problem, what we can do is we can use the
estimated position and maximum leida range to find all possible landmarks that could be in view.
So we have the complete set of landmarks map in the database.
And what we can do is we can use the estimated position from the filter and we can first then filter
out all landmarks, which could not be possibly in view.
And this allows us to get a smaller set of landmarks that could be in view.
And then we can do data processing on top of these landmarks.
The subset to work out what we're saying, we can use the current estimate, the position and heading
to best match up the landmarks to find the data associations.
So from this reduced set of landmarks, we can then search through all of them and see which ones match
up with the best heading estimate.
Now, if we don't know the current estimated headings, such as during initialization, then you might
need to find the best heading that fits the data.
So we might need another optimization process to find out what best fits the current situation for the
given position.
So hopefully all this gives you some hints about how you go about solving this capstone project and
the challenges that arise from dealing with this situation.