package com.wytehat.btlogger;

public class RssiKalmanFilter {
    private final double processNoise;
    private final double measurementNoise;
    private double estimate;
    private double covariance = 1.0;
    private boolean initialized;

    public RssiKalmanFilter() { this(0.125, 0.8); }

    public RssiKalmanFilter(double processNoise, double measurementNoise) {
        this.processNoise = processNoise;
        this.measurementNoise = measurementNoise;
    }

    public synchronized double update(int rawRssi) {
        if (!initialized) {
            estimate = rawRssi;
            initialized = true;
            return estimate;
        }
        double priorCovariance = covariance + processNoise;
        double gain = priorCovariance / (priorCovariance + measurementNoise);
        estimate = estimate + gain * (rawRssi - estimate);
        covariance = (1.0 - gain) * priorCovariance;
        return estimate;
    }

    public synchronized void reset() {
        initialized = false;
        covariance = 1.0;
    }
}
