ukf_comman_head.h

InnovLabSimu在vc++下实现

#pragma once
/********************************************************************
	created:	2008/01/03
	created:	3:1:2008   15:39
	filename: 	C:\Documents and Settings\GaoYang\Local Settings\Temp\~vs7D.h
	file path:	C:\Documents and Settings\GaoYang\Local Settings\Temp
	file base:	~vs7D
	file ext:	h
	author:		GaoYang
	
	purpose:	UKF中共同用到的一些设置及少量函数
*********************************************************************/
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "BOOST_DEBUG_TOOL.h"
//#include "PredictModel.h"
//#include "ObservModel.h"
//#define DEBUG_USE_FLAG//定义则表示使用观察矩阵内容的调试工具
#define USE_DEGREE_ANGLE//定义则表示使用的状态变量中角度单位为度
//#define DEBUG_ON_VC_OUT_PUT_MORE
#define OUT_PUT_OBSERVE_ONLY
#define CONSIDER_TIME_IN_VEL//定义计算速度时必须考虑时间因素
#define USE_TRADITIONAL_AKF //使用传统的方法调整

using namespace boost::numeric::ublas;


typedef matrix<double> SIGMA_POINTS;
typedef matrix<double> SIGMA_POINT_WEIGHT;///<权值矩阵
typedef double PARM;
typedef boost::numeric::ublas::vector<double> STATE_STYLE;
typedef matrix<double> VARIANCE;
typedef matrix<double> matrix_type;

typedef matrix <double> TRANSTATE_MAT_TYPE;
typedef boost::numeric::ublas::vector<double> OBSERVE_DATA_STYLE;
//OBSERVE_STYLE
template <typename MAT_TYPE>
void ClearMatrix(MAT_TYPE& A)
{
	for (int i=0;i<A.size1();i++)
	{
		for (int j=0;j<A.size2();j++)
		{
			A(i,j)=0;
		}		
	}
}
template <typename MAT_TYPE>
void MatElemABS(MAT_TYPE& A)
{
	for (int i=0;i<A.size1();i++)
	{
		for (int j=0;j<A.size2();j++)
		{
			A(i,j)=fabs(A(i,j));
		}		
	}
}
//矩阵快速求逆
template <typename MAT_TYPE>
float CalMatrixReverse(MAT_TYPE& mIn,MAT_TYPE&mOut)
{
	/*float *m=new float [RowSize*lineSize];*/
	MAT_TYPE m(mIn);
	size_t RowSize=m.size1();
	size_t lineSize=m.size2();
	int* is=new int [RowSize];//权当维数为行数了，不确定矩阵应申明多大 
	int* js=new int [lineSize]; //权当维数为行数了，不确定矩阵应申明多大 
	float fDet = 1.0f; 
	int f = 1; 
	int i=0;
	int k = 0;
	int j = 0;
	for ( k = 0; k < RowSize; k ++) //k<多少补知道
	{ 
		// 第一步，全选主元 
		float fMax = 0.0f; 
		for (i = k; i < RowSize; i ++) 
		{ 
			for (int j = k; j < lineSize; j ++) 
			{
				const float f = fabs(m(i, j));
				if (f > fMax)
				{					
					fMax	= f;
					is[k]	= i;
					js[k]	= j;
				}

			}
		}
		if (fabs(fMax) < 0.0001f)
		{
			return 0;
		}
		if (is[k] != k)
		{
			f = -f;
			for (int jj=0;jj<lineSize;jj++)
			{
				std::swap(m(k,jj), m(is[k],jj));
			}
			// 			swap(m[k][1], m[is[k]][1]);
			// 			swap(m[k][2], m[is[k]][2]);
			//swap(m(k, 3), m(is[k], 3));
		}
		if (js[k] != k)
		{
			f = -f;
			for(int ii=0;ii<RowSize;ii++)
			{
				std::swap(m(ii,k), m(ii,js[k]));
			}			
		}
		// 计算行列值
		fDet *= m(k,k);
		// 计算逆矩阵
		// 第二步
		m(k,k) = 1.0f / m(k,k);	
		// 第三步
		for (int j = 0; j < lineSize; j ++)//这里不确定是否lineSize。但对矩阵而言没有影响
		{
			if (j != k)
				m(k,j) *= m(k,k);
		}
		// 第四步
		for (i = 0; i < RowSize; i ++)
		{
			if (i != k)
			{
				for	(j = 0; j < lineSize; j ++)
				{
					if (j != k)
						m(i,j) = m(i,j) - m(i,k) * m(k,j);				
				}
			}
		}
		// 第五步
		for (i = 0; i < RowSize; i ++)
		{
			if (i != k)
				m(i,k) *= -m(k,k);
		}
	}
	for	(k = RowSize-1; k >= 0; k --)
	{
		if (js[k] != k)
		{
			for (int jj=0;jj<lineSize;jj++)
			{
				std::swap(m(k,jj), m(js[k],jj));
			}			
			// 			swap(m[k][1], m[js[k]][1]);
			// 			swap(m[k][2], m[js[k]][2]);
			//swap(m(k, 3), m(js[k], 3));
		}
		if (is[k] != k)
		{	
			for(int ii=0;ii<RowSize;ii++)
			{
				std::swap(m(ii,k), m(ii,is[k]));
			}
			// 			swap(m[0][k], m[0][is[k]]);
			// 			swap(m[1][k], m[1][is[k]]);
			// 			swap(m[2][k], m[2][is[k]]);
			//swap(m(3, k), m(3, is[k]));
		}
	}
	mOut = m;
	delete[] is;
	delete[] js;
	//memcpy(pResultMatrix,&m[0][0],sizeof(m));//错误，不能这么用
	//return m;
	return fDet * f;
}