package ceka.KFNN;

public class Kalman {
    private double q; // process noise covariance  过程噪声协方差
    private double r; // measurement noise covariance 测量噪声协方差
    private double x; // estimated value 估计值
    private double p; // estimation error covariance 估计误差协方差
    private double k; // adaptive Kalman filter gain 自适应Kalman滤波器增益
 
    public Kalman(double q, double r, double p, double initialValue) {
        this.q = q;
        this.r = r;
        this.p = p;
        this.x = initialValue;
    }
 
    public double update(double measurement, double processNoise) {
        // prediction update
        x = x + processNoise;
        p = p + q;
 
        // measurement update
        k = p / (p + r);
        x = x + k * (measurement - x);
        p = (1 - k) * p;
 
        return x;
    }
}