

#ifndef  __INC_KALMAN__
#define  __INC_KALMAN__


#include <matrix.h>

typedef enum { KalmExtrapolate =1, KalmUpdate, KalmNew, KalmInit } KalmState;

typedef struct {
  matrix phi;                                /* system model */
  matrix Q;                                  /* system noise */
  matrix H;                                  /* measurement model */

  matrix K;                                  /* kalman gain */

  matrix P;                                  /* covariance estimate */
  matrix xhat;                               /* state estimate */
  matrix zhat;                               /* H * xhat */
  
  int k;                                     /* time */
  KalmState state;                           /* last op = EXTRAPOLATE or UPDATE? */
} *KalmanFilter;



extern KalmanFilter kalmNew(matrix phi, matrix Q, matrix H);
extern void         kalmFree(KalmanFilter kf);

extern KalmanFilter kalmClone(KalmanFilter kf);

extern void         kalmInitialize(KalmanFilter kf, matrix b, matrix M);
extern void         kalmExtrapolate(KalmanFilter kf);
extern void         kalmUpdate(KalmanFilter kf, matrix z, matrix R);
extern double       kalmWeighCand(KalmanFilter kf, matrix z, matrix R);


#endif   
