Duy tran / KalmanFilter

General Kalman filter

Dependents:   HealthCare_Graduation

Fork of KalmanFilter by long dao

KamalFilterRSSI.cpp@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.cpp
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 #include "KamalFilterRSSI.h"
longdh 0:dd8a94919e2e 9
longdh 0:dd8a94919e2e 10 KamalFilterRSSI::KamalFilterRSSI(double q, double r, double p)
longdh 0:dd8a94919e2e 11 : _q(q), _q_init(q), _r(r), _r_init(0), _x(0), _p(p), _p_init(p), _k(_p / (_p + _r))
longdh 0:dd8a94919e2e 12 {
longdh 0:dd8a94919e2e 13
longdh 0:dd8a94919e2e 14 }
longdh 0:dd8a94919e2e 15
longdh 0:dd8a94919e2e 16 KamalFilterRSSI::~KamalFilterRSSI() {
longdh 0:dd8a94919e2e 17 }
longdh 0:dd8a94919e2e 18
longdh 0:dd8a94919e2e 19 /**
longdh 0:dd8a94919e2e 20 * update data
longdh 0:dd8a94919e2e 21 *
longdh 0:dd8a94919e2e 22 * @param measurement
longdh 0:dd8a94919e2e 23 * @return
longdh 0:dd8a94919e2e 24 */
longdh 0:dd8a94919e2e 25 double KamalFilterRSSI::kalmanUpdate(double measurement) {
longdh 0:dd8a94919e2e 26 //prediction update
longdh 0:dd8a94919e2e 27 //omit _x = _x
longdh 0:dd8a94919e2e 28 _p = _p + _q;
longdh 0:dd8a94919e2e 29
longdh 0:dd8a94919e2e 30 //measurement update
longdh 0:dd8a94919e2e 31 _k = _p / (_p + _r);
longdh 0:dd8a94919e2e 32 _x = _x + _k * (measurement - _x);
longdh 0:dd8a94919e2e 33 _p = (1 - _k) * _p;
longdh 0:dd8a94919e2e 34
longdh 0:dd8a94919e2e 35 return _x;
longdh 0:dd8a94919e2e 36 }