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

namespace WindowsFormsApplication1
{
    class Kalman
    {
        public Kalman(double q1, double r1, Matrix F1)
        {
            Q = q1;
            R = r1;
            F = F1;

            G = new Matrix(3, 1);
            G.Mbody[2, 0] = 1;
            H = new Matrix(1, 3);
            H.Mbody[0, 0] = 1;
            
            Xkk = new Matrix(3, 1);
            Xk = new Matrix(3, 1);
            Pkk = new Matrix(3, 3);
            Pk = new Matrix(3, 3);
            Pk = Matrix.MI(3, 3);
            Kk = new Matrix(3, 1);
            this.size = 3;
        }

        int size;

        public Kalman(double q1, double r1, Matrix F1, int n, double p)
        {
            Q = q1;
            R = r1;
            F = F1;
            this.size = n;

            G = new Matrix(n, 1);
            G.Mbody[n-1, 0] = 1;
            H = new Matrix(1, n);
            H.Mbody[0, 0] = 1;
            
            Xkk = new Matrix(n, 1);
            Xk = new Matrix(n, 1);
            Pkk = new Matrix(n, n);
            Pk = new Matrix(n, n);
            Pk = Matrix.MI(n, n) * p;
            Kk = new Matrix(n, 1);
        }
        public Matrix F { set; get; }

        Matrix G;

        Matrix H;

        public double Q { set; get; }
        public double R { set; get; }
        Matrix Xkk; 
        public Matrix Xk;
        Matrix Pkk;
        Matrix Pk; 
        Matrix Kk; 

        public void Next(double z)
        {
            Xkk = F * Xk;
            Pkk = F * Pk * Matrix.T(F) + G * Q * Matrix.T(G);
            Kk = Pkk * Matrix.T(H) * (1/(Matrix.ToDouble(H * Pkk * Matrix.T(H)) + R));
            Xk = Xkk + Kk * (z - Matrix.ToDouble(H * Xkk));
            Pk = (Matrix.MI(size, size) - Kk * H) * Pkk;
        }
    }

}