Фильтр Калмана + Arduino, на практике

Фильтр Калмана + Arduino на практике. Вот рабочий пример:

Красная линия - без фильтрации Калмана, зелёная - с фильтром.

Код для Arduino:

// Filter temperature sensor readings using the Kalman process

const int sensorPin = A0;  // named constant for the pin the sensor is connected to

// kalman variables
float varVolt = 1.12184278324081E-05;  // variance determined using excel and reading samples of raw sensor data
float varProcess = 1e-8;
float Pc = 0.0;
float G = 0.0;
float P = 1.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;

void setup() {
  // open a serial connection to display values
  Serial.begin(9600);
}

void loop() {
  int sensorVal = analogRead(sensorPin);     // read the value on AnalogIn pin 0 and store it in a variable
  float voltage = sensorVal * 5.0 / 1023.0;  // convert the ADC reading to voltage

  // kalman process
  Pc = P + varProcess;
  G = Pc/(Pc + varVolt);    // kalman gain
  P = (1-G)*Pc;
  Xp = Xe;
  Zp = Xp;
  Xe = G*(voltage-Zp)+Xp;   // the kalman estimate of the sensor voltage
    
  Serial.print(voltage,5);
  Serial.print(",");
  Serial.print(Xe,5);
  Serial.println();

}

Исходник для Arduino.
Arduino Code: https://gist.github.com/Zymotico/836c5d82d5b52a2a3695