// EKF.h: interface for the EKF class.
//
//////////////////////////////////////////////////////////////////////

#if !defined(AFX_EKF_H__9ACAA5CD_95F2_491B_932B_8F3FD28E0001__INCLUDED_)
#define AFX_EKF_H__9ACAA5CD_95F2_491B_932B_8F3FD28E0001__INCLUDED_

#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

//#include "matrix1.h"

/*
  擴(kuò)展Kalman濾波

  狀態(tài)轉(zhuǎn)移方程:		X(k) = F*X(k-1) + G*v
  觀測方程:			Z(K) = h(X(k))  + w

*/
class SickObject;
class EKF  
{
public:
	EKF();
	EKF(EKF &ekf);
	EKF operator = (EKF ekf);
	friend SickObject;

	void SetProcessNoiseCov(double X_Acc_Cov,double Y_Acc_Cov);
	void SetObservationNoiseCov(double RadiusCov, double AzimuthCov);
	void Initialize(double Radius1,double Azimuth1,
		             double Radius2,double Azimuth2,
					 double Radius3,double Azimuth3,
					 double T1,double T2);
	void Update(double Radius,double Azimuth);
	void Predict(double T);

	CMatrix GetState();
	CMatrix GetUpdateState();
	CMatrix GetCovariance();

	virtual ~EKF();

public:
	
	CMatrix		PredictState;
	CMatrix		UpdateState;
	CMatrix		CurrentState;

	CMatrix		PredictCovariance;
	CMatrix		UpdateCovariance;
	CMatrix		CurrentCovariance;

	CMatrix		LastUpdateState;
	CMatrix		LastUpdateCov;

	CMatrix		F;
    CMatrix		G;
	CMatrix	Q;

	CMatrix	Z;
	CMatrix	H;
	CMatrix	R;

	CMatrix	S;
    CMatrix	Gain;
};

#endif // !defined(AFX_EKF_H__9ACAA5CD_95F2_491B_932B_8F3FD28E0001__INCLUDED_)