SimpleKalmanFilter

SimpleKalmanFilter

class SimpleKalmanFilter
{
public:
	
	float xhat, xhatminus;
	float P, Pminus;
	float _x, _p;
	
	float Q, R, K;
	
	SimpleKalmanFilter()
	{
		_x = xhat = 0;
		_p = Pminus = 1;
	}
	
	void init(float Q=1e-5, float R=0.01)
	{
		this->Q = Q;
		this->R = R;
	}
	
	float filter(float z)
	{
		xhatminus = _x;
		Pminus = _p + Q;
		
		K = Pminus / (Pminus + R);
		xhat = xhatminus + K * (z - xhatminus);
		P = (1 - K) * Pminus;
		
		_x = xhat;
		_p = P;
		
		return xhat;
	}
};