├── KalmanFilter ├── KalmanFilter.cpp ├── KalmanFilter.h ├── examples │ └── sample │ │ └── sample.ino └── keywords.txt ├── README.md └── kalman.png /KalmanFilter/KalmanFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Kalman.cpp - Arduino Library for simple Kalman filter 3 | * 4 | * Created by Max Lunin http://max.lunin.info : on 27.09.13. 5 | * as part of Arduino automatization described at http://exotic-garden-at-home.blogspot.com 6 | * Released into the public domain. 7 | */ 8 | 9 | #include "KalmanFilter.h" 10 | 11 | KalmanFilter::KalmanFilter(double q, double r, double f, double h) { 12 | F = f; 13 | Q = q; 14 | H = h; 15 | R = r; 16 | } 17 | 18 | double KalmanFilter::getX0() const { 19 | return x0; 20 | } 21 | 22 | double KalmanFilter::getP0() const { 23 | return p0; 24 | } 25 | 26 | double KalmanFilter::getF() const { 27 | return F; 28 | } 29 | 30 | double KalmanFilter::getQ() const { 31 | return Q; 32 | } 33 | 34 | double KalmanFilter::getH() const { 35 | return H; 36 | } 37 | 38 | double KalmanFilter::getR() const { 39 | return R; 40 | } 41 | 42 | double KalmanFilter::getState() const { 43 | return state; 44 | } 45 | 46 | void KalmanFilter::setState(double state) { 47 | this->state = state; 48 | } 49 | 50 | double KalmanFilter::getCovariance() const { 51 | return covariance; 52 | } 53 | 54 | void KalmanFilter::setCovariance(double covariance) { 55 | this->covariance = covariance; 56 | } 57 | 58 | void KalmanFilter::correct(double data) { 59 | x0 = F*state; 60 | p0 = F*F*covariance + Q; 61 | 62 | //measurement update - correction 63 | double K = (H*p0)/(H*p0*H + R); 64 | state = x0 + K*(data - H*x0); 65 | covariance = (1 - K*H)*p0; 66 | } 67 | -------------------------------------------------------------------------------- /KalmanFilter/KalmanFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * kalmanFilter.h - Library for simple Kalman filter 3 | * 4 | * Created by Max Lunin http://max.lunin.info : on 27.09.13. 5 | * as part of Arduino automatization described at http://exotic-garden-at-home.blogspot.com 6 | * Released into the public domain. 7 | */ 8 | 9 | /* 10 | // Usage : 11 | KalmanFilter kalmanFilter; // create with default parameters 12 | 13 | kalmanFilter.setState(startValue); 14 | kalmanFilter.setCovariance(0.1); 15 | 16 | while (true) 17 | { 18 | double value = getValueFromSensor(); 19 | kalmanFilter.correct(value); 20 | double correctedValue = kalmanFilter.getState(); 21 | } 22 | */ 23 | 24 | #ifndef Kalman_Kalman_h 25 | #define Kalman_Kalman_h 26 | 27 | #include "Arduino.h" 28 | 29 | // http://en.wikipedia.org/wiki/Kalman_filter 30 | 31 | class KalmanFilter { 32 | public: 33 | double getState() const; 34 | void setState(double state); 35 | 36 | void setCovariance(double covariance); 37 | 38 | KalmanFilter(double q = 1, double r = 1, double f = 1, double h = 1); 39 | 40 | void correct(double data); 41 | 42 | public: 43 | double getCovariance() const; 44 | double getX0() const; 45 | double getP0() const; 46 | double getF() const; 47 | double getQ() const; 48 | double getH() const; 49 | double getR() const; 50 | 51 | private: 52 | double x0; // predicted state 53 | double p0; // predicted covariance 54 | double F; // factor of real value to previous real value 55 | double Q; // measurement noise 56 | double H; // factor of measured value to real value 57 | double R; // environment noise 58 | double state; 59 | double covariance; 60 | }; 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /KalmanFilter/examples/sample/sample.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * Sample for Kalman filter for Arduino library https://github.com/nut-code-monkey/KalmanFilter-for-Arduino 3 | * Created by Max Lunin http://max.lunin.info : on 27.09.13. 4 | * as part of Arduino automatization described at http://exotic-garden-at-home.blogspot.com 5 | * Released into the public domain. 6 | */ 7 | 8 | #include "KalmanFilter.h" 9 | 10 | KalmanFilter kalmanFilter; 11 | 12 | double getSomeValue() { 13 | // return values in range 0..1023 14 | return analogRead(A0); 15 | } 16 | 17 | void setup() { 18 | 19 | kalmanFilter.setState( getSomeValue() ); 20 | // kalmanFilter.setCovariance(0.1); // optional 21 | 22 | Serial.begin(9600); 23 | } 24 | 25 | void loop() { 26 | 27 | double value = getSomeValue(); 28 | 29 | kalmanFilter.correct(value); 30 | double correctedValue = kalmanFilter.getState(); 31 | 32 | Serial.print(value); Serial.print(" | "); Serial.println(correctedValue); 33 | } 34 | -------------------------------------------------------------------------------- /KalmanFilter/keywords.txt: -------------------------------------------------------------------------------- 1 | KalmanFilter KEYWORD1 2 | getState KEYWORD2 3 | setState KEYWORD2 4 | getCovariance KEYWORD2 5 | setCovariance KEYWORD2 6 | getX0 KEYWORD2 7 | getF KEYWORD2 8 | getQ KEYWORD2 9 | getH KEYWORD2 10 | getR KEYWORD2 11 | correct KEYWORD2 -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | KalmanFilter-for-Arduino 2 | ======================== 3 | 4 | Simple one varable Kalman filter library for Arduino 5 | 6 | Install: 7 | -------- 8 | Import 'KalmanFilter' directory as Arduino library: 9 | 10 | Arduino.app -> Sketch -> Import Library... -> Add Library 11 | 12 | 13 | Usage: 14 | ------ 15 | 16 | Initialize filter: 17 | ``` 18 | #include "KalmanFilter.h" 19 | 20 | KalmanFilter filter; // create with default parameters 21 | filter.setState( defaultValue ); // setup vith default value 22 | ``` 23 | And use: 24 | ``` 25 | double value = getValueFromSensor(); 26 | 27 | filter.correct( value ); // add new value from sensor 28 | double correctedValue = filter.getState(); // get corrected value 29 | ``` 30 | Output: 31 | ------- 32 | ![kalman filter](https://raw.github.com/nut-code-monkey/KalmanFilter-for-Arduino/master/kalman.png) 33 | -------------------------------------------------------------------------------- /kalman.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nut-code-monkey/KalmanFilter-for-Arduino/1c6a9f4d145cc2ffcdc99c0ce9d481083446c1bf/kalman.png --------------------------------------------------------------------------------