initstateofukf.cpp

InnovLabSimu在vc++下实现

//#include "StdAfx.h"
#include "InitStateOfUKF.h"

CInitStateOfUKF::CInitStateOfUKF(void):m_sInitialStateVal(4), m_vInitStateCov(4,4), m_vInitControlNoise(2,2), m_vInitModelNoise(4,4), m_vInitObserveNoise(2,2)
{
}

CInitStateOfUKF::~CInitStateOfUKF(void)
{
}
CInitStateOfUKF::CInitStateOfUKF(size_t StateSize,size_t ObservSize,size_t ControlSize):m_sInitialStateVal(StateSize),m_vInitStateCov(StateSize,StateSize),
m_vInitControlNoise(ControlSize,ControlSize),m_vInitModelNoise(StateSize,StateSize),m_vInitObserveNoise(ObservSize,ObservSize)
{

}
void CInitStateOfUKF::InitStateAndCov(double sDis,double sAngle,double sDv,double sAv,
									  double vCovDis,double vCovAng,double vCovDv,double vCovAv,
									  bool IsDegree)
{
	///////////设状态、及方差初值///////////////////////////////////////////////////////////////

	m_sInitialStateVal(0)=sDis;
	m_sInitialStateVal(2)=sDv;
	if (IsDegree)
	{
		m_sInitialStateVal(1)=TransAngleToDegree(sAngle);
		m_sInitialStateVal(3)=TransAngleToDegree(sAv);
	}	
	else
	{
		m_sInitialStateVal(1)=(sAngle);
		m_sInitialStateVal(3)=(sAv);
	}
	ClearMatrix(m_vInitStateCov);
	m_vInitStateCov(0,0)=vCovDis;
	m_vInitStateCov(2,2)= vCovDv;
	if (IsDegree)
	{
		m_vInitStateCov(1,1)= TransAngleToDegree(vCovAng);
		m_vInitStateCov(3,3)= TransAngleToDegree(vCovAv);
	}
	else
	{
		m_vInitStateCov(1,1)= vCovAng;
		m_vInitStateCov(3,3)= vCovAv;
	}	
	/*for (int i=0;i<m_vInitStateCov.size1();i=i+2)
	{
		m_vInitStateCov(i,i)=100;
		m_vInitStateCov(i+1,i+1)=1;
	}*/
}
void CInitStateOfUKF::InitNoiseVar(double vControlNoiseDl,double vControlNoiseDr,
								   double vMoNoisDis,double vModNoisAng,double vModNoisDv,double vModNoisAv,
								   double vObserveNoisDis,double vObserveNoisAng,
								   bool IsDegree)
{
	///////////设控制噪声方差阵及模型噪声方差阵///////////////////////////////////////////////////////////////
	
	ClearMatrix(m_vInitControlNoise);
	m_vInitControlNoise(0,0)=vControlNoiseDl;
	m_vInitControlNoise(1,1)=vControlNoiseDr;

	ClearMatrix(m_vInitModelNoise);
	m_vInitModelNoise(0,0)=vMoNoisDis;
	m_vInitModelNoise(2,2)=vModNoisDv;
	if (IsDegree)
	{
		m_vInitModelNoise(1,1)=TransAngleToDegree(vModNoisAng);
		m_vInitModelNoise(3,3)=TransAngleToDegree(vModNoisAv);
	}
	else
	{
		m_vInitModelNoise(1,1)=vModNoisAng;
		m_vInitModelNoise(3,3)=vModNoisAv;
	}
	
	////////////设观测量方差阵2X2//////////////////////////////////////////////////////////////
	ClearMatrix(m_vInitObserveNoise);
	m_vInitObserveNoise(0,0)=vObserveNoisDis;
	if (IsDegree)
	{
		m_vInitObserveNoise(1,1)=TransAngleToDegree(vObserveNoisAng);
	}
	else
	{
		m_vInitObserveNoise(1,1)=(vObserveNoisAng);
	}
	
	
}