long dao / KalmanFilter

RSSI kalman filter

Dependents:   VNG_BLEBeaconPostion

KamalFilterRSSI.h@0:dd8a94919e2e, 2016-08-02 (annotated)

Committer:
longdh
Date:
Tue Aug 02 18:06:10 2016 +0000
Revision:
0:dd8a94919e2e
First commit, untested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
longdh 0:dd8a94919e2e 1 /*
longdh 0:dd8a94919e2e 2 * File: KamalFilterRSSI.h
longdh 0:dd8a94919e2e 3 * Author: longdh
longdh 0:dd8a94919e2e 4 *
longdh 0:dd8a94919e2e 5 * Created on July 12, 2016, 1:04 PM
longdh 0:dd8a94919e2e 6 */
longdh 0:dd8a94919e2e 7
longdh 0:dd8a94919e2e 8 #ifndef KAMALFILTERRSSI_H
longdh 0:dd8a94919e2e 9 #define KAMALFILTERRSSI_H
longdh 0:dd8a94919e2e 10
longdh 0:dd8a94919e2e 11 class KamalFilterRSSI {
longdh 0:dd8a94919e2e 12 public:
longdh 0:dd8a94919e2e 13 KamalFilterRSSI(double q, double r, double p);
longdh 0:dd8a94919e2e 14 virtual ~KamalFilterRSSI();
longdh 0:dd8a94919e2e 15
longdh 0:dd8a94919e2e 16 void init(double x){ _x = x ;}
longdh 0:dd8a94919e2e 17 void setProcessNoiseCovariance(double i){ _q = i; _q_init = i;}
longdh 0:dd8a94919e2e 18 void setMeasurementNoiseCovariance(double i){_r = i; _r_init = i ;}
longdh 0:dd8a94919e2e 19 void setEstimatiomErrorCovariance(double i){_p = i; _p_init = i ;}
longdh 0:dd8a94919e2e 20 virtual double kalmanUpdate(double measurement);
longdh 0:dd8a94919e2e 21 void reset(){_q = _q_init; _r = _r_init ; _p = _p_init;};
longdh 0:dd8a94919e2e 22
longdh 0:dd8a94919e2e 23 double getProcessNoiseCovariance(){ return _q;}
longdh 0:dd8a94919e2e 24 double getMeasurementNoiseCovariance(){return _r;}
longdh 0:dd8a94919e2e 25 double getEstimatiomErrorCovariance(){return _p;}
longdh 0:dd8a94919e2e 26 double getKalmanGain() { return _k;}
longdh 0:dd8a94919e2e 27
longdh 0:dd8a94919e2e 28 private:
longdh 0:dd8a94919e2e 29 double _q; //process noise covariance
longdh 0:dd8a94919e2e 30 double _q_init;
longdh 0:dd8a94919e2e 31 double _r; //measurement noise covariance
longdh 0:dd8a94919e2e 32 double _r_init;
longdh 0:dd8a94919e2e 33 double _x; //value
longdh 0:dd8a94919e2e 34 double _p; //estimation error covariance
longdh 0:dd8a94919e2e 35 double _p_init;
longdh 0:dd8a94919e2e 36 double _k; //kalman gain
longdh 0:dd8a94919e2e 37 };
longdh 0:dd8a94919e2e 38
longdh 0:dd8a94919e2e 39 #endif /* KAMALFILTERRSSI_H */