simple kalman filter

KalmanFilterSimple.java

/**
 * Kalman filter implementation
 */
public class KalmanFilterSimple {

    private float X0;
    private float P0;

    private float F;
    private float Q;
    private float H;
    private float R;

    private float state;
    private float covariance;


    /**
     * Initializes a new instance of the KalmanFilterSimple
     */
    public KalmanFilterSimple(float q, float r) {
        this(q,r,1,1);
    }

    /**
     * Initializes a new instance of the KalmanFilterSimple
     * @param q - Measurement noise
     * @param r - Environment noise
     * @param f - Factor of real value to previous real value
     * @param h - Factor of measured value to real value
     */
    public KalmanFilterSimple(float q, float r, float f, float h)
    {
        Q = q;
        R = r;
        F = f;
        H = h;
    }

    public void setState(float state, float covariance)
    {
        this.state = state;
        this.covariance = covariance;
    }

    public void correct(float data)
    {
        X0 = F*state;
        P0 = F*covariance*F + Q;

        float K = H * P0 / (H * P0 * H + R);
        state = X0 + K*(data - H*X0);
        covariance = (1 - K*H)*P0;
    }

}