// KalmanFilter.cpp : Defines the class behaviors for the application.
//

#include "stdafx.h"
#include "KalmanFilter.h"
#include "KalmanFilterDlg.h"
#include "math.h"
#include "myStruct.h"
#include "Matrix.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif


/////////////////////////////////////////////////////////////////////////////
// CKalmanFilterApp

BEGIN_MESSAGE_MAP(CKalmanFilterApp, CWinApp)
	//{{AFX_MSG_MAP(CKalmanFilterApp)
		// NOTE - the ClassWizard will add and remove mapping macros here.
		//    DO NOT EDIT what you see in these blocks of generated code!
	//}}AFX_MSG
	ON_COMMAND(ID_HELP, CWinApp::OnHelp)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CKalmanFilterApp construction

CKalmanFilterApp::CKalmanFilterApp()
{
	// TODO: add construction code here,
	// Place all significant initialization in InitInstance
}

/////////////////////////////////////////////////////////////////////////////
// The one and only CKalmanFilterApp object

CKalmanFilterApp theApp;

/////////////////////////////////////////////////////////////////////////////
// CKalmanFilterApp initialization

BOOL CKalmanFilterApp::InitInstance()
{
	AfxEnableControlContainer();

	// Standard initialization
	// If you are not using these features and wish to reduce the size
	//  of your final executable, you should remove from the following
	//  the specific initialization routines you do not need.

#ifdef _AFXDLL
	Enable3dControls();			// Call this when using MFC in a shared DLL
#else
	Enable3dControlsStatic();	// Call this when linking to MFC statically
#endif

	CKalmanFilterDlg dlg;
	m_pMainWnd = &dlg;
	int nResponse = dlg.DoModal();
	if (nResponse == IDOK)
	{
		// TODO: Place code here to handle when the dialog is
		//  dismissed with OK
	}
	else if (nResponse == IDCANCEL)
	{
		// TODO: Place code here to handle when the dialog is
		//  dismissed with Cancel
	}

	// Since the dialog has been closed, return FALSE so that we exit the
	//  application, rather than start the application's message pump.
	return FALSE;
}


BOOL CKalmanFilter::InitKalman(Kalman &kalman,Matrix stateInit,Matrix covInit, Matrix transitionMatrix, 
							   Matrix measureMatrxi,Matrix processNoiseCov, Matrix measureNoiseCov,
							   Matrix controlMatrix)
{
	if (stateInit.RowCount()!=kalman.DP||measureNoiseCov.RowCount()!=kalman.MP)
	{
		return false;
	}
	if (!positive(processNoiseCov)||!positive(measureNoiseCov))
	{
		return false;
	}
	kalman.state_post=stateInit;
	kalman.error_cov_post=covInit;
	kalman.transition_matrix=transitionMatrix;
	kalman.measurement_matrix=measureMatrxi;
	kalman.process_noise_cov=processNoiseCov;
	kalman.measurement_noise_cov=measureNoiseCov;
	if (kalman.CP>0)
	{
		kalman.control_matrix=controlMatrix;
		return true;
	}
	else
	{
		return false;
	}
	

}

BOOL CKalmanFilter::InitKalman(Kalman &kalman,Matrix stateInit,Matrix covInit, Matrix transitionMatrix, 
							   Matrix measureMatrxi, Matrix processNoiseCov, Matrix measureNoiseCov)
							   
{
	if (stateInit.RowCount()!=kalman.DP||measureNoiseCov.RowCount()!=kalman.MP)
	{
		return false;
	}
	if (!positive(processNoiseCov)||!positive(measureNoiseCov))
	{
		return false;
	}
	kalman.state_post=stateInit;
	kalman.error_cov_post=covInit;
	kalman.transition_matrix=transitionMatrix;
	kalman.process_noise_cov=processNoiseCov;
	kalman.measurement_noise_cov=measureNoiseCov;
	kalman.measurement_matrix=measureMatrxi;
		
	return true;

}


