Revision 0:dd8a94919e2e, committed 2016-08-02

Comitter:

longdh

Date:

Tue Aug 02 18:06:10 2016 +0000

Commit message:

First commit, untested

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KamalFilterRSSI.cpp	Tue Aug 02 18:06:10 2016 +0000
@@ -0,0 +1,36 @@
+/* 
+ * File:   KamalFilterRSSI.cpp
+ * Author: longdh
+ * 
+ * Created on July 12, 2016, 1:04 PM
+ */
+
+#include "KamalFilterRSSI.h"
+
+KamalFilterRSSI::KamalFilterRSSI(double q, double r, double p)
+    : _q(q), _q_init(q), _r(r), _r_init(0), _x(0), _p(p), _p_init(p), _k(_p / (_p + _r))
+{
+
+}
+
+KamalFilterRSSI::~KamalFilterRSSI() {
+}
+
+/**
+ * update data
+ * 
+ * @param measurement
+ * @return 
+ */
+double KamalFilterRSSI::kalmanUpdate(double measurement) {
+    //prediction update
+    //omit _x = _x
+    _p = _p + _q;
+
+    //measurement update
+    _k = _p / (_p + _r);
+    _x = _x + _k * (measurement - _x);
+    _p = (1 - _k) * _p;
+
+    return _x;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KamalFilterRSSI.h	Tue Aug 02 18:06:10 2016 +0000
@@ -0,0 +1,39 @@
+/* 
+ * File:   KamalFilterRSSI.h
+ * Author: longdh
+ *
+ * Created on July 12, 2016, 1:04 PM
+ */
+
+#ifndef KAMALFILTERRSSI_H
+#define KAMALFILTERRSSI_H
+
+class KamalFilterRSSI {
+public:
+    KamalFilterRSSI(double q, double r, double p);
+    virtual ~KamalFilterRSSI();
+    
+    void init(double x){ _x = x ;}
+    void setProcessNoiseCovariance(double i){ _q = i; _q_init = i;}
+    void setMeasurementNoiseCovariance(double i){_r = i; _r_init = i ;}
+    void setEstimatiomErrorCovariance(double i){_p = i; _p_init = i ;}
+    virtual double kalmanUpdate(double measurement);
+    void reset(){_q = _q_init; _r = _r_init ; _p = _p_init;};
+    
+    double getProcessNoiseCovariance(){ return _q;}
+    double getMeasurementNoiseCovariance(){return _r;}
+    double getEstimatiomErrorCovariance(){return _p;} 
+    double getKalmanGain() { return _k;}
+    
+private:
+    double _q; //process noise covariance
+    double _q_init;
+    double _r; //measurement noise covariance
+    double _r_init;
+    double _x; //value
+    double _p; //estimation error covariance
+    double _p_init;
+    double _k; //kalman gain
+};
+
+#endif /* KAMALFILTERRSSI_H */