Smoothing Noisy Sensor Data with Kalman Filters

Sensor data can often be quite noisy, making it challenging to extract meaningful information. Kalman filters are a powerful tool for filtering and smoothing noisy sensor data, with applications in fields such as robotics, control systems, and signal processing.

I'm currently building a model rocket, and I am planning to add a flight computer that uses an MPU6050 inertial measurement unit (IMU), I've encountered the challenge of dealing with noisy sensor data. The MPU6050 provides gyroscope and accelerometer measurements, but these can be quite erratic due to factors like vibrations and other environmental interference. To tackle this issue, I've implemented a discrete Kalman filter, which serves as a simple yet powerful sensor fusion technique to smooth out the noisy data.

The Kalman filter implementation used in this post is inspired by the paper "An Introduction to the Kalman Filter" by Greg Welch and Gary Bishop from the University of North Carolina. Kalman filters are widely used in various fields, including robotics, control systems, and signal processing, to extract meaningful information from noisy sensor data. In the following sections, we'll explore the Kalman filter using Python and NumPy, and explore the underlying mathematics behind it.

The Kalman Filter Algorithm

The Kalman filter is a recursive algorithm that estimates the state of a dynamic system from a series of noisy measurements. The algorithm consists of two main steps:

Prediction: The Kalman filter uses a model of the system's dynamics to predict the next state of the system based on the previous state.
Correction: The Kalman filter compares the predicted state with the actual measurement and calculates a weighted average, where the weight is determined by the relative certainty of the prediction and the measurement.

The Kalman filter algorithm can be described mathematically as follows:

Prediction step:

Correction step:

Where:

is the a priori state estimate at time given the information at time
is the a posteriori state estimate at time given the information at time
is the a priori error covariance at time
is the a posteriori error covariance at time
is the Kalman gain at time
is the state transition matrix
is the observation matrix
is the process noise covariance
is the measurement noise covariance
is the measurement at time

Implementing the Kalman Filter in Python

Now, let's take a look at the Python implementation of the Kalman filter:

python

class KalmanFilter:
    def __init__(self, process_variance, measurement_variance):
        self.process_variance = process_variance
        self.measurement_variance = measurement_variance
        self.posteri_estimate = 0.0
        self.posteri_error_estimate = 1.0

    def update(self, measurement):
        priori_estimate = self.posteri_estimate
        priori_error_estimate = self.posteri_error_estimate + self.process_variance

        blending_factor = priori_error_estimate / (priori_error_estimate + self.measurement_variance)
        self.posteri_estimate = priori_estimate + blending_factor * (measurement - priori_estimate)
        self.posteri_error_estimate = (1 - blending_factor) * priori_error_estimate

        return self.posteri_estimate

The KalmanFilter class takes two parameters: process_variance and measurement_variance, which represent the uncertainty in the system's dynamics and the measurement process, respectively.

The update method takes a new measurement and returns the filtered estimate. It first calculates the a priori estimate and error estimate, then computes the Kalman gain and uses it to update the a posteriori estimate and error estimate.

Applying the Kalman Filter to Gyroscope Data

Let's apply the Kalman filter to some simulated gyroscope data. We'll generate noisy gyroscope data for the x, y, and z axes, and then use the Kalman filter to smooth the data.

python

# Generate some demo raw data
np.random.seed(0)
time = np.linspace(0, 60, 600)  # 60 seconds, 10 Hz sampling
gyro_x = np.random.normal(0, 1, size=len(time))
gyro_y = np.random.normal(0, 1, size=len(time))
gyro_z = np.random.normal(0, 1, size=len(time))

# Initialize Kalman filters for each axis
kalman_x = KalmanFilter(process_variance=0.01, measurement_variance=0.1)
kalman_y = KalmanFilter(process_variance=0.01, measurement_variance=0.1)
kalman_z = KalmanFilter(process_variance=0.01, measurement_variance=0.1)

# Apply Kalman filter to each axis
filtered_gyro_x = [kalman_x.update(measurement) for measurement in gyro_x]
filtered_gyro_y = [kalman_y.update(measurement) for measurement in gyro_y]
filtered_gyro_z = [kalman_z.update(measurement) for measurement in gyro_z]

Finally, we can plot the raw and Kalman-filtered data to see the results:

python

# Plot raw data vs. Kalman-filtered data
plt.figure(figsize=(12, 6))
plt.subplot(3, 1, 1)
plt.plot(time, gyro_x, label='Raw Data')
plt.plot(time, filtered_gyro_x, label='Kalman Filtered Data')
plt.title('X-axis Gyro Data')
plt.legend()

plt.subplot(3, 1, 2)
plt.plot(time, gyro_y, label='Raw Data')
plt.plot(time, filtered_gyro_y, label='Kalman Filtered Data')
plt.title('Y-axis Gyro Data')
plt.legend()

plt.subplot(3, 1, 3)
plt.plot(time, gyro_z, label='Raw Data')
plt.plot(time, filtered_gyro_z, label='Kalman Filtered Data')
plt.title('Z-axis Gyro Data')
plt.legend()

plt.tight_layout()
plt.show()

The Kalman filter effectively smooths out the noisy gyroscope data, making it easier to extract meaningful information.

Conclusion

