So let's have a look at the calculation of an optimistic example.
So continue on our innovation example using the lead measurement model and without without 2D nonlinear
vehicle model, we have the following data that we've calculated in us here in the series of previous
videos we have that the current state, we have the current covariance, we have our measurements.
So this is our predicted measurement from a measurement model.
And let's assume that we have a sense measurement that gives these values here.
So this is what we have from the sensor.
This is what we have from our prediction.
We have the innovation, which is just going to be a difference between the two and we have our innovation
covariance such we've calculated before.
And lastly, we have our Jacobins.
So after we've collected all this information, we can go through and actually apply the comerio to
apply ISTEP equations.
So the first step in the process is to calculate a common field of game metrics K.
And again, this is fairly straightforward now that we have all the metrics.
So we have our previous covariance matrix p showing here.
We have the Giacomini metrics.
So Hech metrics here and we have our metrics as shown here.
So to calculate K, we just have our covariance times, the transpose of our Jacobean metrics multiplied
by the inverse of our innovation covariance.
And if we go through do the operations mathematically expanding at the magic series and performing the
inverse, we end up with this comment for the game matrix here.
So if we have a look at what this is saying, it means that this top term, this top line of here corresponds
to how much we can update the velocity.
This is how much we can update the heading information.
This is how much we can update the exposition.
And this is how much we can update the Y position, given errors in the based on the innovation.
So when we actually do the innovation update step here, we can say that we have the current state this
hour, minus the innovation and our common field again matrix so we can go through and we can multiply
the innovation by a common game.
So this is going to calculate how much we need to update our best estimate vector.
So when we expand this out, we end up with our updated vector so we can see that from the common gain
that we haven't updated.
I've lost the arrow because there's no information about the velocity, but we can see that we've come
up with a better estimate for the heading now because we've had we had a bit of hitting error in the
we had a bit of relative bearing error from the measurement.
So we've now updated our heading estimate for the vehicle.
You can also say that we've slightly updated our position in the X and position in the wall in the Y
because we had a bit of measurement error for the range.
So this is how we calculate the updated state estimate.
We also have to update the covariance time.
So looking at this equation here, all we have to do is calculate the scaling term and multiply it by
our previous covariance to get the updated covariance.
So basically we multiply our common gain by how GIACOBBI and subtract the identity matrix of the same
size of it.
And that becomes the scaling time and again.
We have the Matrix's down here.
So basically all we do is we fill in the different matrices and we end up with this expanded equation
with the old covariance matrix on the end, and then we multiply it out and then we get the new updated
covariance matrix.
So you can actually see now we haven't had any improvement in velocity because we haven't actually had
any information there.
But you can see now heading estimate, it's only a bit smaller.
We can see that our position variances are also smaller as well.
So we've improved the quality of our estimates of the state vector.