In this video, we’ll demonstrate how to use an extended Kalman filter in Simulink.
Previously, we’ve used a simple pendulum system and assumed that the pendulum’s angular position, theta, is measured with a
sensor. In order to filter out the noisy measurement, we used a Kalman filter and showed that it provides a good estimate when
the initial condition of theta is small. However, when we increased the value of theta to 90 degrees, we couldn’t estimate accurately,
because the pendulum model acts nonlinearly for larger values of theta. So, in order to estimate theta of this nonlinear system, we will
use an extended Kalman filter. We’ll start by adding the Extended Kalman Filter block from Control System Toolbox to our
model. Remember that in the previous videos we’ve discussed that state transition and measurement functions might be nonlinear, and
how the extended Kalman filter algorithm uses linear approximations to these nonlinear functions to compute the state estimates. Now,
in the Extended Kalman Filter block, we will specify these functions. Here, we’ll use MATLAB functions to create f and g, but note
that you can also use Simulink function blocks. The state transition function uses the current state and the input to determine how the states
of the next time step will be calculated. Here, any input to the state transition function other than the states and process noise is connected
through an input port. So, we connect u as the first input to the block. We first discretize this signal using a zero-order hold as the Extended
Kalman Filter block estimates states of a discrete nonlinear system. In our example, we won’t specify the Jacobian,
as it will be computed numerically. But if an analytical Jacobian is available, you can create a function to calculate it and enter the name of
that function here. Next, we enter the process noise covariance, which we’ve previously defined in a script. We also enter the initial values of the
states. In the pendulum model, theta is initialized with 90 degrees and 0 velocity. We use the same values in the Extended Kalman
filter block. Since we know the initial state values, we’ll set the initial state estimation error covariance to a small value. But in case you
don’t have confidence in the initial state values, you can specify a high value for the covariance.Next, we specify the measurement function,
again using a MATLAB function. Then we connect the measured theta corresponding to the measurement function that we’ve just
specified in the block. And we enter the measurement covariance whose value has been previously defined in the script. Remember that
a Kalman filter is also referred to as a sensor fusion algorithm, since it combines measurements from multiple sensors to reduce
the uncertainty in the state estimate. So, if you have multiple measurements, you can enter them by clicking “Add Measurement.” And
different sensors may have different sampling rates, so you can specify the sample times of measurements and the state transition under
the “Multirate” tab. And lastly, we enter the sample time. Now that we specified the parameters for the
Extended Kalman Filter block, we can run this simulation and look at the results. Here, the blue signal shows the actual theta, the noisy
one shows the measurement, and the green line is the estimated theta. This result shows us that, although the pendulum system is
nonlinear, using an extended Kalman filter gives us a good estimate of theta. However, note that if you have more severe nonlinearities in your
system, the extended Kalman filter might not be sufficient to handle the nonlinearity in the system. But in this case, you can use an
unscented Kalman filter, which you can find in Control System Toolbox.In this simulation, we showed how to estimate
states of a nonlinear system using the extended Kalman filter. If you’re interested in running state estimation on your hardware in real time, you
can generate code from this block and deploy it to your hardware. Remember that Kalman filter algorithm consists of two steps: the prediction
and the correction. If you look at the generated code, you’ll recognize functions that take care of the calculation of these algorithm steps. This
code snippet for example shows how the correction is calculated. Here, we conclude our discussion on Kalman
filters. In this series, we’ve discussed why you would use Kalman filters, what they are, how you can handle state estimation in nonlinear
systems, and we also showed examples in Simulink. We’ll continue doing Controls Tech Talks about different controls topics. In the
meantime, feel free to leave any comments and share your suggestions about the future topics.