Kalman Filter

A simple overview for single dimension

• x_n = x_n-1 + K * (x_in - x_n-1)

• K is the Kalman gain and is recalculated in every iteration
  
• Q and R are constants
  

300px

Simple explanations

• Poormankalman pdf

• Simple kalman filter example

• 1 Understanding and Applying Kalman Filtering, Lindsay Kleeman

• IEEE SIGNAL PROCESSING MAGAZINE Understanding the Basis of the Kalman FilterVia a Simple and Intuitive Derivation,Ramsey Faragher

Simple C examples

1-dimensional Kalman Filter, Arduino version

• 1-dimensional Kalman Filter, Arduino version

File:Kalman-02.jpg

1-dimensional Kalman Filter, Arduino version .ino

File:Kalman-03.jpg

1-dimensional Kalman Filter, Arduino version .h

//kalmanA.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);
}

//kalmanA.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);

---

From above code:

• x_n = x_n-1 + K * (x_in - x_n-1)

• K is the Kalman gain and is recalculated in every iteration

---

single state kalman filtering

• single state kalman filtering

File:Kalman-01.jpg

Single state Kalman filtering

/*kalman variables, Sz and Sw are also known as Q and R respectively
They should be initialized before use, in your own code.
usage example:
#include "kalman.c"
struct KALMAN kalman_data;
kalman_data.Sz = 0.005f;
kalman_data.Sw = 5.0f;
while(1){
stepKalman(&kalman_data, (float)some_kind_of_measurement);
printf("output: %f\n", kalman_data.x);
}
-----------------------------------------------------------------------------*/
#define KALMAN_TYPE float
struct KALMAN{
KALMAN_TYPE x;
KALMAN_TYPE x_last;
KALMAN_TYPE P;
KALMAN_TYPE P_last;
KALMAN_TYPE Sz;
KALMAN_TYPE Sw;
};
//
void stepKalman(struct KALMAN *kal, KALMAN_TYPE measurement){
KALMAN_TYPE P_temp, K, x_temp;
//predict
x_temp = kal->x_last;
P_temp = kal->P_last + kal->Sw; //Q
//update
K = (1/(P_temp + kal->Sz)) * P_temp; //R
kal->x = x_temp + K * (measurement - x_temp);
kal->P = (1 - K) * P_temp;
//save previous states
kal->x_last = kal->x;
kal->P_last = kal->P;
}

External Links

• Greg Czerniak
• wikipedia
• Kalman Filtering
• mbed library Oskar Weigl
• mbed library Shuto Naruse
• c++ library
  • in mbed forum
• kalman-filtering-for-programmers at stackoverflow
  • Kalman Filter For Dummies
• Simple Kalman Filter In C
• Kalman filter in C++ for Arduino available
  • SourceForge.net > Find Software > urbanchallenge > SCM Repositories > urbanchallenge > Elcano > KalmanFilter.cpp
• Kalman Filter Speed Estimation from Single Loop Data

---

• Simplified kalman finally to low pass filter, but still usefull
  • Read the comments
• Filtering Sensor Data with a Kalman Filter...Implementing this filter in C code (e.g. for an Arduino) is quite simple.
  • script for the graphics ... for general use...
  • arduino .pde at github

• Yet another Kalman filter implementation for Arduino
  • Checkout my implementation of Kalman filter. Original algorithm found here and wrapped into C++ class.
• Low Pass Filter – Kalman – PID