$$\phantom{******** If you see this on the webpage then the browser could not locate *********}$$
$$\phantom{******** jsMath/easy/load.js or the variable root is set wrong in this file *********}$$
$$\newcommand{\vector}[1]{\left[\begin{array}{c} #1 \end{array}\right]}$$ $$\newenvironment{matrix}{\left[\begin{array}{cccccccccc}} {\end{array}\right]}$$ $$\newcommand{\A}{{\cal A}}$$ $$\newcommand{\W}{{\cal W}}$$

/* Copyright 2008-2011 Research Foundation State University of New York   */

/* This file is part of QUB Express.                                      */

/* QUB Express is free software; you can redistribute it and/or modify    */
/* it under the terms of the GNU General Public License as published by   */
/* the Free Software Foundation, either version 3 of the License, or      */
/* (at your option) any later version.                                    */

/* QUB Express is distributed in the hope that it will be useful,         */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of         */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          */
/* GNU General Public License for more details.                           */

/* You should have received a copy of the GNU General Public License,     */
/* named LICENSE.txt, in the QUB Express program directory.  If not, see  */
/* <http://www.gnu.org/licenses/>.                                        */

/*

*/

#ifdef _WIN32
  #include <windows.h>
#else
  #include <stdlib.h>
  #define BOOL int
  #define TRUE 1
  #define FALSE 0
#endif

#include <iostream>
#include <fstream>
#include <vector>

#include "qub_kalman.h"
#include "ublas_matrixutil.h"

using std::cout;
using std::cerr;
using std::endl;



class qub_kalman_storage
{
public:
	int Np, Nm;
	pointy_matrix<double> A, Q, H, R;

	qub_kalman_storage(int Nprocess, int Nmeasure)
		: Np(Nprocess), Nm(Nmeasure), A(Nprocess, Nprocess), Q(Nprocess, Nprocess),
		H(Nmeasure, Nprocess), R(Nmeasure, Nmeasure)
	{
		fill_identity(A.m);
	}
};

extern "C" QUBFAST_API qub_kalman_const * qub_kalman_create(int Nprocess, int Nmeasure)
{
  qub_kalman_const *f = new qub_kalman_const;
  qub_kalman_storage *s = new qub_kalman_storage(Nprocess, Nmeasure);
  f->storage = s;
  f->Nprocess = Nprocess;
  f->Nmeasure = Nmeasure;
  f->A = s->A.pp;
  f->Q = s->Q.pp;
  f->H = s->H.pp;
  f->R = s->R.pp;
  return f;
}

extern "C" QUBFAST_API void qub_kalman_free(qub_kalman_const *filter)
{
  delete (qub_kalman_storage *) filter->storage;
  delete filter;
}

class qub_kalman_state_storage
{
public:
  qub_kalman_const *kalman;
  qub_kalman_storage *kst;
  pointy_col_vector<double> X, Z;
  pointy_matrix<double> P;
  pointy_col_vector<double> Xprior, ZmHx;
  pointy_matrix<double> Pprior, PpAt, PpHt, HPpHt, HPpHtRi, K, Ip, KH, ImKH;
  
  qub_kalman_state_storage(qub_kalman_const *kalman_)
    : kalman(kalman_), kst( (qub_kalman_storage *) kalman->storage ), X(kst->Np), Z(kst->Nm),
      P(kst->Np, kst->Np), Xprior(kst->Np), ZmHx(kst->Nm),
      Pprior(kst->Np, kst->Np), PpAt(kst->Np, kst->Np), PpHt(kst->Np, kst->Nm),
      HPpHt(kst->Nm, kst->Nm), HPpHtRi(kst->Nm, kst->Nm),
      K(kst->Np, kst->Nm), Ip(kst->Np, kst->Np), KH(kst->Np, kst->Np), ImKH(kst->Np, kst->Np)
  {
    fill_identity(Ip.m);
    reset();
  }
  
  qub_kalman_state_storage& operator=(const qub_kalman_state_storage& other)
  {
    X.m = other.X.m;
    Z.m = other.Z.m;
    P.m = other.P.m;
    return *this;
  }
  
  void reset()
  {
    P.m = kst->Q.m;
  }

  void next(qub_kalman_state_storage& from, double *z)
  {
		noalias(Xprior.m) = prod(kst->A.m, from.X.m);          // x- = A*x;
		noalias(PpAt.m)   = prod(from.P.m, trans(kst->A.m));
		noalias(Pprior.m) = prod(kst->A.m, PpAt.m);     // p- = A*P*At
		Pprior.m         += kst->Q.m;                   //      + Q;
		
		noalias(PpHt.m)   = prod(Pprior.m, trans(kst->H.m));
		noalias(HPpHt.m)  = prod(kst->H.m, PpHt.m);
		HPpHt.m          += kst->R.m;
		if ( kst->Nm > 1 )
			sv_invm(HPpHt.pp, kst->Nm, HPpHtRi.pp);         // inv(H*P-*Ht + R)
		else
			HPpHtRi.p[0] = 1.0 / HPpHt.p[0];
		noalias(K.m)    = prod(PpHt.m, HPpHtRi.m); // K = P-*Ht * inv(HP-Ht + R);

		noalias(ZmHx.m) = prod(kst->H.m, Xprior.m);
		for (int i=0; i<kst->Nm; ++i)
			ZmHx.p[i] = z[i] - ZmHx.p[i];          // (Z - H*Xp)
		noalias(X.m)    = prod(K.m, ZmHx.m);
		for (int i=0; i<kst->Np; ++i)
			X.p[i]   += Xprior.p[i];               // X = Xp + K(Z - HXp);

		noalias(KH.m)   = prod(K.m, kst->H.m);
		ImKH.m          = Ip.m - KH.m;
		noalias(P.m)    = prod(ImKH.m, Pprior.m);  // P = (I - KH)Pp;

		noalias(Z.m)    = prod(kst->H.m, X.m);          // Z = HX;
	}
};

extern "C" QUBFAST_API qub_kalman_state * qub_kalman_state_create(qub_kalman_const *kalman)
{
	qub_kalman_state *f = new qub_kalman_state;
	qub_kalman_state_storage *s = new qub_kalman_state_storage(kalman);
	f->storage = s;
	f->kalman = kalman;
	f->X = s->X.p;
	f->Z = s->Z.p;
	f->P = s->P.pp;
	return f;
}

extern "C" QUBFAST_API void qub_kalman_state_free(qub_kalman_state *state)
{
	if ( state )
		delete (qub_kalman_state_storage*) state->storage;
	delete state;
}

extern  "C" QUBFAST_API void qub_kalman_state_copy(qub_kalman_state *into, qub_kalman_state *from)
{
  * (qub_kalman_state_storage *) into->storage = * (qub_kalman_state_storage *) from->storage;
}

extern "C" QUBFAST_API void qub_kalman_state_reset(qub_kalman_state *state)
{
	((qub_kalman_state_storage*) (state->storage))->reset();
}

extern "C" QUBFAST_API void qub_kalman_state_next(qub_kalman_state *state, double *z)
{
  qub_kalman_state_storage *st = (qub_kalman_state_storage*) state->storage;
  st->next(*st, z);
}

extern "C" QUBFAST_API void qub_kalman_state_next_into(qub_kalman_state *into, qub_kalman_state *from, double *z)
{
  qub_kalman_state_storage *st_into = (qub_kalman_state_storage*) into->storage;
  qub_kalman_state_storage *st_from = (qub_kalman_state_storage*) from->storage;
  st_into->next(*st_from, z);
}

extern "C" QUBFAST_API void qub_kalman_filter(qub_kalman_state *state, int N, double *z, double *p)
{
  qub_kalman_state_storage *st = (qub_kalman_state_storage*) state->storage;
  for (int i=0; i<N; ++i) {
    st->next(*st, z+i);
    z[i] = state->Z[0];
    if ( p )
      p[i] = state->P[0][0];
  }
}