/*---------------------------------------------------------------
	FILE: EKF.h

	Class EKF

   ISA - Universidad de Malaga - http://www.isa.uma.es

   Autor: Antonio J. Ortiz de Galisteo (AJOGD)
  ---------------------------------------------------------------*/
#ifndef EKF_H
#define EKF_H

#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/KFCapable.h>

namespace UTILS
{
	class EKF
	{
		/** This class provide a method to implement the Extended Kalman Filter easily.
		 *
		 */
    protected:
		KFCapable	*obj;

	public:
	/*Constructor
	 *
	 */
		EKF(KFCapable *o):obj(o){}

	/**	Prediction for next state in Kalman Filter
	*	Xv(k+1|k) = f(Xv(k)) <br>
	*	P(k+1\k)  = df_dxv(k|k) · P(k|k) · df_dxv(k|k)' + Q(k) <br>
	*	INPUT <br>
	*	\param	u --> matrix action
	*	OUTPUTS
	*	\param	H --> transition matrix
	*	\param	S --> estimation covariance matrix
	*	\param	R --> noise process matrix
	*	\param	h --> measure prediction matrix
	*/
		void  prediction_state_and_measure(CMatrixTemplateNumeric<float> &u, CMatrixTemplateNumeric<float> &H, CMatrixTemplateNumeric<float> &S, CMatrixTemplateNumeric<float> &h);


	/** Update for actual state in Kalman Filter <br>
	*	x(k+1|k+1) = x'(k+1|k) + K(k+1)·v(k+1) <br>
	*	p(k+1|k+1) = (I - K(k)·H(k))·P(k+1|k) <br>
	*	K --> Kalman Gain	<br>
	*	v --> Innovation (z-h) <br>
	*	INPUTS
	*	\param	H --> transition matrix[dh_dxv  dh_dy]
	*	\param	R --> noise process matrix
	*	\param	h --> measure prediction matrix
	*	\param	z --> measure matrix (observation)
	*	Internally x(k|k) and p(k|k) will be modified
	*/
		void update(CMatrixTemplateNumeric<float> &H, CMatrixTemplateNumeric<float> &R, CMatrixTemplateNumeric<float> &h, CMatrixTemplateNumeric<float> &z);

	}; // end class

} // end namespace

#endif //__EKF_H
