using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace GKalman
{
    public class GKalman
    {
        double _err_measure = 0.0F;
        double _err_estimate = 0.0F;
        double _q = 0.0F;
        double _last_estimate = 0.0F;
        public GKalman(double mea_e, double est_e, double q)
        {
            setParameters(mea_e, est_e, q);
        }

        // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
        public GKalman(double mea_e, double q)
        {
            setParameters(mea_e, mea_e, q);
        }

        // разброс измерения, разброс оценки, скорость изменения значений
        public void setParameters(double mea_e, double est_e, double q)
        {
            _err_measure = mea_e;
            _err_estimate = est_e;
            _q = q;
        }

        // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
        public void setParameters(double mea_e, double q)
        {
            setParameters(mea_e, mea_e, q);
        }

        // возвращает фильтрованное значение
        public double filtered(double value)
        {
            double _kalman_gain, _current_estimate;
            _kalman_gain = _err_estimate / (_err_estimate + _err_measure);
            _current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate);
            _err_estimate = (1.0 - _kalman_gain) * _err_estimate + Math.Abs(_last_estimate - _current_estimate) * _q;
            _last_estimate = _current_estimate;
            return (double)_current_estimate;
        }
    }
}