Now that we worked out the linear approximation of the system that we want to estimate, we can use
this linear approximation along with the linear filter to estimate the dynamic states of the pendulum
is in measurements of the angle theta.
The common field of code will be exactly the same as the code that we've used in the past.
But all we have to do is change the state vector, the process model and the measurement model.
So in general, to apply the linear common filter to your estimation problem, you need to decide on
the state vector X the process model F the system process uncertainty, matrix skew measurement, model
H and the measurement uncertainty, covariance matrix are the rest of the calculations are basically
the same for any estimation problem.
So here is our continuous linear system of the form X equals X plus B you with the state's theta and
theta dot, we will again assume that we have zero torque or zero input again, just like we did with
the 2D track, for example, and will be doing this just because we are only interested in the free
response to the system or unforced response of the system.
Now we need to turn this continuous system into this great form, and we're going to do this by calculating
the matrix, which we can do using the Matrix exponential relationship that we've derived in the past.
We will also assume that we have a random unknown amount of input or talk given by this random parameter
W.
And this random input is only going to be affecting the theater, not state the size of this uncertainty,
a process more noise will be controlled by the parameter sigma squared tau so we can set the process
model noise, covariance matrix due to simply be this value here of our sigma squared value.
So every time we do a prediction step, we add an extra uncertainty to the angular velocity state.
This will cause the uncertainty in the system to grow.
Every time we do a prediction that this increased uncertainty can help the system work in cases when
we have bad approximations or approximations that aren't entirely correct, the measurement model,
again, is a very simple model.
We will simply take measurements of the angular position data and assume that there's some additive
noise v where they is a random variable of a zero main Gaussian distribution with variance sigma squared
theta.
So our measurement model is simply a selection matrix for the first theta.
And our measurement model covariance matrix is simply the variance sigma squared theta.
We will be doing a simulation exercise like we did last time, your exercise will be to go through and
implement the equations as explained on the previous slides.
And once you've done that, that's all you need to do to have implemented a common filter for the pendulum
dynamic system.
So in the code for this assignment, there is the initialise function for the Kamasutra or you have
to do is to go out and fill in the values for the F key Hajnal Matrixes and put them into our initialize
function as shown here.
So if we switch over to the code.
You can see the function that we have to modify right here.
So this is the assignment to underscore filtered up by code.
So this is the code that we need to modify for you to have implemented the common filter for the Pentagon
system, the rest of the field.
So it's such as their prediction step and the update step of the code is pretty much exactly the same
as the carry that we use for the previous filters.
The only thing we need to change is the system matrixes for the filter itself.
So the simulation options for this assignment have already been set up correctly for this exercise.
So the simulation has a timestep or or one.
We run it for 10 seconds.
We have a measurement.
Right.
So how often do we make measurements of theta at 10 hertz?
Each of these measurements is going to have a random noise associated with it and it's going to be all
of our standard deviation of point one degrees.
We're always going to start at a same angle, which is going to be a small angle inside a linear range.
And of course, we're going to draw the plots and do the animation.
So the common filter, when we're running it, we're going to have a pretty small noise talk of point
zero one.
They're going to make sure our measurement standard deviation for the matrix is going to be the same
as what we've actually used inside the simulation.
So point one degrees.
We're also going to initialize the filter on our first measurement.
So we're not going to start the filter off at zero zero.
We're going to take the first theta measurement and then we're going to set that to be our current theta
state and we're going to set the velocity to be zero.
But I have an uncertainty of about 10 degrees per second.
So once this is done, we can run that and then we can see the results.
So switching over to the answer, when we run this, you should see this result.
We should see the pendulum moving back and forth and you can see that the system is tracking the truth
very well.
So the red one is the estimate, the blue one underneath is this system truth?
If we have a look at our measurement innovations, you can see that our standard deviation, our standard
deviation for our measurement, covariance, innovation, covariance, sorry, is about our point,
too.
We can see down here the opposition to the measurements.
Innovation is point to seven.
So they're very closely aligned and so they should be fairly well trained.
If we have a look at our angles here, we can see the estimated theta along with the tree theta and
we can see the same thing for our velocity.
You can see that we start the initial condition of zero from the first measurement here.
And you can see that we initialize our velocity to be zero because we don't have a measurement at that
time.
But you can see after one or two measurements, it pretty much converges to what the truth is.
We can see as we run the simulation, we estimate the ferocity of the theta quite well, even though
we're only getting position measurements for Theta.
If we have a look at a balance here, we we can see that the theta position error is within a three
sigma balance.
So it's fairly well contained.
And the same thing for our theta have lost the error.
It's well within our three sigma bounds here.
So once you've finished implementing your vision of the common filter, you should pretty much get the
exact same results as Shane saying on the screen here.
And once you've done that, then we can move on to the next video.
Will we talk about turning the filter and the performance of the filter?