Projekt Arduino

#include <SimpleKalmanFilter.h>

double e_mea = 10;
double e_est = 10;
double q = 0.01;

SimpleKalmanFilter skf = SimpleKalmanFilter(e_mea, e_est, q);

double ultraSonicDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long microseconds = pulseIn(echoPin, HIGH);
  return (double)(microseconds/58);
}

void setup() {
  pinMode(12, OUTPUT);
  pinMode(13, INPUT);
}

void loop() {
  double distance = ultraSonicDistance(12, 13);
  double distanceSKF = skf.updateEstimate(distance);

  Serial.print(distance);
  Serial.print(" ");
  Serial.print(distanceSKF);
  Serial.println(" ");

  delay(100);
}