In the previous video, we looked at the extent to come a further update, step equations.
So in this video, we're going to have a closer look at how we derive them.
So we have seen that the state vector and the covariance matrix can be updated using these set of equations
here.
But where do these equations come from?
How do we know that these are valid for when we're using the filter for non-linear systems?
The first step in this process is to look at expanding the estimation era with the innovation correction
step.
So if you look at calculating the estimation error, the estimation error is Alex with the tilde symbol
above it.
And it's just going to be the difference between the true state and our estimated state.
I can substitute in our correction step for our estimation error by using the common four step.
So this is our update step here.
We have our previous best assessment, plus our common gane times the innovation and we can also substitute
in our definition for the innovation.
So in one of the previous videos, we calculated the Taylor series expansion for the innovation and
which is this time here.
So we look at the Jacobins of the measurement model and we look at probation's away from the true state.
So this is the area between the true state and the estimated state.
And we look at how the process and we look at how the measurement model affects the process here.
So taking this approximation for our innovation is shooting it into this equation here.
We end up with these equations and again, having a closer inspection of this.
We can say that this time here is the difference between the true state and the estimate, the state
as well as over here.
So these two terms here are just our estimation error.
So we can rewrite this equations in terms of only the estimation error and rearrange it to get it into
this form.
Here we have the current estimated era.
After we do, the update is going to be a function of the current estimation error before we do the
update, as well as the amount of noise in the system.
And then we have these two scaling matrices here.
So this pretty much gives us every kind of relationship of how do you describe how the error changes
before and after we do the update.
So using the definition of the posterior error covariance, which is given by this equation here.
So this is the error covariance after we do the update.
So it's like a variance matrix of the error for TimeStep K given all the information up to TimeStep,
K is just the expected value of our estimation.
Error squared.
So on the previous slide, we worked out the estimation error so we can go about just substituting it
in our value for the estimation error.
And we end up with this big set of equations here.
So we're using the expectation of right here, but we know that the expected value of any concern is
just going to be that content.
So you can see that we've put out a set of concerns here instead of constant fear.
And same thing here and here.
And we just left with the expected value and we just left with the expectation operator on these two
sets of parameters here.
Now, we know that this is just the expected value of the estimation, R-squared, which is just another
term for the covariance matrix.
So this is just the covariance matrix without the current measurement applied.
And this covariance matrix over here is just the measurement noise, covariance matrix.
So the matrix.
So we've come up with an equation that links the covariance matrix before the update to the covariance
matrix after the update.
And it's also going to be a function of the current uncertainty in the system from the measurement.
So where the magic happens is that we want to find the solution for the common for the game matrix k
that minimizes the trace of the updated covariance matrix P and then this will give us the optimum solution
to minimize the conditional mean squared estimation error.
So this is the whole process of the common filter.
So this is where the computer calculates the optimal value of the K matrix to provide the optimal solution.
The first step in this equation is to define a cost function and we're going to define the cost function
l to just be the trace of the covariance matrix.
So the covariance matrix, the equation that we worked out on the last slide, we can substituted into
this equation here, calculate the trace of it, and now we want to minimize the cost function.
And we do this by looking at the derivative of this cost function with respect to a common field again.
So this is basically going to calculate the common field again that minimizes this function here.
So to do that, we want to differentiate our cost function with respect to common field again.
And we end up with this equation here and define the minimum of this equation.
We want to set it equal to zero.
Once we do that, it's just a matter of rearranging the equation to work out what K is.
And when we do that, we end up with this common field or update function here that calculates the coming
for the game matrix.
So you can see that we have our inverse s and again, we know what this is, is just the innovation
covariance matrix.
So this is just our Jacobsen's times, our previous covariance transformation.
And then the same thing for the measurement model noise.
So this is where we get the comment for the game matrix from, so comment for the game Matrix K is the
optimum solution to minimize the trace of the covariance matrix, which minimizes the conditional mean
squared estimation error.
So now that we have the comment for the game, we can actually go through the update step.
So the solution for the update step equation is just this set of equations here.
So we have our updated state estimate from our previous estimate, plus the common field of game at
risk times in innovation.
So this is what we started with initially and this is the definition of the common thought, the game
matrix.
So this is how we calculate K.
Now, the solution for the covariance is also easily recovered.
So we have our covariance matrix update set.
So we have our covariance equation here.
We can substitute in our definition for the optimum common gain matrix K and we end up with this equation
here.
So basically we have a function of the previous covariance matrix minus this K as K transpose.
So this transforms the innovation covariance into the way it looks at how much we have to update the
covariance matrix by.
And if we substitute in our previous equation for S, we can actually rearrange this equation and times
cancel out and the left with this set.