ultrasonic.ino

#define TRIG_PIN 12
#define ECHO_PIN 11

#define SAMPLE_INTERVAL_US 30

#define TEMPERATURE 25
#define HUMIDITY 50

static float prv_get_speed_of_sound() {
  // Linearized 0% RH:   331.45 + 0.606718 * temperature
  // Linearized 100% RH: 332.39208 + 0.683792 * temperature
  // Combined algebraically to eliminate branches, divisions, and square roots:
  return 331.45f + 0.606718f * TEMPERATURE + (0.0094208f + 0.000077074f * TEMPERATURE) * HUMIDITY;
}

float SPEED_OF_SOUND = prv_get_speed_of_sound();

struct KalmanFilter {
  float q; // Process noise (increase this if Kalman lags too much during fast moves)
  float r; // Measurement noise
  float x; // Estimated value
  float p; // Error covariance
  float k; // Kalman gain

  void init(float process_noise, float meas_noise, float initial_guess) {
    q = process_noise;
    r = meas_noise;
    x = initial_guess;
    p = 1.0f;
  }

  float update(float measurement) {
    p = p + q;
    k = p / (p + r);
    x = x + k * (measurement - x);
    p = (1.0f - k) * p;
    return x;
  }
};

KalmanFilter kf;

void setup() {
  Serial.begin(9600);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

void loop() {
  test_1(50); // Test 1
  // test_2(30); // Test 2
  delay(100);
}

static void test_1(uint8_t sample_count) {
  float samples[sample_count];
  float sum = 0;

  for (uint8_t i = 0; i < sample_count; i++) {
    float distance = SPEED_OF_SOUND * prv_read() * 0.5e-4;
    samples[i] = distance;
    sum += distance;
    delayMicroseconds(SAMPLE_INTERVAL_US);
  }

  float avg = sum / sample_count;

  float ss = 0;
  for (uint8_t i = 0; i < sample_count; i++) {
    ss += ((samples[i] - avg) * (samples[i] - avg));
  }

  float sd = sqrt(ss / sample_count);

  Serial.print("Avg = ");
  Serial.print(avg);
  Serial.print(" | Standard Deviation = ");
  Serial.println(sd);
}

static void test_2(uint8_t sample_count, float d_ref) {
  float samples[sample_count];
  float sum = 0;
  uint8_t valid_count = 0;

  for (uint8_t i = 0; i < sample_count; i++) {
    uint32_t duration = prv_read();

    if (duration == 0) {
      samples[i] = -1.0f;
    } else {
      float distance = SPEED_OF_SOUND * duration * 0.5e-4;
      samples[i] = distance;
      sum += distance;
      valid_count++;
    }
    delayMicroseconds(SAMPLE_INTERVAL_US);
  }

  if (valid_count == 0) {
    Serial.print("Avg = 0 | Standard Deviation = 0 | Error = 0 | Valid Rate = 0%");
    return;
  }

  float avg = sum / valid_count;
  float valid_rate = ((float)valid_count / sample_count) * 100.0f;

  float ss = 0;
  for (uint8_t i = 0; i < sample_count; i++) {
    if (samples[i] != -1.0f) {
      ss += ((samples[i] - avg) * (samples[i] - avg));
    }
  }

  float sd = sqrt(ss / valid_count);

  Serial.print("Avg = ");
  Serial.print(avg);
  Serial.print(" | Standard Deviation = ");
  Serial.print(sd);
  Serial.print(" | Error = ");
  Serial.print(avg - d_ref);
  Serial.print(" | Valid Rate = ");
  Serial.print(valid_rate);
  Serial.println("%");
}

static void test_3() {

}

static uint32_t prv_read() {
  // Make sure its off first
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIG_PIN, LOW);
  
  return pulseIn(ECHO_PIN, HIGH, 30000);
}

static uint32_t prv_get_median(uint32_t arr[], uint8_t n) {
  if (n <= 0) return 0;
  
  for (uint8_t i = 1; i < n; i++) {
    const uint32_t key = arr[i];
    int8_t j = i - 1;

    while (j >= 0 && arr[j] > key) {
      arr[j + 1] = arr[j];
      j = j - 1;
    }
    arr[j + 1] = key;
  }

  return arr[n / 2];
}