/*
	KalmanFilter.hpp

	Wednesday, May 30, 2007

	Copyright(c) 2007, Smarter Than You Software.  All rights reserved.
*/

#ifndef __KalmanFilter__hpp__
#define __KalmanFilter__hpp__

// Include your definitons of matrices and vectors here
//#include "Math/TMatrixMxN.h"
//#include "Math/TVectorN.h"

namespace Soma
{
	namespace Math
	{
		class KalmanFilter
		{
			public:
				KalmanFilter();

				float getCurrentStateBestEstimate( const int stateIndex );// Returns the current stateEst of stateIndex
				void setCurrentStateBestEstimate( const int stateIndex, const float value );

				float getCurrentStateUncertainty( const int stateIndex );// Returns the current uncertainty of stateIndex

				void updateTime( const float deltaTime, const float measuredAccel );
				void updateMeasurement( const float* measurement );

				const float getTimePropUncertainty( const int index );
				const float getMeasurementPropUncertainty( const int index );

		private:
// Hoepfully you can tell the difference between vectors and matrices and that all are made of floats.
// The few pointers are so that entire matrices are not copied druing updates then refilled.
				TVectorN<3,float>* pX;//Points to Current stateVector
				TVectorN<3,float>* pXprev;//Points to Previous stateVector

				TVectorN<3,float> Xa;//stateVector
				TVectorN<3,float> Xb;//stateVector

				TMatrixMxN<3,3,float>* pP;// Points to Current Covariance matrix
				TMatrixMxN<3,3,float>* pPprev;// Points to Previous Covariance matrix
				TMatrixMxN<3,3,float> Pa;// Covariance matrix
				TMatrixMxN<3,3,float> Pb;// Covariance matrix

				TMatrixMxN<3,3,float> F;
				TMatrixMxN<3,3,float> Phi;// Depends on time
				TMatrixMxN<3,3,float> PhiTranspose;// Depends on time
				TMatrixMxN<3,2,float> G;// noise time to state space matrix
				TMatrixMxN<2,3,float> GTranspose;
				TVectorN<2,float> timePropNoiseVector;

				TVectorN<1,float> Z;//measurementVector;

				TMatrixMxN<1,3,float> H;
				TMatrixMxN<3,1,float> HTranspose;
				TVectorN<1,float> measurmentNoiseVector;

				TVectorN<3,float> K;// Gain

				void timeUpdateSetPrevious();
				void measurementUpdateSetPrevious();
		};
	}
}



#endif // #ifndef __Soma__Particles__KalmanFilter__h__