SimpleKalman.hpp

class SimpleKalman
{
	double P, K;
	double X;
public:
	double Q;
	double R;
	SimpleKalman()
		:P(0.0), X(0.0)
		 {};

	double initialize(double first_value)
	{
		X = first_value;
		return first_value;
	}

	double update(double measurement)
	{
		measurementUpdate();
		double result = X + (measurement - X) * K;
		X = result;
		return result;
	}

private:
	void measurementUpdate()
	{
		K = (P + Q) / (P + Q + R);
		P = R * (P + Q) / (R + P + Q);
	}
};