//FilterKalman.cs

namespace FusionFiltering
{

    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(double R, double Q, double A, double B, double H)
        {
            this.R = R;  //過程噪聲 
            this.Q = Q;  //測量噪聲

            this.A = A;  //狀態(tài)轉(zhuǎn)移矩陣
            this.B = B;  //控制矩陣  u為控制向量
            this.H = H;  //將估計(jì)范圍與單位轉(zhuǎn)化為與系統(tǒng)變量(或者說測量值)一致的范圍與單位

            this.cov = double.NaN;
            this.x = double.NaN; // estimated signal without noise
        }

        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;

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

                // Correction
                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;

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

                // Correction
                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;
        }
    }
}

//ProgramTestData.cs

using System;
using System.Linq;

namespace FusionFiltering
{
    public class ProgramTest
    {
        /// <summary>
        /// kalman濾波測試1
        /// </summary>
        [System.Diagnostics.Conditional("DEBUG")]
        public static void TestKalmanFilter1()
        {
            Console.WriteLine("FilterKalman Usage");

            FilterKalman test = new FilterKalman(0.008, 0.1);
            double[] testData = { 66, 64, 63, 63, 63, 66, 65, 67, 58 };
            foreach (var x in testData) {
                Console.WriteLine("Input data: {0:#,##0.00}, Filtered data:{1:#,##0.000}", x, test.filter(x));
            }
        }

        /// <summary>
        /// Example Usage with controlled input
        /// </summary>
        [System.Diagnostics.Conditional("DEBUG")]
        public static void TestKalmanFilterWithControlled()
        {
            Console.WriteLine("FilterKalman Usage with controlled input");

            FilterKalman test = new FilterKalman(0.008, 0.1, 1, 1, 1);
            double[] testData = { 66, 64, 63, 63, 63, 66, 65, 67, 58 };
            double u = 0.2;
            foreach (var x in testData) {
                Console.WriteLine("Input data: {0:#,##0.00}, Filtered data:{1:#,##0.000}", x, test.filter(x, u));
            }

        }
    }
}

//Program.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using FusionFiltering;
namespace ConsoleApplication1
{
    class Program
    {
        static void Main(string[] args)
        {

            ProgramTest.TestKalmanFilter1();
            Console.ReadKey();
            Console.WriteLine();
            ProgramTest.TestKalmanFilterWithControlled();
            Console.ReadKey();

        }
}
}