Go Down

Topic: Kalman library for Arduino (Read 3810 times) previous topic - next topic

point5

Hi,

I have come across a nice Arduino library for the Kalman noise filter but don't know how to use it.... wondering if anyone out there does?  My sketch is measuring temperature every x seconds and I would like to smooth out the noise a bit :-)

FilteringScheme.h
Code: [Select]
#ifndef FilteringScheme_h
#define FilteringScheme_h

//inspired by interactive-matter

class KalmanFilter {
  public:
    KalmanFilter();
    KalmanFilter(float q, float r, float p, float intial_value);
    void KalmanInit(float q, float r, float p, float intial_value);
    void set_q(float _q);
    void set_r(float _r);
    float q; //process noise covariance
    float r; //measurement noise covariance
    float x; //value
    float p; //estimation error covariance
    float k; //kalman gain
    float intial_value;
    float measureRSSI(float raw_rssi);   
   
};

#endif //FilteringScheme_h




FilteringScheme.cpp
Code: [Select]
#include "FilteringScheme.h"


KalmanFilter::KalmanFilter() {}
KalmanFilter::KalmanFilter(float _q, float _r, float _p, float _intial_value) {
  q = _q;
  r = _r;
  p = _p;
  intial_value = _intial_value;


void KalmanFilter::KalmanInit(float _q, float _r, float _p, float _intial_value) {
  q = _q;
  r = _r;
  p = _p;
  intial_value = _intial_value;


float KalmanFilter::measureRSSI(float raw_rssi) {
  p = p+q; 
  k = p/(p+r)*1.0;
  x = x+k*(raw_rssi - x);
  p = (1 - k)*p*1.0;
  return x;
}

void KalmanFilter::set_q(float _q) {
  q = _q;
}

void KalmanFilter::set_r(float _r) {
  r = _r;
}

point5

Hi, Ben from electricstitches.com has kindly helped out with usage :-) Thanks Ben - see below for those interested.

Does anyone know how to go about setting Q & R Vals?  What the limits are and what each val does?

My sensor output ranges from 400 to 2000 and has noise of about +/-25 but the settings below give filtered output at or close to zero?

Code: [Select]
#include <Wire.h>
#include <Sensor.h>
#include "FilteringScheme.h"

int sampleRate = 5000;
float sampleValue =0;

KalmanFilter kFilters[1]; //only one value to filter
void setup()
{
  Serial.begin(9600);
  float qVal = 0.015; //Set Q Kalman Filter
  float rVal = 1.2; //Set K Kalman Filter
  float p = 5.0;
  float InitVal = 65;
  kFilters[0].KalmanInit(qVal,rVal,p,InitVal); //Initialize Kalman Filter
}

jwatte


Code: [Select]

float KalmanFilter::measureRSSI(float raw_rssi) {
  p = p+q; 
  k = p/(p+r)*1.0;
  x = x+k*(raw_rssi - x);
  p = (1 - k)*p*1.0;
  return x;
}



I don't understand why this filter multiplies by 1.0. That type extends from "float" to "double," but that immediately gets truncated back to "float" in the assignment. Maybe there's some subtlety that's beyond me here, and I'd love to understand why.

How many samples are you using? Is the problem simply that the filter is slow to respond, or would it come out with the wrong value no matter how many inputs you push into it? Specifically, if you just push in a particular value (say, 2000), how long does it take until it reaches that value?

In general, what I know of Kalman filters (which is not super much) you need some number of measurements to make it "go" -- one sample per minute might not be sufficient for a good estimate until you've let it run for a longer time.

Finally, you initialize it with a guess of 65, but you say your range is from a few hundred to a few thousand. Are you supposed to convert the range of the sensors to an estimate of degrees Farenheit? If so, are you doing that conversion before you apply the filter?

You might want to Serial.println() the sensor value, the range-converted value, and the filtered output value for each measurement, and check out how they move over time.

rexlee

The variable
Code: [Select]
intial_value is initialized but is never used in the algorithm.
I believe that if
Code: [Select]
x instead is initialized with the initial "best guess value", the algorithm will converge more quickly (assuming the initial guess was a good guess)
Thus
Code: [Select]
void KalmanFilter::KalmanInit(float _q, float _r, float _p, float _intial_value) {
  q = _q;
  r = _r;
  p = _p;
  x = _intial_value;
}


initial_value can be given the flick!

rexlee

Quote
I don't understand why this filter multiplies by 1.0.

I did not understand it either, so I deleted it!  It turns out that the sketch size remains the same, so the compiler must be smart enough to have given it the flick!

robtillaart

Quote
I don't understand why this filter multiplies by 1.0.

sometimes used to explicitly convert an integer value to float. But in this case the expression is already "float"
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

PaulS

Quote
I don't understand why this filter multiplies by 1.0.

Perhaps as a place holder if you need to multiply by 1.05 or 0.95 instead.

Krupski


Hi,

I have come across a nice Arduino library for the Kalman noise filter but don't know how to use it.... wondering if anyone out there does?  My sketch is measuring temperature every x seconds and I would like to smooth out the noise a bit :-)



Can't you just take many readings and average them to remove noise?
Gentlemen may prefer Blondes, but Real Men prefer Redheads!

jwatte

That's known as a rectangular integration window, and is one of the most basic filters. (it's an instance of Finite Impulse Response filters or general convolution.) It's not that great, because it needs a lot of data to remove noise, and at the same time, it reacts kind-of slowly to changes. Kalman filters try to strike a different balance between noise rejection, response time, memory usage and computation requirements. Once you get into it, there are tons of different filters and statistical methods you can use to shape your readings, if you have the time, memory, computation power, and inclination to do so.

chung


I have come across a nice Arduino library for the Kalman noise filter


Can you give a reference for this library?
An Arduino development board costs €20~60. A pack of 20 Zener diodes to protect your board from almost certain damage costs less than €1...

Go Up