In this blog post, we've explored the Kalman filter algorithm and its implementation in Python. Kalman filters are a powerful tool for filtering and smoothing noisy sensor data, and they have a wide range of applications in various fields. By understanding the underlying mathematics and the implementation details, you can adapt the Kalman filter to your specific needs and apply it to a variety of sensor data problems.

The full code for the Kalman filter implementation and the gyroscope data example is available below:

python

import numpy as np
import matplotlib.pyplot as plt

# Kalman filter implementation
class KalmanFilter:
    def __init__(self, process_variance, measurement_variance):
        self.process_variance = process_variance
        self.measurement_variance = measurement_variance
        self.posteri_estimate = 0.0
        self.posteri_error_estimate = 1.0

    def update(self, measurement):
        priori_estimate = self.posteri_estimate
        priori_error_estimate = self.posteri_error_estimate + self.process_variance

        blending_factor = priori_error_estimate / (priori_error_estimate + self.measurement_variance)
        self.posteri_estimate = priori_estimate + blending_factor * (measurement - priori_estimate)
        self.posteri_error_estimate = (1 - blending_factor) * priori_error_estimate

        return self.posteri_estimate

# Generate some demo raw data
np.random.seed(0)
time = np.linspace(0, 60, 600)  # 60 seconds, 10 Hz sampling
gyro_x = np.random.normal(0, 1, size=len(time))
gyro_y = np.random.normal(0, 1, size=len(time))
gyro_z = np.random.normal(0, 1, size=len(time))

# Initialize Kalman filters for each axis
kalman_x = KalmanFilter(process_variance=0.01, measurement_variance=0.1)
kalman_y = KalmanFilter(process_variance=0.01, measurement_variance=0.1)
kalman_z = KalmanFilter(process_variance=0.01, measurement_variance=0.1)

# Apply Kalman filter to each axis
filtered_gyro_x = [kalman_x.update(measurement) for measurement in gyro_x]
filtered_gyro_y = [kalman_y.update(measurement) for measurement in gyro_y]
filtered_gyro_z = [kalman_z.update(measurement) for measurement in gyro_z]

# Plot raw data vs. Kalman-filtered data
plt.figure(figsize=(12, 6))
plt.subplot(3, 1, 1)
plt.plot(time, gyro_x, label='Raw Data')
plt.plot(time, filtered_gyro_x, label='Kalman Filtered Data')
plt.title('X-axis Gyro Data')
plt.legend()

plt.subplot(3, 1, 2)
plt.plot(time, gyro_y, label='Raw Data')
plt.plot(time, filtered_gyro_y, label='Kalman Filtered Data')
plt.title('Y-axis Gyro Data')
plt.legend()

plt.subplot(3, 1, 3)
plt.plot(time, gyro_z, label='Raw Data')
plt.plot(time, filtered_gyro_z, label='Kalman Filtered Data')
plt.title('Z-axis Gyro Data')
plt.legend()

plt.tight_layout()
plt.show()

C++ Implementation

I wrote the python code because it was easier for me to implement directly from the paper, however, the implementation in my microcontroller is done in C++.

Below is the C++ implementation, the full code is on Github (KalmanFilter-cpp).

KalmanFilter.cppmain.cppKalmanFilter.h

cpp

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter(double process_variance, double measurement_variance) :
    process_variance_(process_variance),
    measurement_variance_(measurement_variance),
    posteri_estimate_(0.0),
    posteri_error_estimate_(1.0) {}

/*
* implemented from:
https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
*/
double KalmanFilter::update(double measurement) {
    double priori_estimate = posteri_estimate_;
    double priori_error_estimate = posteri_error_estimate_ + process_variance_;

    double blending_factor = priori_error_estimate / (priori_error_estimate + measurement_variance_);
    posteri_estimate_ = priori_estimate + blending_factor * (measurement - priori_estimate);
    posteri_error_estimate_ = (1 - blending_factor) * priori_error_estimate;

    return posteri_estimate_;
}

cpp

#include <iostream>

int main() {
    KalmanFilter kalman_filter(0.01, 0.1); // init w process variance & measurement variance

    // Example measurements
    double measurements[] = {1.1, 1.2, 1.3, 1.4, 1.5};

    // Apply Kalman filter to each measurement
    for (double measurement : measurements) {
        double filtered_measurement = kalman_filter.update(measurement);
        std::cout << "Measurement: " << measurement << ", Filtered Measurement: " << filtered_measurement << std::endl;
    }

    return 0;
}

cpp

#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_

class KalmanFilter {
public:
    KalmanFilter(double process_variance, double measurement_variance);
    double update(double measurement);

private:
    double process_variance_;
    double measurement_variance_;
    double posteri_estimate_;
    double posteri_error_estimate_;
};

#endif // KALMAN_FILTER_H_

References

[1] Welch, Greg and Gary Bishop. “Welch & Bishop , An Introduction to the Kalman Filter

The Kalman Filter Algorithm ​

Implementing the Kalman Filter in Python ​

Applying the Kalman Filter to Gyroscope Data ​

Conclusion ​

C++ Implementation ​

References ​

The Kalman Filter Algorithm

Implementing the Kalman Filter in Python

Applying the Kalman Filter to Gyroscope Data

Conclusion

C++ Implementation

References