In this video, we’ll use a simple pendulum example to demonstrate
how you can use a Kalman filter to estimate the pendulum’s angular position in Simulink.
In this system, we’re interested in observing the angular position theta.
Let’s assume that we can measure this angle with a 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 a Kalman filter.
Before we switch to Simulink, let’s look at the free body diagram and 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’s a nonlinear term associated with the angular position.
If we 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 write it in this form.
Next, we can represent this 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 this script.
As we mentioned before, we’re interested in estimating theta through a Kalman filter because the measurement of theta is noisy.
You can use either MATLAB or Simulink to implement a linear time-invariant or time-varying Kalman filter.
Here, for estimating the angular position of the pendulum, we’ll use Simulink.
We start by dragging this prebuilt pendulum block into our model.
This is created using Simscape Multibody which lets us define a mechanical system using components
such as bodies, joints, force elements and sensors.
It then solves for the equations of motion for the mechanical system.
Here, we modeled the pendulum by connecting a fixed pivot with a link through a revolute joint.
We can also look at these in the mechanics explorer.
In these blocks, 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 this Simulink model from the link in this video’s description if you want to look at the inside of the blocks.
As we’ve 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’ll assume that the process noise acts only on the angular acceleration.
The covariance of the process noise was defined in the script.
So, we enter this value into our block.
We set the initial conditions for the states by double-clicking the pendulum block.
We’ll start the pendulum at 10 degrees, which corresponds to pi/18 radians.
And we’ll initiate it at zero angular velocity.
The pendulum block’s 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 we’ll simulate using this noise block.
Similar to what we’ve done with the process noise, we’re defining the noise characteristics using the covariance R.
Now, we have the actual and measured values of theta.
Next, we want to estimate theta using the Kalman filter. We add the Kalman filter block from Control System Toolbox to our model.
This block computes the Kalman filter algorithm equations, which we’ve discussed in the previous videos.
It uses the system model and the measurement and solves for the Kalman gain by minimizing the error covariance P,
and outputs the optimal state estimate. For our example, we’ll continuous-time estimation.
But note that if you want to run the estimation on a microprocessor in real-time, then you can use discrete-time Kalman filter,
generate C/C++ code for this block and deploy it to your hardware.
This block takes the system model, which we’ve defined in the script, along with the input to it and it also uses the measurement.
In the pendulum model, we used an initial condition of pi/18 radians for the angular position.
Here, we’ll assume that we know it only approximately and enter a slightly different number to investigate how the Kalman 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 2x2 matrix.
But since we’re assuming that the process noise acts on the angular acceleration only, we enter the associated term with it 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 0.
Also note that you can specify the covariance matrices as time-varying by unchecking the box next to them.
If unchecked, the block adds an additional input for the time-varying covariance.
Next, we’ll run the script and the Simulink model and look at the estimated theta through the Kalman filter.
One thing to note is that the pendulum model used in this block includes the nonlinear term sin(theta), but the Kalman filter uses the
linearized system. However, we still expect the Kalman filter to work well because we initialized theta with a small value, so the pendulum model
almost acts linearly. 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 noisy signal that is shown in orange is the measurement. And estimated theta is shown in blue.
This estimate shows us that the Kalman filter filters out the noise. And although its initial conditions slightly differ from the model itself,
we see that the Kalman filter converges in approximately 5 seconds and provides a good estimate of the pendulum’s angular position.
In this video, we’ve simulated a pendulum system and estimated its angular position using a Kalman 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. In this simulation, we initialized theta with a small value of 10 degrees and observed a good
estimate through Kalman filter. What if we use a larger value of theta?
If we now increase the initial value of theta to 90 degrees both in the pendulum model and Kalman filter,
and then simulate the model, we get a poor estimation of theta.
This is because for larger values of theta, the pendulum model acts nonlinearly and a Kalman 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 Simulink.