In this video, we're going to look at the extent come and filter prediction step and how we actually
go about deriving the equations that we use to implement the STIP.
So we have seen that the state sector and covariance matrix can be predicted using the following equations,
we have the state vector prediction equation and we have the covariance matrix prediction equations.
But where do these equations actually come from?
So we see we have seen that they're very similar to the linear computer, but are they still applicable
for the extended computer?
So we have to go through and actually draw these equations to actually prove that they're still applicable
for the situation.
So first off, we're going to start to look at a Taylor series expansion, a Taylor series expansion
is one way of approximating a non-linear system with a linear system.
So taking the Taylor series, expansion of the state process model as shown here, and we're going to
take the Taylor series about the true state with zero process model noise.
We get this equation here.
So we take the state transition function here and we make a Taylor series approximation about this parameter
here by the state vector by substituting in our best prediction state, expanding it out, we get this
equation here.
So this is pretty much saying the Taylor series expansion is the state process model evaluated at this
position, plus the Jacobean matrix multiplied by the difference in positions between the true state
and the estimated state, plus the Jakobshavn for the noise multiplied by noise.
And then we're going to get all these higher extra order terms.
Now, this equation can be limited to the first or the terms only, so get rid of all these higher order
terms and we only get left with the first order times shown here.
So these terms here, which are repeated down here, this is an approximation of what the true state
is at times.
OK, given the information at the best estimate, TimeStep K minus one and the true state a K minus
one.
So this is going to be a key identity that we use later on.
So now if we look at the extended filter state predictions step, we can use a definition of the a prior
estimate.
So the April, I estimate, is basically saying we're getting the expected value of the true state at
time k given all the information up to came on this one.
And of course, we do not like this.
So we have the negatives emoted, noting that this is the a priori because it's very easy information
up to the previous timestep is four times HepC and it's the best estimate because it's hot.
So it's the estimate of the state, Victor.
We can substitute in an approximation for České, so the true state at times said, okay, we can use
our Taylor series expansion, substituting it into this equation here to get this following equation.
So what we have here is that we have taken the expectation of writer on side our first order Taylor
series expansion.
And we can have a look at this time here.
And we know by definition this term is the previous a posterior estimate of the state vector.
So if we substitute this in here, since this is also in the expectation of Rita and this is the expectation
operator as well, we know that we can change this value here just to be our true state, a K minus
one.
So when we do that, we can say we have X K minus one, minus K minus one, which means that this whole
term here pretty much gets canceled out.
We can also have a look at this time over here, so we're looking at this time here we have our noize,
Victor Omega, but since we're inside an expectation of radar, we also know the identity, the expected
noise in the system is going to be zero because as we take the because we know that the noise should
have zero remain.
So if we take the expectation of radar on the noise, it should be zero.
So this time here and this time here, both cancel out.
So what we get left with is obvious implication.
We get the extended coming field of state prediction, which is basically saying that the state estimate
for Tompsett.
K is pretty much just a state transition function for the previous best estimate predicted forward using
the current control and using zero Maine for the noise.
So this is what we expected it to be.
So this should be fairly straightforward, but we've proved that this is valid using the Taylor series
expansion for the extended Matura.
So that's the prediction step.
So now we can look at the covariance prediction step.
So, again, let's have a look at the definition of the state prediction era, the state prediction
error is basically the true state minus the estimated state, and that's going to give us a clearer
state.
So we can substitute in our Taylor series approximation for XQ that's we've developed before, and we
can also now substitute in for our predicted using the common filter prediction step.
So basically, when we do that, we end up with this equation here.
So we have our Taylor series expansion, which is this first term here, subtracted it from our prediction
estimate, which is this term here.
This is the state transition process model.
So when we look at this, what we can say is we can say that this term here and this term here, they
both cancel themselves out.
And we just left with this term over here.
So this is going to be our time for our state prediction error relationship.
So our predicted state era is just going to be the driving times, the predicted error from last time,
plus the Jacobean tons of noise.
We can also use the definition of the AIPA covariance.
So this covariance matrix here is looking at the amount of error around our predicted state so we can
substitute in our equation for our estimation error that we worked out from the last slide into here.
We can see this time here and this time here.
Both these are both the estimation error.
So we can substitute this term in here for both of them and we end up with this equation here.
Now, on closer inspection, we can say a few things, this here is pretty much the estimation error
squared and we already know what that is.
This term is just going to be the covariance matrix for the previous timestep as given this by this
equation here.
We also can look at these terms over here.
So this is the process, the noise squared and what's inside the expectation of radar.
So, again, this is just going to be the variance of the noise, which we already know is key matrix.
So we can substitute in our various matrix here for the process model for the previous step and our
noise covariance matrix here.
And we end up with the common field of covariance prediction equation, which is shown here.
So this equation is very similar to the linear computer, is that now we're using the Jacobean matrix
instead of the F and L matrixes.
And basically what this is doing, this is taking a first order approximation of the error in the system.
And this is perfectly valid because we prove that it is valid when we taking a Taylor series expansion
around the state estimate.
However, this is, of course, going to have a few caveats that come with it when we take the Taylor
series expansion, we're doing a first order linear approximation on the process model.
So if we get outside the linear region or if the linear regression point is too far away from linear
region, then this equation starts to break down.
But for reasonably linear systems, this equation is perfectly valid.
So in summary, we can get the predicted state estimate by using the process model here, putting in
our previous state to get our new state, putting in our current control and put it in zero noise.
We can also calculate a predicted covariance estimate, which is pretty much propagating the previous
covariance, using the Jacobins for the process model and taking account the extra noise which are induced
by the process model noise.
So basically taking our key matrix and transforming it, using the process model Jacobins with respect
to noise, transforming it into our state's space x so we basically transform it both into the state
vector system frame.
So this is where the assumptions get baked into the extent of your equations, we've used a Taylor series
expansion about a linear ization point, about the best estimate.
And because we've done that, it makes some assumptions.
It assumes that we're always operating with a good estimate of the system that's close to the truth.
And it is assuming that the system can be fairly well represented by this Taylor series expansion with
only including the first order terms.