1. Introduction
Kalman filtering is an algorithm that uses a series of measurements (including statistical noise and other inaccuracies) observed over time to make estimates of unknown variables more accurate than just use a single measurement.
Kalman filtering is an algorithm that uses a series of measurements (including statistical noise and other inaccuracies) observed over time to make estimates of unknown variables more accurate than just use a single measurement.
Figure: Blue line is sensor data from ADC with noise and Brown line is sensor data after applying Kalman Filter
In order to understand more about the equations and calculations, please refer this.In summary, the filter process includes 2 steps: Prediction and Correction.
- Prediction step: tries to predict the value based on system model with previous corrected value.
- Correction step: tries to correct the predicted value in prediction step by measurement value.
This post will make a simple demo of applying Kalman Filter to ESP to make sensor measurement from a LDR (Light Dependent Resistor) more accurate.
Figure: LDR sensor
Figure: ESP32 module
ESP32 3.3V connect LDR Vcc
ESP32 GND connect LDR GND
ESP32 G34(ADC pin) connect LDR A0
3. Software
I developed the matrix version of Kalman filter.
It can be applied to Arduino, ESP8266/ESP32/MCUs.
There are 3 files that you need to include in your project (ESP8266/ESP32/MCUs):
- ArduiKalman.cpp
- ArduiKalman.h
- mat.h
where:
n: number of states
m: number of measurement values
xc[n]: correct state vector
xp[n]: predict state vector
A(n, n): System dynamics matrix
H(m, n): Measurement matrix
Q(n, n): Process noise covariance
R(m, m): Measurement noise covariance
P(n, n): Estimate error covariance
Predict step:
float *predict = m_kf.predict()
Correct step:
float *correct = m_kf.correct(measurement)
An example when n=4, m=2, and how to initialize the matrices.
A[0][0] = 1.0f;
A[1][1] = 1.0f;
A[2][2] = 1.0f;
A[3][3] = 1.0f;
H[0][0] = 1.0f;
H[1][1] = 1.0f;
Q[0][0] = 0.01f;
Q[1][1] = 0.01f;
Q[2][2] = 0.01f;
Q[3][3] = 0.01f;
R[0][0] = 0.1f;
R[1][1] = 0.1f;
P[0][0] = 1.0f;
P[1][1] = 1.0f;
P[2][2] = 1.0f;
P[3][3] = 1.0f;
xc[0] = 0.01f;
xc[1] = 0.01f;
xc[2] = 0.01f;
xc[3] = 0.01f;
Here is the source code. You can read comments to understand.
4. Result
#include "ArduiKalman.h" #define ADC_PIN 34 #define UPDATE_TIME 100 long current; int stateNum = 1; int measureNum = 1; //init matrix for Kalman float xc[1]; // correct state vector float xp[1]; // predict state vector float A[1][1]; // prediction error covariance float Q[1][1]; // process noise covariance float R[1][1]; // measurement error covariance float H[1][1]; // Measurement model float P[1][1]; // Post-prediction, pre-update KalmanFilter m_kf; void setup() { Serial.begin(115200); Serial.print(0); // To freeze the lower limit Serial.print(" "); Serial.print(3.3); // To freeze the upper limit Serial.print(" "); // put your setup code here, to run once: m_kf.init(stateNum, measureNum, &A[0][0], &P[0][0], &Q[0][0], &H[0][0], &R[0][0], &xp[0], &xc[0]); m_kf.zeros(); A[0][0] = 1.0f; H[0][0] = 1.0f; Q[0][0] = 0.01f; R[0][0] = 100.0f; P[0][0] = 1.0f; xc[0] = analogRead(ADC_PIN)/4095.0 * 3.3; } void loop() { // predict float *predict = m_kf.predict(); // correct float rand_noise = random(-100,100)/100.0; float measured_value = analogRead(ADC_PIN)/4095.0 * 3.3 + rand_noise; float measurement[measureNum]; measurement[0] = measured_value; float *correct = m_kf.correct(measurement); float estimated_value = correct[0]; //update plotter if (millis() > current) { Serial.print(measured_value); Serial.print(","); Serial.print(estimated_value); Serial.println(); current = millis() + UPDATE_TIME; } }
Figure: Blue line is sensor data from ADC with noise and Red line is sensor data after applying Kalman Filter
0 Comments