Kalman Filter: Python Implementation

Table of Contents

In control systems and signal processing, Kalman filters are commonly used to estimate the internal state of a process in the presence of noise. This blog post shows how to use a Kalman Filter to estimate the angular velocity of a DC motor, using only a noisy position reading. Using Python as our programming language, we can implement a Kalman filter to smooth out the noise and track the motor’s true velocity over time. In a minute, we will go through the theory of Kalman filters and find out how to implement one in Python for a DC motor.

The Kalman Filter – Theory

A Kalman filter is an algorithm that provides estimates of unknown variables by combining measurements and system models. It was developed in the 1960s (it takes its name by Rudolf E. Kálmán, one of the main contributors) and has since been widely used in control systems, signal processing, and robotics.

The Kalman filter works by predicting the next state of a dynamic system based on its model, and then updating that prediction using real-time measurements. The key advantage of this filter is that it works optimally (in the minimum mean-square-error sense) for systems with noise or uncertainty in measurements and system dynamics.

Kalman Filter – Model

As mentioned before, the Kalman filter is one of the most important and common estimation approaches to estimate an unknown state of a dynamic system from a series of noisy measurements.

The linear Kalman Filter model assumes a linear dynamic system discretized:

$x_k = F x_{k-1} + B u_k + w_k$

$z_k = H x_{k} + v_k$

where

$w_k \sim \mathcal{N}(0, Q)$

$v_k \sim \mathcal{N}(0, R)$

are the process and measurement noise, both drawn from a zero mean Gaussian noise, with covariance respectively Q and R.

Kalman Filter – Implementation

The Kalman filter implementation calculates the predicted state and covariance estimate:

$\hat{x}_{k|k-1} = F x_{k-1|k-1} + B u_k$

$\hat{P}_{k|k-1} = F P_{k-1|k-1} F^T + Q$

the innovation and its covariance:

$\tilde{y}_k = z_k - H \hat{x}_{k|k-1}$

$S_k = H \hat{P}_{k|k-1} H^T + R$

the optimal Kalman gain:

$K_k =\hat{P}_{k|k-1} H^T S^{-1}_k$

and the updated state and covariance estimate:

$x_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y}_k$

$P_{k|k} = (I - K_k H) \hat{P}_{k|k-1}$

DC Motor

As mentioned before, our example aims to estimate the angular velocity of a DC Motor based on a noisy reading of its angular position.

The mechanical and electrical drawing of a DC electric motor is shown below.

The image describes the architecture of a DC Motor: a voltage generator (V) in series with a resistance (R), inductance (L), and EMF generator.

The torque moves the rotor which "sees" a damping of b*omega.

Differential Equations

The armature circuit has resistance (R) and inductance (L) characteristics.

The rotor, when in motion, generates a back Electro-Magnetic Force (EMF) $k_e \omega$ . Essentially a voltage that acts against the input voltage V.

The differential equation of the electrical part is:

$L \frac{di}{dt} + R i = V - k_e \omega$

The mechanical part, using Newton’s second law for rotational motion, is:

$J \dot{\omega} + b \omega = \tau, ~ \tau = k_t i$

where:

$J$ : rotational inertia, typically from the load (as the inertia of the motor tends to be negligible)
$b$ : damping factor
$k_t$ : torque sensitivity

To model in Python we have to isolate the highest-level derivatives:

$\dot{\omega} = \frac{k_t i - b \omega}{J}$

$\frac{di}{dt} = \frac{V - k_e \omega - R i}{L}$

Include angular position – we may be interested in position control in the future:

$\dot \theta = \omega$

Parameters

Because we want to use representative parameters, we refer to the datasheet of a real DC motor (C23-L33-W10) from Moog (https://www.moog.com/content/dam/moog/literature/MCG/moc23series.pdf) where we can derive our parameters:

Torque sensitivity ( $k_t$ ) = 0.0187 Nm/A
Back EMF ( $k_e$ ) = 0.0191 V/(rad/s)
Terminal resistance ( $R$ ) = 0.6 Ohm
Terminal inductance ( $L$ ) = 35 mH = 0.035 H
Damping factor ( $b$ ) = 0.001 Nm/KRPM = 0.0000095 Nm/(rad/s)

Assuming that we are spinning a disc of radius 5 cm and mass 0.1 kg, we have:

$J = 0.5 m r^2 = 0.000125 ~ kg m^2$

DC Motor Model in Python

The DC Motor discussed is implemented in Python using a class.

The Class DCMotor contains the motor’s parameters and state (velocity, position, and current):

Its __init__ function initialises parameters and state.
Its method Step computes an integration step.

Python - The class DCMotor, its __init__ and Step functions. Code can be found on the GitHub repository linked at the end of the post.

and this is where we initialize the DCMotor object:

Python - Initializing parameters and creating DCMotor object. Code can be found on the GitHub repository linked at the end of the post.

Kalman Filter for Angular Velocity Estimation

Model

Here we want to design a Kalman Filter without assuming any knowledge of the DC Motor physics.

Hence we assume a kinematic model.

We measure the angular position every Δt seconds, and we want to estimate its angular velocity.The position and velocity of the motor are described by the linear state space: $x_k = \left[ \begin{array}{ll} \theta \\ \omega \end{array} \right]$ , where $\theta$ : angular position and $\omega$ : angular velocity.

We assume that $\alpha_k$ (acceleration) is unknown and normally distributed with mean 0 and standard deviation $\sigma_\alpha.$ Then:

$x_k = F x_{k-1} + G \alpha_k$

where $F = \left[ \begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array} \right]$ and $G = \left[ \begin{array}{c} \frac{1}{2} \Delta t^2 \\ \Delta t \end{array} \right]$

