# Kalman library for Arduino

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

``````#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;

};

#endif //FilteringScheme_h
``````

FilteringScheme.cpp

``````#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;
}

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;
}
``````

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?

``````#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
}
``````

point5: ``` 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.

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

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

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!

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"

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.

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 :-)

Can't you just take many readings and average them to remove noise?

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.

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

Can you give a reference for this library?