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

namespace IBox.WinFrmUI
{
    public class FilterKalman
    {
        private double A = 1;
        private double B = 0;
        private double H = 1;

        private double R;
        private double Q;

        private double cov = double.NaN;
        private double x = double.NaN;
        public FilterKalman()
        {

        }
        /// <summary>
        /// 卡尔曼滤波
        /// </summary>
        public FilterKalman(double R, double Q, double A, double B, double H)
        {
            this.R = R;  
            this.Q = Q;  

            this.A = A; 
            this.B = B; 
            this.H = H;  

            this.cov = double.NaN;
            this.x = double.NaN;
        }

        /// <summary>
        /// 
        /// </summary>
        public FilterKalman(double R, double Q)
        {
            this.R = R;
            this.Q = Q;
        }

        public double filter(double measurement, double u)
        {
            if (double.IsNaN(this.x))
            {
                this.x = (1 / this.H) * measurement;
                this.cov = (1 / this.H) * this.Q * (1 / this.H);
            }
            else
            {
                double predX = (this.A * this.x) + (this.B * u);
                double predCov = ((this.A * this.cov) * this.A) + this.Q;

                double K = predCov * this.H * (1 / ((this.H * predCov * this.H) + this.Q));

                this.x = predX + K * (measurement - (this.H * predX));
                this.cov = predCov - (K * this.H * predCov);
            }
            return this.x;
        }

        public double filter(double measurement)
        {
            double u = 0;
            if (double.IsNaN(this.x))
            {
                this.x = (1 / this.H) * measurement;
                this.cov = (1 / this.H) * this.Q * (1 / this.H);
            }
            else
            {
                double predX = (this.A * this.x) + (this.B * u);
                double predCov = ((this.A * this.cov) * this.A) + this.R;

                double K = predCov * this.H * (1 / ((this.H * predCov * this.H) + this.Q));

                this.x = predX + K * (measurement - (this.H * predX));
                this.cov = predCov - (K * this.H * predCov);
            }
            return this.x;
        }

        public double lastMeasurement()
        {
            return this.x;
        }

        public void setMeasurementNoise(double noise)
        {
            this.Q = noise;
        }

        public void setProcessNoise(double noise)
        {
            this.R = noise;
        }
    }
}