/*---------------------------------------------------------------
	FILE: HybridKF.h

	Class HybridKF

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

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

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

namespace UTILS
{
	class HybridKF
	{
    protected:
		KFCapable	*obj;
		float		k;

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

	/**	Prediction for next state in Kalman Filter
	*	Xv(k+1|k) = f(Xv(k))
	*	P(k+1\k)  = df_dxv(k|k) · P(k|k) · df_dxv(k|k)' + Q(k)
	*	INPUT
	*		u --> matrix action
	*	OUTPUTS
	*		H --> transition matrix
	*		S --> estimation covariance matrix
	*		R --> noise process matrix
	*		h --> measure prediction matrix
	*/
		void  prediction_state_and_measure(CMatrixTemplateNumeric<float> &u, CMatrixTemplateNumeric<float> &Hx, CMatrixTemplateNumeric<float> &S, CMatrixTemplateNumeric<float> &h);
/************************************************************************************************************************/
	/** Update for actual state in Kalman Filter
	*	x(k+1|k+1) = x'(k+1|k) + K(k+1)·v(k+1)
	*	p(k+1|k+1) = (I - K(k)·H(k))·P(k+1|k)
	*	K --> Kalman Gain
	*	v --> Innovation (z-h)
	*	INPUTS
	*		H --> transition matrix[dh_dxv  dh_dy]
	*		R --> noise process matrix
	*		h --> measure prediction matrix
	*		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> &zz, CVectorTemplate<float> &match);

		void coordinateHhz(CMatrixTemplateNumeric<float> &H,CMatrixTemplateNumeric<float> &h,CMatrixTemplateNumeric<float> &zz, CMatrixTemplateNumeric<float> &z, CVectorTemplate<float> &match);

	}; // end class

} // end namespace

#endif //__HybridKF_H
