Duy tran / KalmanFilter

General Kalman filter

Dependents:   HealthCare_Graduation

Fork of KalmanFilter by long dao

KalmaFilterPulse.cpp@1:f696623db4b5, 2018-06-05 (annotated)

Committer:
DuyLionTran
Date:
Tue Jun 05 04:17:31 2018 +0000
Revision:
1:f696623db4b5
General Kalman Filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DuyLionTran 1:f696623db4b5 1 /*
DuyLionTran 1:f696623db4b5 2 * File: KalmanFilterPulse.cpp
DuyLionTran 1:f696623db4b5 3 * Author: Duy Lion Tran
DuyLionTran 1:f696623db4b5 4 *
DuyLionTran 1:f696623db4b5 5 * Created on July 12, 2016, 1:04 PM
DuyLionTran 1:f696623db4b5 6 */
DuyLionTran 1:f696623db4b5 7
DuyLionTran 1:f696623db4b5 8 #include "KalmanFilterPulse.h"
DuyLionTran 1:f696623db4b5 9
DuyLionTran 1:f696623db4b5 10 KalmanFilterPulse::KalmanFilterPulse(double q, double r, double p)
DuyLionTran 1:f696623db4b5 11 : _q(q), _q_init(q), _r(r), _r_init(0), _x(0), _p(p), _p_init(p), _k(_p / (_p + _r))
DuyLionTran 1:f696623db4b5 12 {
DuyLionTran 1:f696623db4b5 13
DuyLionTran 1:f696623db4b5 14 }
DuyLionTran 1:f696623db4b5 15
DuyLionTran 1:f696623db4b5 16 KalmanFilterPulse::~KalmanFilterPulse() {
DuyLionTran 1:f696623db4b5 17 }
DuyLionTran 1:f696623db4b5 18
DuyLionTran 1:f696623db4b5 19 /**
DuyLionTran 1:f696623db4b5 20 * update data
DuyLionTran 1:f696623db4b5 21 *
DuyLionTran 1:f696623db4b5 22 * @param measurement
DuyLionTran 1:f696623db4b5 23 * @return
DuyLionTran 1:f696623db4b5 24 */
DuyLionTran 1:f696623db4b5 25 double KalmanFilterPulse::kalmanUpdate(double measurement) {
DuyLionTran 1:f696623db4b5 26 //prediction update
DuyLionTran 1:f696623db4b5 27 //omit _x = _x
DuyLionTran 1:f696623db4b5 28 _p = _p + _q;
DuyLionTran 1:f696623db4b5 29
DuyLionTran 1:f696623db4b5 30 //measurement update
DuyLionTran 1:f696623db4b5 31 _k = _p / (_p + _r);
DuyLionTran 1:f696623db4b5 32 _x = _x + _k * (measurement - _x);
DuyLionTran 1:f696623db4b5 33 _p = (1 - _k) * _p;
DuyLionTran 1:f696623db4b5 34
DuyLionTran 1:f696623db4b5 35 return _x;
DuyLionTran 1:f696623db4b5 36 }