Then going back to the nominal dynamic system:

$x_k = F x_{k-1} + B u_k + w_k$

where $B u_k = 0$ under the assumption of no known control inputs.

Therefore

$w_k \sim \mathcal{N}(0, Q)$

where

$Q = G G^T \sigma^2_{\alpha} = \left[ \begin{array}{cc} \frac{1}{4} \Delta t^4 & \frac{1}{2} \Delta t^3 \\ \frac{1}{2} \Delta t^3 & \Delta t^2\end{array} \right] \sigma^2_{\alpha}$

The noisy measurement of the angular position of the rotor is

$\theta_k = H x_k + v_k$

where $H = \left[ \begin{array}{cc} 1 & 0 \end{array} \right]$ . $v_k$ is normally distributed with mean 0 and standard deviation $\sigma_{\theta}$ . Hence $R = \sigma^2_{\theta}$ .

Important: the approach model agnostic proposed here (we don’t assume any knowledge on the DC motor) has pros and cons:

Pros: Simpler and independent of model uncertainties.
Cons: A model that knows what to expect in terms of state evolution would be more accurate. For example, we are not using the knowledge of V (which we have) in this approach.

Kalman Filter in Python

The Kalman Filter discussed is implemented in Python using a class.

The Class KalmanFilter contains the filter’s parameters and state (velocity, position, and error covariance):

Its __init__ function initialises parameters and state.
Its method Step executes a step of the Kalman Filter.

Python - The class KalmanFilter, its __init__ and Step functions. Code can be found on the GitHub repository linked at the end of the post.

and this is where we initialize the KalmanFilter object:

Python - Initializing parameters and creating KalmanFilter object. Code can be found on the GitHub repository linked at the end of the post.

Interesting to notice here that the sample time of the filter is 0.01 seconds (10 ms). We will see later that the simulation runs with sample time = 1 ms, because the DC motor is a fast dynamic system and we need this frequency to simulate the real system accurately.

However, the Kalman Filter is a digital entity, it’s supposed to run on a machine, presumably an embedded device, and here we are simulating a more “reasonable” sample time. This shows also that the filter works well with this “reasonable” sample time and that doesn’t need a super fast device.

Python Simulation

Loop

In the main function, we simulate the DC motor and the Kalman Filter, using a fast loop that runs every 1 ms and simulates the evolution of the DC motor differential equations. As explained before, the Kalman Filter runs only every 10 ms to account for the fact that it is running inside a digital device and 10 ms is more reasonable.

The architecture of the simulation is shown below:

Illustration of the architecture of the simulation. A voltage step excites the DC motor dynamics, the Kalman Filter reads a noisy angular position measurement and estimates the angular velocity.

Firstly, we initialize the simulation parameters, KalmanFilter, and DCMotor objects:

Python - Initializing simulation parameters, KalmanFilter, and DCMotor objects. Code can be found on the GitHub repository linked at the end of the post.

We then enter a loop that represents our simulation. At each step, we:

Set the input voltage to simulate a step of 12 V
Run a step of the DCMotor
Store the outputs of the DCMotor
Generate the desired noise
Add it to $\theta$ to produce $\theta_{meas}$
Run a step of the KalmanFilter if the iteration is the right one (remember that it will run only every 10 ms)

Python - Simulation loop iteration. Code can be found on the GitHub repository linked at the end of the post.

Lastly, we plot a comparison between $\omega$ and $\omega_{est}$ and the noise (to show the magnitude).

Simulation Result

The following plot shows the comparison between $\omega$ and $\omega_{est}$ :

The following plot shows the magnitude of the noise applied to $\theta$ to simulate $\theta_{meas}$ :

Interesting to notice that the noise is quite extreme (around +/- 100 deg consistently). However, the Kalman Filter still manages to produce a very accurate angular velocity estimation.

Conclusion

In this blog post, we’ve seen how to use a Kalman filter in Python to estimate the angular velocity of a DC motor. We implemented both the motor and the Kalman filter in Python, and demonstrated how to run a simulation. The Kalman filter, through its predictive and corrective steps, allows for real-time estimation even in the presence of noise, making it a useful tool in control systems.

If you’re interested in exploring the code further, you can find the full implementation on this GitHub repository. To improve your understanding (and have some fun :)), consider experimenting with different noise levels and system parameters. Python’s flexibility makes it ideal for these types of simulations, and it’s a valuable skill for anyone working in control engineering.

Kalman Filter: Python Implementation

The Kalman Filter – Theory

Kalman Filter – Model

Kalman Filter – Implementation

DC Motor

Differential Equations

Parameters

DC Motor Model in Python

Practical Model Predictive Control Course

PID Control – Things I Wish I Knew When I Graduated

Kalman Filter for Angular Velocity Estimation

Model

Kalman Filter in Python

Python Simulation

Loop

Simulation Result

Conclusion

Leave a Comment Cancel Reply

Practical Model Predictive Control Course

PID Control - Things I Wish I Knew When I Graduated

Kalman Filter: Python Implementation

The Kalman Filter – Theory

Kalman Filter – Model

Kalman Filter – Implementation

DC Motor

Differential Equations

Parameters

DC Motor Model in Python

Practical Model Predictive Control Course

PID Control – Things I Wish I Knew When I Graduated

Kalman Filter for Angular Velocity Estimation

Model

Kalman Filter in Python

Python Simulation

Loop

Simulation Result

Conclusion

Related Posts

Leave a Comment Cancel Reply