observmodel.cpp

InnovLabSimu在vc++下实现

//#include "StdAfx.h"
#include "ObservModel.h"
#include <boost/numeric/ublas/vector.hpp>
using namespace std;
CObservModel::CObservModel(TRANSTATE_MAT_TYPE& m_mTranslateMatrix):m_nStateSize(4),m_mTranslateMatrix(m_mTranslateMatrix)// Linear_uncorrelated_observe_model(4,2)
{
	//for(int i=0;i<Hx.size1();i++)
	//{
	//	for (int j=0;j<Hx.size2();j++)
	//	{
	//		Hx(i,j)=0;
	//	}
	//}
	//Hx(0,0)=1;
	//Hx(1,1)=1;
	///***观测方差对角元赋值***/
	//Zv(0)=10;
	//Zv(1)=1;
	//Zv(2)=Zv(0);
	//Zv(3)=Zv(1);
	//ClearMatrix(m_mTranslateMatrix);//转移阵清0
	//m_mTranslateMatrix(0,0)=1;
	//m_mTranslateMatrix(1,1)=1;
	
}
CObservModel::CObservModel(size_t StateSize,size_t ObservSize,TRANSTATE_MAT_TYPE& m_mTranslateMatrix):m_nStateSize(StateSize),m_mTranslateMatrix(m_mTranslateMatrix)
{
	

	
}
CObservModel::~CObservModel()
{

}
void CObservModel::InitObserve(TRANSTATE_MAT_TYPE& TranslateMatrix)
{

	if (m_mTranslateMatrix.size2()==m_nStateSize)
	{
		m_mTranslateMatrix=TranslateMatrix;
	}
	else
	{
		//TRACE("Error TranslateMatrix");
	}
}
OBSERVE_STYLE& CObservModel::Update(STATE_STYLE& StateMean)
{
	m_mPredictObserve=prod(m_mTranslateMatrix,StateMean);

	return m_mPredictObserve;
}