in this video we'll use a simple
pendulum example to demonstrate how you
can use a common filter to estimate the
pendulums angular position in sync in
this system were interested in observing
the angular position theta let's assume
that we can measure this angle with the
sensor such as a rotary potentiometer
although we can measure theta our
measurements will be noisy in order to
filter out the noise and get a good
estimate of theta we're going to use
common filter before we switch to
similar ink let's look at the free body
diagram an equation of motion of the
pendulum for the pendulum model we will
assume zero friction imagine that the
pendulum is connected to a rigid rod and
we initiate it at an angle and when we
let it go it starts to swing back and
forth
note that there is a non-linear term
associated with the angular position if
you look at the plot of the sine
function we see that for small values of
theta this function almost acts linearly
therefore for small angles we can
linearize this equation and writer than
this one
next we can represent the system in
state space form where we choose the
first state as angular position and the
second one as angular velocity we define
the parameters and state space model in
the scripts as we mentioned before we're
interested in estimating theta for a
common filter because the measurement of
theta is non Z you can use either MATLAB
or symlink to implement a linear
time-invariant or time varying common
filter here for estimating the English
position of the pendulum we will use
Simulink we start by dragging this
pre-built pendulum block into our model
this is created using sim scape
multi-body
which lets us define a mechanical system
using components such as bodies joints
force elements and sensors it does hold
for the equations of motion for the
mechanical system here we modeled the
pen
by connecting a fixed pivot with a link
through airavata joint we can also look
at these in the mechanics Explorer in
these logs we define dimensions mass and
inertial properties of the bodies the
driving torque acting on the joint is
the sum of these two signals and we
output the joint position theta through
the revolute joint block feel free to
download the sibling model from the link
in this video's description if you want
to look at the inside of the box as we
have just seen this block takes two
inputs the first input is the applied
torque which we will set to zero the
second input is the process noise in
this example we assume that process
noise acts only on the angular
acceleration the covariance of the
process noise was defining the script so
we enter this value into our block we
set the initial conditions for the
states by double-clicking the pendulum
block will start to pendulum at 10
degrees which corresponds to PI over 18
radians and will initiate it at 0
angular velocity the pendulum blocks
output is the angular position which is
assumed to be measured with a sensor
we'll expect the sensor readings to have
some noise which will simulate using
this nice block similar to what we've
done with the process noise we're
defining the noise characteristics using
the covariance are now we have actual
and measured values of theta next we
want to estimate theta using the common
filter we have two common filter block
from control system toolbox to our model
this block computes the common filter
algorithm equations which we'll discuss
in the previous videos it uses the
system model and the measurement and so
for the Kalman gain by minimizing the
error covariance P and outputs the opt
most state estimates for our example we
will use continuous time estimation but
note that if you want to run the
estimation on a microprocessor in real
time then you can use the screen time
Kalman filter generate C C++ code for
this block and deploy it to your
hardware this block takes a system model
which will define an script along with
the input to it and it also uses the
measurement in the pendulum model we use
an initial condition of PI over 18
radians for the angular position here
we'll assume that we know with only
approximately and interests like the
different number to investigate how the
comma filter estimate will converge when
the initial conditions are not known
exactly other parameters that we need to
configure are noise covariance matrices
which we choose to be the same as in the
pendulum system note that the pendulum
system has two states so the process
noise covariance is a two by two matrix
but since we're assuming that the
process noise acts on the angular
acceleration only we entered associated
terminate in the covariance matrix as
zero next we enter the measurement
covariance the cross covariance matrix n
is used to capture the correlation
between the process and measurement
noise in our example they are
uncorrelated so n is zero also note that
you can specify the covariance matrices
as time varying by unchecking the box
next to them if uncheck the block as an
additional input for the time area
covariance next we run the script and
the Similan model and look at this
method theta through the common filter
one thing to notice that the pendulum
model used in this block includes the
nonlinear term sine of theta
but the Kalman filter uses the
linearized system however we still
expect the comma
to work well because we initialize theta
with a small values so the pendulum
model almost XD nearly let's look at the
simulation results we assumed perfect
knowledge of the pendulum model whose
output the actual theta is shown with
the green line the noise a signal that
is shown in orange is the measurement
and estimated theta is shown in blue
this estimate shows us that Kalman
filter filters out the noise and
although its initial conditions like a
differ from the model itself we see that
the Kalman filter converges in
approximately five seconds and provides
a good estimate of the pendulums angular
position in this video always simulated
a pendulum system then estimated its
angular position using a common filter
we assumed perfect knowledge of the
system model and process and noise
covariance matrices Q and R but in case
you don't know the system parameters or
the sensor characteristics exactly then
this simulation gives you an opportunity
to play with your model parameters as
well as noise covariance matrices and
observe and improve your state
estimation installation we initialize
theta with a small value of 10 degrees
and observe the good estimate to come
officer what if you use a larger value
of theta if we now increase the initial
value of theta to 90 degrees both in the
pendulum model and common filter and
then simulate the model we get a poor
estimation of theta this is because for
larger values of theta the pendulum
model x nonlinearly and a common filter
is only defined for linear systems as we
discussed in the previous video this
problem can be addressed by using an
extended Kalman filter in the next video
we will use the same pendulum model and
demonstrate how you can use an extended
Kalman filter in sync