In this video, we're going to try to develop a deeper understanding and some insights into how the
extended karma filter works and how different things affect it, and in particular, we will have a
deeper look into trying to understand the effects of the linear approximation of the nonlinear uncertainty,
transformation.
And we will also have a look at how the large initial conditions or large estimation errors affect a
solution with linear ization effects.
Of the key assumptions that Linnear come, I feel to operate with is that the uncertainty in the system
can be represented as a Gaussian distribution made up of a mean and covariance.
This allows the linear transformation of the probability distribution to be used as the linear transformation
of Gaussian distribution is just another Gaussian distribution.
So what that means is that if we have a random variable vector X that is made from a normal distribution
of a main export and covariance Sigma X, then we apply the linear transformation X plus B is equal
to Y, then the random variable vector Y is just going to have A, it's just going to be a normal distribution
with these parameters.
The main is just going to be a linear transformation of the main of X and then the covariance is just
going to be the transform variance into the wireframe using this transformation here.
And we have seen this and prove this mathematically before.
However, let's actually have a look at a numerical simulation example.
So let's say we have a random variable X that is a two dimensional Gaussian distribution with Amane
of exper and covariance Sigma X and we sample it 100 times.
So these are the 100 samples of this normal distribution here.
We can apply the linear transformation.
Y is equal to X, plus be you, where A is the transformation vector and B is the offset vector.
And we end up with this distribution of Y samples here.
Now we can also plot the covariance of our examples, which is just plotting the ellipse for our three
sigma value of these parameters here.
And then we can use this covariance and we can transform it using our linear transformation, using
this equation here that we've shown on the previous slide.
And we can plot the answer on the Ellipse in green here.
And we can see that the green, the linear transformation of the blue eyes only to the green on.
So you can see that it fits the data very well.
So this is the assumption that the linear homophily uses when we have a linear model.
Now, we can confirm this is true by actually sampling and measuring the covariance of the Y data points
to get a Y bar and sigma y.
And what this means is that we just take all these data points, the green data point, and we can calculate
these parameters numerically using our equations for the main.
So we just sum up all the values divided by the number of samples to get the main.
And can we also work out the covariance by using this equation here as well?
So if we do this and get these two parameters here and then we got the covariance matrix, we get this
red Dasht covariance ellipsoid here and these two, the green one and the red one are going to converge
if we had more sample points.
But you can see even with only 100 points, the covariance ellipses fit quite well with each other.
So this shows that the linear transformation, this operation here is a very good well, it is a it
is the actual answer for the uncertainty, transformation into the frame.
So we know that the extended coming through allows us to operate on non-linear systems and it uses a
linear approximation of the nonlinear system.
So let's actually have a look at how acceptable that is in a numerical example such as this.
So in this case, we're going to change it up a bit.
We're going to have a nonlinear function for basically a range and bearing.
So we have a range and a bearing theta and we want to take the parameters in this space is a range and
bearing and convert it into contingent coordinates, X and Y.
So you can see we have a range and bearing value.
We put it through this transformation here and give X and Y and we plot it.
So a range is this parameter here.
A bearing is going to be this one.
We take a parameter here and we transform it into the X and Y frame.
So these 100 data points here are sampled from a distribution shown here.
So we have a normal distribution for range of a main one hundred and a variance of five squared.
And we also have the bearing distribution as being another normal distribution with a main value of
forty five degrees and a variance of twenty five degrees squared.
So one hundred point sampled from this, these two distributions he plotted in the X and Y plane gives
us all these green points here.
So these are the transform results of the nonlinear function here.
Now we can look at what happens if we apply the extended karma filter approximation of the uncertainty
transformation's.
So this is where we calculate the jakovčić matrixes.
So Alef Matrix education and multiply the covariance in the original frame to get the covariance in
the final frame.
The Jacobean matrix from the measurement model that we had on the previous slide is this matrix shown
here.
And our covariance matrix for the range in bearing is just going to be the variance parameters for the
range and the variance parameters for a bearing.
So if we take these two parameters here, do this operation, find out the uncertainty inside the X
and Y frame and plot that we get the green Lipps there and we can see that the green ellipse is not
the best approximation of this distribution for all these green points.
So this is what the extent Kamasutra uses.
However, if we actually calculate the if we actually calculate the mean and covariance from all these
data points numerically like we did in past, we get this measured uncertainty ellipse here.
And you can see that this is a well, this is the true uncertainty inside this data set here.
And this distribution is where the mean and covariance a sample numerically and measured from the data
directly by doing the domain of Rasor and the expectation of radar to get the main and the covariance.
So we can see in this case here the linear approximation of the nonlinear function for the uncertainty,
which the extent of Kamasutra uses is not always the best fit.
So there's going to be cases where the uncertainty inside the extended Kamasutra is not the best approximation
of the true uncertainty inside the system.
We can actually have a look at the green ellipse here is pretty valid for when we're near the main.
So over here you can see that if it's this so we can see the distribution inside this area here, if
it's very well.
But as we get away from all initialization point, it breaks down in terms of how good approximation
it is.
And we can see that by carrying out the same experiment again.
But this time we're going to change our uncertainty inside Alberini measurement to be five degrees.
So from twenty five to five degrees.
So now we can say that everything sort of stays in the linear range of the system and we can see that
our Jacobean approximation and our measured approximation of the uncertainty inside the system are pretty
are a lot more correct, a lot more similar.
So I can say that this becomes a better approximation.
So the smaller the uncertainty is and closer to the linear ascension point, the better the approximation
is going to be.
So we can say that the Ukrainian transformation is a linear approximation of the non-linear transform,
and it works quite well if the system is approximately linear at the winterization point.
We can also see that large uncertainties can lead to more linear causation error effects depending on
the measurement model.
We can see that the linear approximation is most accurate in the linear ization point.
And we're going to go into more detail in the second half of this video on the effects of linear ization.
The linear approximation of uncertainty that uses the Jacobean tends to underestimate the error in the
system.
And this means it leads to overconfident estimates so that the error, uncertainty inside the extended
computer usually underestimates how much uncertainty in the real system it has.
And if when we're operating the extended field, there's a large mismatch in the true uncertainty of
the system and what it thinks it is, then this can actually lead to a degraded performance and the
field can actually start to diverge.
Now we can have a look at what happens when we have large estimation errors.
So let's have an example where our state vector is just going to be a single scalar and it's just going
to be parameter X.
We also have a measurement model which is linear and it looks like this.
So we have a Y equals to X and it's just going to be a linear and it's just going to be a linear function
as shown here.
Let's assume that our true state is given by this value here.
So this is a true value is and at this true value, this is what the measurement is going to be.
So we can have a Y is equal to X where X is the true value.
So this is what the sensor or measurement information is going to get.
However, let's assume that we have an estimated value for our current best estimate of X to be this
parameter here.
So we can say we have a large estimation error.
So when we calculate what our think, what we think the measurement should be, it will be this primarily.
So this is going to be our predicted measurement for the current state.
So when we evaluate our recovery metrics for our best estimate now, it's going to be the Caribbean
at this point here and we're going to see that the Caribbean is going to be positive.
So that's pretty much saying that the slope of this line here, the measurement function is going to
be positive.
So it's going to be in this direction, increasing this way.
So when we use a positive Jacobean and we calculate the common gain metrics and then we do this operation
here.
So this is the innovation between our measurement.
So that's going to be this value over here.
So this is going to be measurement made about the true state and we're going to subtract, subtract
off the predicted measurement.
So this is what we think the measurements are going to be about.
Our estimate is that if we do this innovation, multiply it by our common gain.
So this whole term here is going to be negative.
It's going to be smaller than zero.
So when we do the common update step, we know that the updated state estimate, our X plus is going
to be smaller than our current best estimate, X minus, because this term here is smaller than zero.
So the updated state is going to be over here somewhere.
So we can say this is where our updated estimates are going to be.
We look at the error inside the system, so the difference between the true position and the estimated
position, we can see after we do the update, this amount of error here has decreased compared to the
true statements of previous best estimate.
So our estimation error after we do the update is smaller than the estimation error before the update.
So this leads to a smaller estimation error as we do the update, which is what we want to do.
This is the whole purpose of the common filter.
So we can see that when we do this on a linear system, we have a nice response.
The UPD predicted the update.
The state moves towards the true state.
So now let's have a look at this example.
But instead of using a linear measurement model, we're going to use a nonlinear measurement model that
now looks like this.
So even though our true state and estimates are the same at and then our measurement and our predicted
measurement are exactly the same, we have a different describing metrics.
So now our describing at this point is going to be negative because we have the slope decreasing over
here at this linear regression point.
So now let me do the common update multiplied by the innovation.
This whole time here is going to be greater than zero.
So it's going to be positive, which means that the value of the optimal state is going to be greater
than the previous best estimate.
So that means that our estimate is going to move this way in this direction here.
And this means that we've actually increased the amount of error.
So the error between the true state and estimate of state before the update, this amount here and after
the update is this amount over here.
So the estimation error after the update is actually larger than the estimation error before the update.
So we have a larger estimation error.
So this means that failure is actually diverging.
The more we run it, the worse the estimate is going to get.
And we don't want this.
This is where the system becomes unstable.
So this is the effect of large estimation errors on a nonlinear system.
We can get to a state where the linear approximation does not hold is no longer valid for the point
that we are evaluating the system about, and it means the filter will not converge to the truth.
So this is one of the problems with the extended common filter.
We always want the estimated state to be true close to the true state, which we don't know.
So the filter operates correctly.
So large estimation errors can occur when we have wrong or incomplete initial conditions, such as if
the vehicle is initialized to be traveling at zero degrees heading, but is really traveling at 180
degrees heading.
We've seen that this can cause the field to diverge.
Large estimation errors can also occur when we get bad faulty data or measurements that get fused into
the system.
It can drive the estimated state away from the true state, which then which then in turn causes the
field to diverge.
So this is why we confuse bad data or measurements into the filter is not robust.
Large estimation errors can also happen when we miss data so we get a lack of information for a long
period of time.
The filter can actually start to drift.
And if it drifts too far away from the true state, then these large linear ization effects can happen
actually cause a filter to diverge.
We can also get large estimation errors by bad moral assumptions, so if the model that we model inside
the system does not represent the true model of the system, estimating this can cause large estimation
errors as well.
So large estimation errors can cause the extent to camouflage the verge when the linear approximation
assumptions are invalid for the current condition and we can't tell that this is happening.
To be able to tell that this is happening, we would need to know what the true state of the system
really is to calculate the estimation error, to see whether it's increased after the update.
So this is why we need careful tuning and simulation of the extent field to make sure that it works.
So we need to test all the assumptions and operating points to make sure that the filter stays stable
and it's giving us good results.
Now, we know that inertial navigation systems use nonlinear equations and are typically implemented
using the extended common filter.
But in general it works because the nonlinear navigation equations can be approximated quite well with
linear approximations if we have some idea about what the current state is.