Tools: System control

Kalman Filter

Kalman Filter is widely used in sensor fusion and estimation topic.

In this project, I followed the MATLAB Tech Talks tutorials to implement Kalman Filter and EKF in the MATLAB to estimate the motion of a pendulum.

Code

The whole system is shown as below:

Block Diagram

If the pendulum swings at small angle and the input torque is zero, the system can be approximately linear. Also, there are noise exist in both processing and measurement.

Measurement

We could implement a Kalman filter using the model and the measurement input, to estimate the pendulum motion in a less noisy way.

Kalman Filter

However, as the initial angle of the pendulum increases to 55°, the linear Kalman Filter would start to lose accuracy.

Nonlinear

To address the nonlinearity in the system, we could use Extended-Kalman Filter to linearize the system in place.

EKF Wave

Demux 1 is the actual response, Demux 2 is the raw measured data (tampered by noise), Demux 3 is the estimate of the linear Kalman Filter, and Demux 4 is the result from the Extended-Kalman Filter.

Let’s take a closer look.

EKF Wave

The green estimate from the EKF is closer to the actual response (Yellow) than the measurement (Blue) does. Well done!

In the tutorial, we also tested the effect of filter’s initial estimate and the filter’s noise estimate. After the tutorial, I have a better understanding toward Kalman Filter, including the pros and cons of Kalman Filter, EKF, and Unscented Kalman filter.