1-dimensional Kalman Filter, Arduino version

Converted the Processing code (which was a conversion of Adrian Boeing's C++ code) to Arduino.

kalman.ino
// simple Kalman filter
// adapted from C code by Adrian Boeing, www.adrianboeing.com 

KalmanFilter::KalmanFilter(float estimate, float initQ, float initR)
{
  Q = initQ;
  R = initR;

  // initial values for the kalman filter
  x_est_last = 0;
  P_last = 0;

  // initialize with a measurement
  x_est_last = estimate;
}


// add a new measurement, return the Kalman-filtered value
float KalmanFilter::step(float z_measured)
{
  // do a prediction
  x_temp_est = x_est_last;
  P_temp = P_last + R*Q;

  // calculate the Kalman gain
  K = P_temp * (1.0/(P_temp + R));

  // correct
  x_est = x_temp_est + K * (z_measured - x_temp_est); 
  P = (1- K) * P_temp;

  // update our last's
  P_last = P;
  x_est_last = x_est;

  return (x_est);
}

kalman.h
class KalmanFilter
{
public:
  KalmanFilter(float estimate, float initQ, float initR);
  float step(float measurement);
private:
  // initial values for the kalman filter
  float x_est_last;
  float P_last;

  // the noise in the system
  float Q;
  float R;

  float K;    // Kalman gain
  float P;
  float P_temp;
  float x_temp_est;
  float x_est;
};

and how to use:

// simplistic Kalman filter for encoder readings
KalmanFilter kf(0, 0.01, 1.0);

float avg_err = kf.step(track_err);

Kalman Filter for Dummies

I found a simple 1-dimensional Kalman filter online.  It doesn't really look much better than an IIR filter, to be honest - no control input, no way of factoring in the "ideal" value into the estimate.  But as the math of Kalman filters eludes me, this will have to do for now.

I've ported it to Processing.  Should be easy to convert to Arduino or similar.

// simple Kalman filter
// adapted from C code by Adrian Boeing, www.adrianboeing.com
class KalmanFilter
{
  // initial values for the kalman filter
  float x_est_last = 0;
  float P_last = 0;
  // the noise in the system
  float Q;
  float R;
  float K;    // Kalman gain
  float P;
  float P_temp;
  float x_temp_est;
  float x_est;
  // constructor - initialize with first estimate
  KalmanFilter(float estimate)
  {
    Q = 0.022;
    R = 0.617;
    // initialize with a measurement
    x_est_last = estimate;
  }
  KalmanFilter(float estimate, float initQ, float initR)
  {
    Q = initQ;
    R = initR;
    // initialize with a measurement
    x_est_last = estimate;
  }
  // add a new measurement, return the Kalman-filtered value
  public float step(float z_measured)
  {
    // do a prediction
    x_temp_est = x_est_last;
    P_temp = P_last + R*Q;
    // calculate the Kalman gain
    K = P_temp * (1.0/(P_temp + R));
    // correct
    x_est = x_temp_est + K * (z_measured - x_temp_est);
    P = (1- K) * P_temp;
    // update our last's
    P_last = P;
    x_est_last = x_est;
    return (x_est);
  }
}

And to use it..

  // initial estimate, measurement noise, process noise
  KalmanFilter kf = new KalmanFilter(0, 0.05, 1);
  // update with a new measurement
  float data = kf.step(measurement);

Womarts 320x240 TFT LCD Shield with Touch Screen

I bought one of these Womarts 320x240 TFT LCD Touch Screen boards on ebay because they are cheaper than the Sainsmart equivalents, and have both the TFT and carrier board in a single package, unlike the separate TFT board and carrier board that Sainsmart has. Heck, these are even cheaper than the 128x64 I2C LCD boards being sold out there.  The downside of the Womarts board is that it consumes a heck of a lot of pins. You'll be hard pressed to do anything else with one of these boards riding your Arduino.

There is a version of the Womarts board for Mega-type Arduino, and another for the Uno form factor. This post is about the Mega version. There has been some discussion on the Arduino forums about how good (or bad) these $25 Womarts boards are, so I had rather low expectations.

The main problem is that one set of pins on the board doesn't match up with my (also off-brand) China Arduino Mega. But the problem is with the board, not the Mega.

Here we see that the board consumes all of the extra pins on the bottom of the Mega. Oops.. there go the SPI pins.. I'm not sure if the SPI pins are actually used (I hope not!)

On the top side of the Arduino (with the USB port on the right):

The bummer: the bottom set of pins don't fit:

The solution is to bend the pins inward. Not the best reliability, but a necessary evil.

And the best part.. the Henning Karlsen UTFT library works out of the box (just uncomment the line near the top of the example program for the Mega).