SU_Kalman1D.h

//==================================================================
/// SU_Kalman1D.h
///
/// Created by Davide Pasca - 2018/06/01
/// See the file "license.txt" that comes with this project for
/// copyright info. 
//==================================================================

#ifndef SU_KALMAN1D_H
#define SU_KALMAN1D_H

//==================================================================
// Need to tweak value of sensor and process noise
// arguments :
// process noise covariance
// measurement noise covariance
// estimation error covariance
class SU_Kalman1D
{
    const double _q; // process noise covariance
    const double _r; // measurement noise covariance
    double   _p = 0; // estimation error covariance
    double   _k = 0; // kalman gain
    double   _x = 0; // value
 
public :
    SU_Kalman1D( double q, double r, double p )
        : _q(q)
        , _r(r)
        , _p(p)
    {
        _k = _p / (_p + _r);
    }

    void SetInitValue( double x ) { _x = x; }

    double 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;
    }
};
 
#endif