/*---------------------------------------------------------------
	FILE: UKF.h

	Class UKF

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

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

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

namespace UTILS
{
	class UKF
	{
		/** This class provide a method to implement the Unscented Kalman Filter easily.
		 *
		 */
    protected:
		KFCapable									*obj;
		int											na;			//(2·na+1) is the number of sample points
		float										k;
		CVectorTemplate<float>						Wi;			//Weights for sample points
		CMatrixTemplateNumeric<float>				Xi;			//Sample points
		CMatrixTemplateNumeric<float>				fv_Xi;		//Prediction for sample points
		CMatrixTemplateNumeric<float>				h_Xi;		//Observation for sample points
		CMatrixTemplateNumeric<float>				h_predicted;//Mean of measured predicted

	public:
	/*Constructor
	 *
	 */
		UKF(KFCapable *o, float k_in):obj(o), k(k_in){}

		/**	Prediction for next state in Kalman Filter (prediction model)
		*	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
		*/
		void  StatePrediction(CMatrixTemplateNumeric<float> &u);

		/**	Prediction of measurements Kalman Filter (observation model)
		*	OUTPUTS
		*	\param	S --> estimation covariance matrix
		*	\param	h --> measure prediction matrix
		*/
		void  PredictedMeasurements(CMatrixTemplateNumeric<float> &h_final, CMatrixTemplateNumeric<float> &S);//Unscented innovation matrix

		/** 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  match --> vector to determine which measurements are agree respect prediction
	*	\param	R --> noise process matrix
	*	\param	z --> measure matrix (observation)
	*	Internally x(k|k) and p(k|k) will be modified
	*/
		void  Update(CVectorTemplate<float> &match, CMatrixTemplateNumeric<float> &R, CMatrixTemplateNumeric<float> &z);

	/** This function not be used be users. Delete the matrix elements which are not agree with the measurements or previous predictions.
	 *
	 */
		void  coordinate_hXi();

	}; // end class

} // end namespace

#endif //__UKF_H
