In this video, we're going to look at calculating the Lider measurement model innovation, so continuing
on with our vehicle example, we have the measurement model that looks like this.
So this is the measurement model for a lot of like sensor that produces range and relative bearing measurements
to a known landmark position.
We can find the measurement prediction al-Said hat by substituting in our current state estimate our
hat and assuming zero measurement noise, so we assume that our noise vector V is just going to be zero.
So these terms over here cancel out.
So we just get left with this measurement model here.
So let's have a look at how we actually go about calculating the innovation and our sense of measurement
for the slide measurement model.
First off is that we have our current state, so we going to assume that we have a current state that
looks like this.
So we have a diversity of 2.5 meters a second.
We have a heading of point seven radians exposition of 10 meters and a Y position of negative five meters.
We also have the landmark location, so let's assume that we can observe landmark number one.
So the landmark location that we know is going to be at 12 meters for the exposition and at two meters
for the Y position.
So we can take these numbers here for the current state and the landmark position, substitute them
into our Lider measurement model and we end up with this set of equations here.
So basically, wherever we have our L, X and Y, we substitute in our values for the landmark locations.
So we have a wall here for Elex and we have a two here for a L y.
So when we substitute in our current state, we end up with these equations here and we just go through
and do the calculations, so expanding them out and we end up with these numbers here.
So this is the current predictive measurement for landmark one using the current state best estimate.
So this is what we predict that the leader should be showing when we actually do the measurement.
Now, the next step into calculating the innovation is that we need to find the innovation covariance,
and to do this, we need the Jacobean matrixes.
Now to start off, we need the a.e matrix of the sensor model.
So this is our little model in this example.
And we want to get its partial derivatives with respect to the Vector X and we want to evaluate it at
our best estimate.
We also want to calculate that you came in for the measurement model with respect to the measurement
noise out of our vector here.
So this is just the derivative of these functions up here with respect to all our noise inputs or our
vector, which is these components here.
So to start off, let's have a look at the measurement model state Jacobean.
So this one over here, we can calculate this Jacobean by taking the partial derivatives of these two
functions here, one for a range measurement, one for our relative bearing information.
We can take the derivatives of these two parameters with respect to the state vector parameters.
So our velocity heading position X and Y, and when we do that, we end up with this set of equations
here.
So if you remember, we had a state vector being our velocity.
So you can see that the velocity does not come into any of these equations up here.
So for the range or the relative bearing, so we get zero zero.
So there's no effect on velocity.
Now, the next day it was all heading and we can see the this top equation here headings not in it.
So we should get zero for the derivative, which we do.
But down here for a relative bearing, we have a negative side so we can see we're going to have a derivative
of negative one when we take the derivative of this whole function with respect to SA so we can see
we get a negative one.
We can do the same thing for our position in the exercise if we differentiate our arrangement with respect
to our position, yes, we end up with this term here.
So this is just a difference in the position divided by our total distance, which is just given by
this equation here.
So this so these components are just a partial derivatives of these two functions with respect to the
position derivatives.
So we're going to put some numbers to these examples here.
So given our current state showing here and our current landmark given by his and our previous predictive
measurement, which we calculated a few years ago, to give these numbers here, we can now evaluate
our Jacobean for this time period.
So what we want to do is we basically want to fill in our parameters here for our current state and
landmark into this matrix here and just evaluate what it is.
So when we do that, we can substitute in the numbers.
So here we have PEX and we know that 10 we have minus Elix.
We know that Alex is 12.
So we get ten minus 12 divided by our distance.
So this range measurement here, which is just going to be our seven point two eight.
So it's divided by seven point to it.
And we just do that for all the components and we end up with these numbers here when we actually evaluate
it out.
So this is the measurement model describing with respect to the state vector.
Now we also want to find the measurement model noise Jacobean.
So this is coming up here and this is just a partial derivatives of our sense of measurement model.
So these parameters here with respect to the input, noise measurement noise are very vector, which
in this model is over here.
And we don't have to do anything fancy for this because we can see that this is just additive noise.
So all we end up with is just an identity matrix.
So this is just showing that we have additive noise for this model.
So we just expect this to be an identity matrix.
So if we calculate the predictive measurement, we also want to calculate our innovation covariance.
So let's assume that we have a current covariance matrix that looks like this.
So we have an uncertainty of ten point one and two two.
So we just have assumed that we have a diagonal matrix for the current covariance.
We also assume that our sensor is going to have this uncertainty here.
So we assume that we have a convergence of 10 and negative one.
So 10 for the range measurements and three point one for the heading estimate or relative bearing estimate.
The Jacobins that we've calculated are these ones here, and we what we want to do is want to evaluate
the innovation covariance, which is given by this certainly equations here.
So knowing all these elements here is pretty straightforward.
What we have to do, we just have to substitute in the different matrices into this equation and then
expand it out and evaluate it.
So if we just substitute in the matrixes, we end up with this set of equations.
So you can say this term here is how do you tribunal for the with respect to the state vector, this
matrix here we have our current covariance matrix.
So this matrix here into here and again, we have our Jacobean matrix transpose.
So that's just this matrix here.
And we transpose it here.
And again, this term here is going to be the sensor noise covariance.
So we know that these two matrices here are just going to be identity matrix, as we've calculated that
before.
So identity and identity transpose, which is just another identity matrix.
And we have our measurement noise covariance matrix R, which is just this matrix here.
So expanding it all out, doing all the matrix multiplications, we end up with this matrix for The
Matrix.
So this is going to be our innovation covariance.
So this is how much error we expect on our innovation.
So this was a fairly straightforward example of how we actually go about calculating the predictive
measurement and the innovation covariance for the measurement model that we've been showing.
So this is for using our vehicle process model of state vector, having vissi positioning position in
the way and with all the measurement model producing range and relative bearing measurement information.
So there's nothing too fancy, nothing complicated going on.
It's just a matter of understanding equations and filling in the relative calculations and then just
going through and doing the calculations.