//-----------------------------------------------------------------------------
//
//  EKF.h++
//
//  Extended Kalman Filter Class Declaration
//
//  Tony Jebara (Copyright (C) 1997)
//
//-----------------------------------------------------------------------------

#ifndef EKF_h
#define EKF_h

#include <time.h>
#include <stdlib.h>
#include <stdio.h>
#include <stream.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <netdb.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>

extern "C" {
#include <tpm/matrix.h>
}

class EKF {

public:

  int    n;
  int    p;

  Vector Y;
  Vector Y_p;
  Vector dY;

  Vector X_e;
  Vector X;
  Vector X_p;
  Vector dX;

  Matrix I;     // Identity
  Matrix P_p;   // Error Covariance Prediction
  Matrix P_e;   // Error Covariance Estimate
  Matrix H;     // Linearized Measurement Model
  Matrix Ht;    // Linearized Measurement Model Transposed
  Matrix Q;     // Dynamics Error Covariance
  Matrix R;     // Measurement Error Covariance
  Matrix K;     // Kalman Gain Matrix
  Matrix Phi;   // State Transition Matrix (STM)
  Matrix TPxP;
  Matrix TPxN;
  Matrix TNxP;
  Matrix TNxN;
  Matrix TNxNt;

  void (*initialize)(EKF *f, void *);	           // init xp, Pp, [Q, R, H, Phi]
  void (*pre_process)(EKF *f, void *);		   // before each step (opt.)
  void (*linearize_signal_model)(EKF *f, void *);  // NULL: H(t)=H(t-1)
  void (*predict_signal)(EKF *f, void *);	   // NULL: Yp(t)=H(t)Xp(t)
  void (*estimate_state)(EKF *f, void *);	   // NULL: Xe(t)=Xp(t)+K(t)dY(t)
  void (*make_state_transition)(EKF *f, void *);   // NULL: Phi(t)=Phi(t-1)
  void (*predict_state)(EKF *f, void *);	   // NULL: Xp(t+1)=Phi(t)Xe(t)
  void (*post_process)(EKF *f, void *);		   // after each step (opt.)

  ~EKF() {destroy();}

  void   set(EKF *f);
  void   newCreate(int in_n, int in_p);
  void   setCreate(EKF *f);
  void   grow(EKF *f);
  void   destroy();
  void   init_obs(Vector &obs);
  void   clear_state();
  void   step(EKF *f);
  void   step(Vector &obs);
  void   step(EKF *f, void *param);
  void   getPred(Vector &pp)  {MatrixVectorMultiply(pp, H, X_p);}
};

#endif