Kalman.cpp

00001 #include "Kalman.h"
00002 
00003 Kalman::Kalman(double q, double r, double p, double intialValue)
00004 {
00005     _kalmanState = KalmanState();
00006     _kalmanState.q = q;
00007     _kalmanState.r = r;
00008     _kalmanState.p = p;
00009     _kalmanState.x = intialValue;
00010 }
00011 
00012 Kalman::~Kalman(){}
00013 
00014 double Kalman::update(double predicted, double measurement)
00015 {
00016   //prediction
00017   _kalmanState.x = predicted * _kalmanState.x;
00018   _kalmanState.p = _kalmanState.p + _kalmanState.q;
00019 
00020   //measurement
00021   _kalmanState.k = _kalmanState.p / (_kalmanState.p + _kalmanState.r);
00022   _kalmanState.x = _kalmanState.x + _kalmanState.k * (measurement - _kalmanState.x);
00023   _kalmanState.p = (1 - _kalmanState.k) * _kalmanState.p;
00024   
00025   return _kalmanState.x;
00026 }
00027 
00028 double Kalman::getEstimated()
00029 {
00030     return _kalmanState.x;
00031 }