Pages: [1]   Go Down
Author Topic: Kalman library for Arduino  (Read 3559 times)
0 Members and 1 Guest are viewing this topic.
UK
Offline Offline
Sr. Member
****
Karma: 1
Posts: 278
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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:
#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;
}
Logged

UK
Offline Offline
Sr. Member
****
Karma: 1
Posts: 278
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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
}
Logged

Offline Offline
God Member
*****
Karma: 4
Posts: 813
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
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.
Logged

Australia
Offline Offline
Newbie
*
Karma: 0
Posts: 12
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

The variable
Code:
intial_value
is initialized but is never used in the algorithm.
I believe that if
Code:
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:
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!
Logged

Australia
Offline Offline
Newbie
*
Karma: 0
Posts: 12
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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!
Logged

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 221
Posts: 13848
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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"
Logged

Rob Tillaart

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

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 632
Posts: 50182
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Worst state in America
Offline Offline
God Member
*****
Karma: 32
Posts: 808
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

Gentlemen may prefer Blondes, but Real Men prefer Redheads!

Offline Offline
God Member
*****
Karma: 4
Posts: 813
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Offline Offline
Jr. Member
**
Karma: 0
Posts: 69
Arduino Rocks!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Can you give a reference for this library?
Logged

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...

Pages: [1]   Go Up
Jump to: