In the last few videos, we looked at a few of the differently square solutions, we first started off
with normal a square solution which has the solution that looks like this, then we extended the solution
for our way to the least squares solution.
So this is what happens when we have a bit more information about the noise and the measurements.
We can include that in the estimation process and we end up with a solution that looks like this.
So I can see the difference here between the solutions that we've now included this time.
So this is the measurement uncertainty into the into the solution.
And since we actually know the uncertainty of the Y measurements due to the noise, we can then use
this equation here to work out the uncertainty in the estimates in X.
So this is the estimate, uncertainty or covariance matrix.
Next, what they're going to look at is called recursively squares.
So in the past, we've learned how to calculate a constant with a square solution.
We build up a model matrix from all the different measurements and we solve the equation.
This requires all the measurements to be available at time that we calculate the solution.
What happens if we don't?
What happens if we want to update the estimate as measurements become available?
Do we keep storing all the measurements and add new euros into the Matrix?
A better way is actually to use a recursive update every time a new measurement is available.
Doing this would mean that we don't have to keep a history of all the previous measurements and we would
only have to keep the best estimate from last time.
So there are huge advantages in.
This is also more applicable in situations where you always want an estimate.
So say you have a sense of measurement coming in at a very low rate, like once an hour, once a day.
You don't want to have to wait a few days to get an estimate.
You want to have an estimate every time.
So we can use recursively squares to do this.
So we're going to look at how we actually calculate this recursively square solution.
So let's have a look at the problem statement.
So first off, we have our measurement equation, which we've seen before.
So this is for a single measurement, not for all measurements, just a single one.
And again, we know properties of the noise vector here.
So we know that first it's going to have a certain variance given by R is going to be a zero Amane expected
value if you sum up all the different noise values from all the different measurements.
And we also know that each of the measurements is going to be each of the noise on the measurement is
going to be uncorrelated.
It's going to be independent.
So there's going to be no correlation.
So for recursive estimation to work, we first start off with a guess, initial guess or a good best
estimate of what our solution is going to be.
Then when we get a measurement, what we do is that we fuse it into the solution and we come up with
our next best estimate using this information here.
Once we got done that, then we get another measurement in and then we update our estimate again so
we recursively step forward the solution every time we get a new measurement.
So he just follows the same process.
Now, you can sort of visualize this as this process here, so we have an initial estimate of the state
estimate, we get information, we get the measurement information, we use it in to update the estimate
to the current time, to the current measurement.
And then this measurement here comes back to become the previous one.
And then we get a new measurement in.
So it just recursively goes around the circle.
The current estimate becomes the previous estimate.
Every time we get a new measurement, we update the estimate and we just keep going through this, so
forth.
Now, how we can actually write this in mathematical terms is that we can use a recursive estimation
equation.
Same here.
So here, this is going to be our best estimate for all the measurements up to and including the K measurement.
This is going to be a previous best estimate.
So this is going to be a best estimate without the current measurement used in and then we're going
to have this recursive term here.
So this term here is the error between the current measurement and what the predicted measurement should
be.
So this is the measurement model metrics.
This is the previous estimate.
So if we multiply these two together, we get a predicted estimate.
And the difference between this predicted estimate of the measurement and the actual true measurement
that we get is going to be an error term that we can feed back to work out how much we have to update
our best estimate from the previous times up to the current.
And we can do this by multiplying this amount of error through this game matrix here.
So this is going to be a correction game.
And this is Kresh, again, is the game that minimizes the square cost function for all the previous
solutions, for all the previous measurements that we've had.
So to actually work out the least squares solution, we have to come up with how we calculate this game
matrix.
Okay, because this is going to affect how much we need to update our previous estimate to get our current
estimate based on our current measurement.
So if we work out how to solve this equation or how to solve this case for it to be the solution to
these squares, then we can just using the information, using this equation here.
And every time we get a new measurement, we can put it in here, calculate an update and get a better
estimate here.
So the first part of the solution, we're going to look at how to ride out the because of estimation
error.
So this is the residual error for the current measurement and it's going to be the difference between
the true state X and our estimate at the current state EXC.
So we can substitute in our recursive solution into this time here to end up with this equation here.
We can also substitute in our mathematical model for our measurement work, and we end up with an equation
that looks like this.
So this can be rearranged into a final solution here, so we now we can say that our current estimation
error is a function of the previous estimation error, plus our current noise.
So this is going to be a recursive estimation of our estimation error.
Now, I can say this is an interesting term here.
This is the identity matrix time minus the game matrix times.
Now, if this was the one and this number was less than one, you can see that the estimation error
is always going to be less afterwards.
So if this is one term here, I mean, multiply it by number less than one, it means that this is going
to be smaller.
So this means that this sort of it shows that as we put more and more estimates into the solution,
ideally this should always be getting less.
They should be always be less estimation error after we do each measurement.
But this relies on the fact that work has to be set up.
Such a Katamatite is going to be less than one.
We also want to have a look at our recursive equation for the uncertainty so we know that the uncertainty
is just going to be the covariance matrix and it's going to be the covariance matrix of the measurement
residuals.
So using this equation here that we've seen before, we can substitute in this equation above so we
can substitute it in and expand it.
We end up with an equation that looks like this.
So this should be sort of a bit familiar.
You can see that we have a sort of matrix times and invariants times matrix again, but transposed and
you can see here.
So this is just how we can transpose the measurement, uncertainty or the covariance from one frame
to another.
So we have the initial covariance here and multiply it by some transition matrix and in that transition
matrix to transpose the uncertainty in a different frame or in after an operation.
And we can say here we do the same thing here.
So the Matrix pretty much maps the uncertainty in the measurement to the uncertainty in a state estimate.
Now we can look at the cost function that we want to minimize.
So the cost function that we're minimizing in recursive estimation is pretty much the we want to minimize
the residuals squared for all the different measurements up to and including K.
So this is a cost function for K.
Now the difference between the true state and the estimated state can be just be written out as the
measurement residuals squared.
And again, they should be looking familiar.
We can rewrite this into a vector equation of just measurement residual transposed time to measure residuals.
Now, this is also very similar.
This is almost the covariance matrix, but this is going to end up giving us a scalar.
But what we can do is that we can actually link this scale to be the trace of the covariance matrix.
So the convergence matrix is just the measurement residual transpose time to measure residuals.
So that's the opposite of this.
But the trace of this matrix is just going to be equal to the cost function.
Therefore, we know that minimizing the solution to solution uncertainty also minimizes the cost function.
So if we find a solution that minimizes this uncertainty matrix p we also finding a solution that minimizes
this cost function.
Define the minimum so that we can find a solution.
What we have to do is we have to minimize our OK, we have to minimize the solution uncertainty.
So we do this by minimizing cost function with respect.
OK, so this time, instead of working at our X, our estimate, we're using recursive estimation.
So now we have to work out the optimum game matrix that minimizes the solution so we differentiate our
cost function.
So if we work out Alpay matrix on the previous slide, we differentiate that with respect to K, we
end up with this solution here.
And in the past we know that we want to find the minimum point.
So we have to find a stationary point by setting the derivative to zero and then we guarantee that this
is a minimum since is only a single minimum.
So we set it to zero.
We rewrite the equation and we end up with this being the optimum cost function.
So this function here minimizes the cost function for the given K.
So this is how we can calculate K that we can put into a recursive solution that always minimizes the
squared error.
So to put that in practice, we have an initial guest of our state or estimate the thing that we want
to estimate and we know how accurate our estimate is.
So we have an initial covariance matrix.
So, you know, initial guess, we might not know these exactly like we might have no information about
what our initial estimate is.
So they might just started off with zero.
And if we do that, well, then we can say that we have no idea what our estimate is.
So we start off our covariance to be some really, really large number, which means we pretty much
have no faith in the current estimate.
But once we've set up our initial estimate of the state at our covariance of the estimate, we then
look at our recursively square solution.
So every time we get a new measurement, we can calculate this game matrix, okay?
We can use that game matrix inside of our updated equation.
So this is at least squares solution that takes in our previous estimate to calculate our new estimate.
And you can say it's a function of this game.
Matrix K is also a function of our measurement here.
So using the measurement information, we can update our current best estimate.
And then when we do that, we can update our solution uncertainty.
So this is a recursive really square solution.
We go through this process every time we get a new update.
And by doing this, we're always going to be getting the best update possible using all the information
up to and including that measurement.
And this happens because all the previous uncertainty and effects of each of those measurements is stored
in this covariance matrix here.
So this convergence matrix always keeps internally a history of all the measurements that have been
made before.