Duy tran / KalmanFilter

General Kalman filter

Dependents:   HealthCare_Graduation

Fork of KalmanFilter by long dao

KalmaFilterPulse.cpp

Committer:
DuyLionTran
Date:
2018-06-05
Revision:
1:f696623db4b5

File content as of revision 1:f696623db4b5:

/* 
 * File:   KalmanFilterPulse.cpp
 * Author: Duy Lion Tran
 * 
 * Created on July 12, 2016, 1:04 PM
 */

#include "KalmanFilterPulse.h"

KalmanFilterPulse::KalmanFilterPulse(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))
{

}

KalmanFilterPulse::~KalmanFilterPulse() {
}

/**
 * update data
 * 
 * @param measurement
 * @return 
 */
double KalmanFilterPulse::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;
}