Demo 43: How to apply Kalman Filter to ESP to make sensor measurement more accurate

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.

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

2. Hardware
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.

https://github.com/nhatuan84/Arduino-KalmanFilter

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.

#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;
  }

}