HB100 doppler module data handling and display to lcd and serial.

HB100_serial.ino

/*
  HB100 Microwave Doppler data analysis and display of max and current speed. 
 */

#include <LiquidCrystal.h>

int FoutPin = 13;
int resetPin = 12;

float vmax = 0;

float vma = 0;
int n = 0;

const float FvsV = 19.49; // V = FsvV * f

const int maxSpeed = 50;
const int minSpeed = 1;

const int timeout = minSpeed * 1000000 / FvsV;

LiquidCrystal lcd(7, 6, 5, 4, 3, 2);


  double kalman_q = 0.5; //process noise covariance
  double kalman_r = 100; //measurement noise covariance
  double kalman_x = 0; //value
  double kalman_p = 1; //estimation error covariance
  double kalman_k; //kalman gain

float medianTable[3] = {0, 0, 0}; 

void swap(float *a, float*b) {
 float tmp = *a; 
 *a = *b; 
 *b = tmp;  
}

float medianFilter3(float value) {
  // TODO: there is some error here... and probably should have length of 5
  medianTable[0] = medianTable[1];
  medianTable[1] = medianTable[2]; 
  medianTable[2] = value; 
  
  float sA[3] = {medianTable[0], medianTable[1], medianTable[2]}; 
  
  if (sA[0] > sA[1]) swap(&sA[0], &sA[1]);
  if (sA[1] > sA[2]) swap(&sA[1], &sA[2]);
  if (sA[0] > sA[1]) swap(&sA[0], &sA[1]);
  
  return sA[1];
}

void
kalman_update(double measurement)
{
  //prediction update
  //omit x = x
  kalman_p += kalman_q;

  //measurement update
  kalman_k = kalman_p / (kalman_p + kalman_r);
  kalman_x = kalman_x + kalman_k * (measurement - kalman_x);
  kalman_p *= (1 - kalman_k);
}

void setup() {
  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);
  pinMode(FoutPin, INPUT);
  pinMode(resetPin, INPUT);
  lcd.begin(16, 2);
  lcd.clear();
  lcd.print("Setting up...");
  delay(1000);
}

void lcdOutput(float v, float vmax) {
  // Display current speed and max speed on lcd
  lcd.clear();
  lcd.setCursor(0, 1);
  lcd.print("v = ");
  lcd.setCursor(4, 1);
  lcd.print(v);
  lcd.setCursor(9, 1);
  lcd.print("km/h");

  lcd.setCursor(0, 0);
  lcd.print("max ");
  lcd.setCursor(4, 0);
  lcd.print(vmax);
  lcd.setCursor(9, 0);
  lcd.print("km/h");

}

float velocity(float frequency) {
  // return velocity calculated from frequency
  return frequency / FvsV;
}

int errors = 0;

int isResetPressed() {
  // Check if reset button is down
  if (digitalRead(resetPin) == LOW)
    return 1;
  else
    return 0;
}

void loop() {
  // TODO: kalman filtering, when errors = 0

  unsigned long duration = pulseIn(FoutPin, HIGH, timeout);

  if (isResetPressed() == 1) {
    vmax = 0.0; 
    lcdOutput(vma, vmax); 
  }
  if (duration == 0) {
    // timeout occurred
    errors++;
    return;
  }

  float f = 500000 / duration;

  float v = velocity(f);

  if (v > maxSpeed) {
    errors++;
    return;
  }



  // calculate moving average of v
  if (n == 0) {
    vma = v;
  }
  else {
    vma = (v + n * vma) / (n + 1);
  }
  n++;

  // todo: make it median 5
  float filtered = medianFilter3(v); 
  kalman_update(filtered);

    Serial.print(v);
    Serial.print(" "); 
    Serial.print(kalman_x);
    Serial.print(" ");
    Serial.println(filtered); 
  

  if (n > 10) {

    if (errors == 0 && kalman_x > vmax) vmax = kalman_x;
    if (errors == 0) lcdOutput(kalman_x, vmax);
    if (errors > 0) lcdOutput(0.0, vmax); 

/*
    Serial.print(v);
    Serial.print(" "); 
    Serial.print(vma);
    Serial.print(" ");
    Serial.print(kalman_x);
    Serial.print(" "); 
    Serial.println(errors);
*/
    errors = 0;
    n = 0;
  }

}

Todo: proper median filtering with 5 element window. This 3 element quick and dirty seems to produce wrong results...