CKalmanFilter::CKalmanFilter(int MP,int DP,int CP)
{
	if (DP<=0||MP<=0)
	{
		AfxMessageBox("state and measurement vectors must have positive number of dimensions!");
		exit(0);
	}
	kalman.DP=DP;
	kalman.CP=CP;
	kalman.MP=MP;
	kalman.state_pre=kalman.state_pre.Zeros(kalman.DP,1);
	kalman.state_post=kalman.state_post.Zeros(kalman.DP,1);
	kalman.transition_matrix=kalman.transition_matrix.Zeros(kalman.DP,kalman.DP);
	kalman.process_noise_cov=kalman.process_noise_cov.Zeros(kalman.DP,kalman.DP);
	kalman.measurement_matrix=kalman.measurement_matrix.Zeros(kalman.MP,kalman.DP);
	kalman.measurement_noise_cov=kalman.measurement_noise_cov.Zeros(kalman.MP,kalman.MP);
	kalman.error_cov_pre=kalman.error_cov_pre.Zeros(kalman.DP,kalman.DP);
	kalman.error_cov_post=kalman.error_cov_post.Zeros(kalman.DP,kalman.DP);
	kalman.gain=kalman.gain.Zeros(kalman.DP,kalman.MP);
	if (kalman.CP>0)
	{
		kalman.control_matrix=kalman.control_matrix.Zeros(kalman.DP,kalman.CP);
	}
	kalman.temp1=kalman.temp1.Zeros(kalman.DP,kalman.DP);
	kalman.temp2=kalman.temp2.Zeros(kalman.MP,kalman.DP);
	kalman.temp3=kalman.temp3.Zeros(kalman.MP,kalman.MP);
	kalman.temp4=kalman.temp4.Zeros(kalman.MP,kalman.DP);
	kalman.temp5=kalman.temp5.Zeros(kalman.MP,1);
}

Matrix CKalmanFilter::KalmanPredict(Kalman &kalman,const Matrix control)
{
    Matrix result;

    /* update the state */

    /* x'(k) = x'(k) + B*u(k) */
	kalman.state_pre=(kalman.transition_matrix)*(kalman.state_post)+(kalman.control_matrix)*(control);
    /* update error covariance matrices */
    /* temp1 = A*P(k) */
    kalman.temp1=(kalman.transition_matrix)*(kalman.error_cov_post);
    /* P'(k) = temp1*At + Q */
	kalman.error_cov_pre=(kalman.temp1)*conj(kalman.transition_matrix)+(kalman.process_noise_cov);
    result = kalman.state_pre;

    return result;
}

Matrix CKalmanFilter::KalmanPredict(Kalman  &kalman)
{
    Matrix result;

    /* update the state */
    /* x'(k) = A*x(k) */
	kalman.state_pre=(kalman.transition_matrix)*(kalman.state_post);

    /* update error covariance matrices */
    /* temp1 = A*P(k) */
    kalman.temp1=(kalman.transition_matrix)*(kalman.error_cov_post);
    /* P'(k) = temp1*At + Q */
	kalman.error_cov_pre=(kalman.temp1)*conj_tran(kalman.transition_matrix)+(kalman.process_noise_cov);
    result = kalman.state_pre;

    return result;
}


Matrix CKalmanFilter::KalmanUpdate(Kalman &kalman, const Matrix measurement)
{
	Matrix result ;
	int meaRowCount=measurement.RowCount();
	int meaColCount=measurement.ColCount();
	Matrix meaTemp;
	meaTemp=meaTemp.Zeros(meaRowCount,1);
	for (int i=0;i<meaRowCount;i++)
	{
		meaTemp(i,0)=measurement(i,meaColCount-1);
	}
    /* temp2 = H*P'(k) */
	kalman.temp2=(kalman.measurement_matrix)*(kalman.error_cov_pre);
	TRACE("measureMatrix:%f,%f",kalman.measurement_matrix(0,0),kalman.measurement_matrix(0,1));
    /* temp3 = temp2*Ht + R */
	kalman.temp3=(kalman.temp2)*conj_tran(kalman.measurement_matrix)+(kalman.measurement_noise_cov);

    /* temp4 = inv(temp3)*temp2 = Kt(k) */
	kalman.temp4=inv(kalman.temp3)*(kalman.temp2);
    /* K(k) */
    kalman.gain=conj_tran(kalman.temp4);

    /* temp5 = z(k) - H*x'(k) */
	kalman.temp5=meaTemp-(kalman.measurement_matrix)*(kalman.state_pre);

    /* x(k) = x'(k) + K(k)*temp5 */
	kalman.state_post=(kalman.state_pre)+(kalman.gain)*(kalman.temp5);

    /* P(k) = P'(k) - K(k)*temp2 */
	kalman.error_cov_post=(kalman.error_cov_pre)-(kalman.gain)*(kalman.temp2);
	
    result = kalman.state_post;

    return result;
}


Matrix CKalmanFilter::GetInitState(Matrix measurement,double timeStep)
{
	Matrix stateInit;
	stateInit=stateInit.Zeros(2,1);
	stateInit(0,0)=measurement(0,0);
	stateInit(1,0)=(measurement(0,1)-measurement(0,0))/timeStep;
	return stateInit;
}


Matrix CKalmanFilter::GetInitCov(Matrix measurementNoise,double timeStep)
{
	Matrix covInit;
	covInit=covInit.Zeros(2,2);
	covInit(0,0)=measurementNoise(0,0);
	covInit(0,1)=(measurementNoise(0,0))/timeStep;
	covInit(1,0)=(measurementNoise(0,0))/timeStep;
	covInit(1,1)=(measurementNoise(0,0))/timeStep/timeStep*2;
	return covInit;
}