How to Use an Extended Kalman Filter in Simulink | Understanding Kalman Filters, Part 7

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.