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?
User | Revision | Line number | New 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 */ |