beaconsimu.cpp

来自「VC&Matlab混合编程实现无线电导航指示器」· C++ 代码 · 共 402 行

CPP
402 行
字号 
// BeaconSimu.cpp: implementation of the CBeaconSimu class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "radio.h"
#include "BeaconSimu.h"
#include "Traj3DShow.h"
#include "matlab.hpp"
#include "APPStatic.h"


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

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CBeaconSimu::CBeaconSimu(CBeaconManage* pBeaconManage)
{
	m_pBeaconManage=pBeaconManage;
	m_dAzMeasure=0.0;
	m_dDisMeasure=0.0;
	m_bNewBeacon=TRUE;
}


CBeaconSimu::~CBeaconSimu()
{

}

BOOL CBeaconSimu::GetNearestBeaconIndex(TRAJ curTraj,UINT *pIndex)
//pIndex输出，最近的信标Index
//没用信标有效返回FALSE,否则返回TRUE
{
	if(m_pBeaconManage==NULL || m_pBeaconManage->GetBeaconList()==NULL || m_pBeaconManage->GetBeaconList()->GetSize()==0) return FALSE;
	
	UINT iIndex=0;
    BOOL bFound=FALSE;
	CArray<BEACON,BEACON>* pBeaconList;
	BEACON beacon;
	double prevDis=1.0e20;
	*pIndex=-1;//not found
    POINT3 posPlane={curTraj.lon,curTraj.lat,curTraj.heg};
    pBeaconList=m_pBeaconManage->GetBeaconList();
	for(int i=0;i<=pBeaconList->GetSize()-1;i++){
		beacon=pBeaconList->GetAt(i);
		double dis,los;
		switch(beacon.type){
		case AIR:
			dis=CalDistance(posPlane,beacon.pos);
			los=CalLosAngle(posPlane,beacon.pos);
			if(dis<m_pBeaconManage->m_dDistAir*1000.0 && dis<prevDis && los>=m_pBeaconManage->m_dBlindAir){
				bFound=TRUE;
				prevDis=dis;
				iIndex=i;
				*pIndex=iIndex;
			}
			break;
		case GROUND:
			dis=CalDistance(posPlane,beacon.pos);
			los=CalLosAngle(posPlane,beacon.pos);
			if(dis<m_pBeaconManage->m_dDistGround*1000.0 && dis<prevDis && los>=m_pBeaconManage->m_dBlindGround){
				bFound=TRUE;
				prevDis=dis;
				iIndex=i;
				*pIndex=iIndex;
				break;
		default:
			return FALSE;
			}
			
		}
	}
	//Found
	if(iIndex!=m_pBeaconManage->m_iAvailableBCIndex){
		m_bNewBeacon=TRUE;
	}else{
		m_bNewBeacon=FALSE;
	}
	return bFound;
}

double CBeaconSimu::CalDistance(POINT3 posPlane,POINT3 posBeacon){
	//计算两点距离:(la,phi,h),(度，度，米)

	double la1=CTraj3DShow::C2Rad(posPlane.x,0,0);
	double phi1=CTraj3DShow::C2Rad(posPlane.y,0,0);
	double h1=posPlane.z;

	double la2=CTraj3DShow::C2Rad(posBeacon.x,0,0);
	double phi2=CTraj3DShow::C2Rad(posBeacon.y,0,0);
	double h2=posBeacon.z;

	double e=0.081819191,Re=6378137.0;
    mwArray Rm=mwArray(Re)*mwArray(1-e*e)/power(sqrt((1-e*e*sin(phi1)*sin(phi1))),mwArray(3));
    mwArray C1=1/(Rm+h1);//纬度比
    mwArray Rn=Re/sqrt((1-e*e*sin(phi1)*sin(phi1)));
    mwArray C2=sec(phi1)/(Rn+h1);//经度比
	
	mwArray mwDis=sqrt((phi1-phi2)*(phi1-phi2)/(C1*C1)+(la1-la2)*(la1-la2)/(C2*C2)+(h1-h2)*(h1-h2));
	return mwDis.ExtractScalar(1);	
}


double CBeaconSimu::CalLosAngle(POINT3 posPlane,POINT3 posBeacon){
	//计算LOS与竖轴夹角:(la,phi,h),(度，度，米)
	double la1=CTraj3DShow::C2Rad(posPlane.x,0,0);
	double phi1=CTraj3DShow::C2Rad(posPlane.y,0,0);
	double h1=posPlane.z;
	
	double la2=CTraj3DShow::C2Rad(posBeacon.x,0,0);
	double phi2=CTraj3DShow::C2Rad(posBeacon.y,0,0);
	double h2=posBeacon.z;
	
	double e=0.081819191,Re=6378137.0;
	mwArray mwTmp=sqrt((1-e*e*sin(phi1)*sin(phi1)));
    mwArray Rm=mwArray(Re)*mwArray(1-e*e)/(mwTmp*mwTmp*mwTmp);
	mwArray Rn=Re/sqrt((1-e*e*sin(phi1)*sin(phi1)));
	
	mwArray p1=(Rn+h1)*cos(phi1)*(la2-la1);
	mwArray p2=(Rm+h1)*(phi2-phi1);
	mwArray p3=h2-h1;
	mwArray mwLos=atan(sqrt(p1*p1+p2*p2)/abs(p3))*180.0/pi();
	return mwLos.ExtractScalar(1);
}



//离散化，F,G为系统某时刻值(F,G),q为系统噪声方差强度阵，T为离散周期
//PHi,Q输出，PHi为一步转称矩阵，Q为白噪声序列方差阵
void CBeaconSimu::Discretize(mwArray& PHi,mwArray& Q,mwArray F,mwArray G,mwArray q,mwArray T)
{
	mwArray M,tQ;
	PHi=expm(T*F);
	M=G*q*transpose(G);
	tQ=M;
	for (int i=2;i<=10;i++){
		M=F*M+transpose((F*M));
		tQ=tQ+M*power(T,i)/factorial(i);
	}
	Q=tQ;
}


//马尔科夫一步递推
//dMavTime相关时间(s)，q白噪声方差强度，dPrevValue上一时刻值，dEllapseTime递推时间
double CBeaconSimu::StepMav(double dMavTime,double dq,double dPrevValue,double dEllapseTime){
	mwArray PHi,Q,F,G,q,T;
	F=mwArray(-1.0/dMavTime);
	G=mwArray(1.0);
	q=mwArray(dq);
	T=mwArray(dEllapseTime);
	Discretize(PHi,Q,F,G,q,T);	
	mwArray mwNextValue;
	mwNextValue=PHi*mwArray(dPrevValue)+rand()*sqrt(Q);
	return mwNextValue.ExtractScalar(1);
}

// 信标到飞机磁北方位角
double CBeaconSimu::CalBeacon2PlaneBearing(POINT3 posPlane,POINT3 posBeacon)
{
 //输入(la,phi,h),(度，度，米)
 //返回度
	double la=CTraj3DShow::C2Rad(posPlane.x,0,0);
	double phi=CTraj3DShow::C2Rad(posPlane.y,0,0);
	double h=posPlane.z;
	
	double la0=CTraj3DShow::C2Rad(posBeacon.x,0,0);
	double phi0=CTraj3DShow::C2Rad(posBeacon.y,0,0);
	double h0=posBeacon.z;

	double e=0.081819191,Re=6378137.0;
    mwArray Rm=mwArray(Re)*mwArray(1-e*e)/power(sqrt((1-e*e*sin(phi)*sin(phi))),mwArray(3));
    mwArray C1=1/(Rm+h);//纬度比
    mwArray Rn=Re/sqrt((1-e*e*sin(phi)*sin(phi)));
    mwArray C2=sec(phi)/(Rn+h);//经度比

	mwArray  mwAz;
	double dVa=(APPStatic::app_dMagneticErrorAngle)*pi().ExtractScalar(1)/180.0;//磁偏角
	if((phi>phi0 && la>=la0)|| phi<phi0){
		mwAz=atan(C1*(la-la0)/(C2*(phi-phi0)))+pi()-dVa;
	}
	else if(phi>phi0 && la<la0){
		mwAz=atan(C1*(la-la0)/(C2*(phi-phi0)))+2.0*pi()-dVa;
	}
	else if(phi==phi0 && la>la0){
		mwAz=pi()/2.0-dVa;
	}
	else if(phi==phi0 && la<la0){
		mwAz=pi()*1.5-dVa;
	}	

	mwAz=(mwAz+pi())*180.0/pi();
	double dAz=mwAz.ExtractScalar(1);
	if(dAz>360.0){
		dAz-=360.0;
	}
	return dAz;
 }


void CBeaconSimu::RefreshMeasure()
{
//////// Dis Measure///////////
	TRAJ traj;
	CArray<BEACON,BEACON>* pBl;
	BOOL bFound;
	UINT iIndex;
	POINT3 posPlane,posBeacon;
	double dDis;
	//
	traj=APPStatic::GetMainView()->m_fileAccess.GetCurrentPackageData();  
	pBl=m_pBeaconManage->GetBeaconList();
	if(pBl->GetSize()==0) return;
	bFound=GetNearestBeaconIndex(traj,&iIndex);
	if(bFound){
		posPlane.x=traj.lon;
		posPlane.y=traj.lat;
		posPlane.z=traj.heg;
		posBeacon=pBl->GetAt(iIndex).pos;
		dDis=CalDistance(posPlane,posBeacon);
		m_pBeaconManage->m_iAvailableBCIndex=iIndex;
		m_pBeaconManage->m_bIsBeaconAvailable=TRUE;
	}
	else{
		m_pBeaconManage->m_bIsBeaconAvailable=FALSE;
	}
	//加误差
	mwArray mwDisMav;
	double dMavTime,dq,dEllapseTime,dPrevValue,dWhiteEr;
	if(bFound){
		switch(m_pBeaconManage->GetBeaconList()->GetAt(iIndex).type){
		case AIR:			
			if(m_bNewBeacon){
				mwDisMav=m_pBeaconManage->m_dMavInitErDisAir*rand();
				m_dDisMav=mwDisMav.ExtractScalar(1);
			}
			else{
                dMavTime=m_pBeaconManage->m_dMavTimeDisAir*3600.0;
				dq=2.0/dMavTime*(m_pBeaconManage->m_dMavInitErDisAir)*(m_pBeaconManage->m_dMavInitErDisAir);
				dEllapseTime=1.0/(m_pBeaconManage->m_dFreq);
				dPrevValue=m_dDisMav;
				m_dDisMav=StepMav(dMavTime,dq,dPrevValue,dEllapseTime);
			}
			dWhiteEr=(m_pBeaconManage->m_dMavWhiteErDisAir*rand()).ExtractScalar(1);
			break;

		case GROUND:
			if(m_bNewBeacon){
				mwDisMav=m_pBeaconManage->m_dMavInitErDisGround*rand();
				m_dDisMav=mwDisMav.ExtractScalar(1);
			}
			else{
                dMavTime=m_pBeaconManage->m_dMavTimeDisGround*3600.0;
				dq=2.0/dMavTime*(m_pBeaconManage->m_dMavInitErDisGround)*(m_pBeaconManage->m_dMavInitErDisGround);
				dEllapseTime=1.0/(m_pBeaconManage->m_dFreq);
				dPrevValue=m_dDisMav;
				m_dDisMav=StepMav(dMavTime,dq,dPrevValue,dEllapseTime);
			}
			dWhiteEr=(m_pBeaconManage->m_dMavWhiteErDisGround*rand()).ExtractScalar(1);
			break;

		default:;
			
		}
	m_dDisMeasure=dDis+m_dDisMav+dWhiteEr;
	}

/////////////////////Az Mearsure/////////////////////////////////

	double dAz=0;
	if(bFound){
		posPlane.x=traj.lon;
		posPlane.y=traj.lat;
		posPlane.z=traj.heg;
		posBeacon=pBl->GetAt(iIndex).pos;
		dAz=CalBeacon2PlaneBearing(posPlane,posBeacon);
		m_pBeaconManage->m_iAvailableBCIndex=iIndex;
		m_pBeaconManage->m_bIsBeaconAvailable=TRUE;

	}
	else{
		m_pBeaconManage->m_bIsBeaconAvailable=FALSE;
	}
	//加误差
	mwArray mwAzMav;
	if(bFound){
		switch(m_pBeaconManage->GetBeaconList()->GetAt(iIndex).type){
		case AIR:			
			if(m_bNewBeacon){
				mwAzMav=m_pBeaconManage->m_dMavInitErAzAir*rand();
				m_dAzMav=mwAzMav.ExtractScalar(1);
			}
			else{
                dMavTime=m_pBeaconManage->m_dMavTimeAzAir*3600.0;
				dq=2.0/dMavTime*(m_pBeaconManage->m_dMavInitErAzAir)*(m_pBeaconManage->m_dMavInitErAzAir);
				dEllapseTime=1.0/(m_pBeaconManage->m_dFreq);
				dPrevValue=m_dAzMav;
				m_dAzMav=StepMav(dMavTime,dq,dPrevValue,dEllapseTime);
			}
			dWhiteEr=(m_pBeaconManage->m_MavWhiteErAzAir*rand()).ExtractScalar(1);
			break;

		case GROUND:
			if(m_bNewBeacon){
				mwAzMav=m_pBeaconManage->m_dMavInitErAzGround*rand();
				m_dAzMav=mwAzMav.ExtractScalar(1);
			}
			else{
                dMavTime=m_pBeaconManage->m_dMavTimeAzGround*3600.0;
				dq=2.0/dMavTime*(m_pBeaconManage->m_dMavInitErAzGround)*(m_pBeaconManage->m_dMavInitErAzGround);
				dEllapseTime=1.0/(m_pBeaconManage->m_dFreq);
				dPrevValue=m_dAzMav;
				m_dAzMav=StepMav(dMavTime,dq,dPrevValue,dEllapseTime);
			}
			dWhiteEr=(m_pBeaconManage->m_MavWhiteErAzGround*rand()).ExtractScalar(1);
			break;

		default:;
			
		}
	m_dAzMeasure=dAz+m_dAzMav+dWhiteEr;
	}
}

/*
void CBeaconSimu::RefreshAzMeasure()
{
	TRAJ traj;
	CArray<BEACON,BEACON>* pBl;
	BOOL bFound;
	UINT iIndex;
	POINT3 posPlane,posBeacon;
	double dAz=0;
	
	traj=APPStatic::GetMainView()->m_fileAccess.GetCurrentPackageData();  
	pBl=m_pBeaconManage->GetBeaconList();
	if(pBl->GetSize()==0) return;
	bFound=GetNearestBeaconIndex(traj,&iIndex);
	if(bFound){
		posPlane.x=traj.lon;
		posPlane.y=traj.lat;
		posPlane.z=traj.heg;
		posBeacon=pBl->GetAt(iIndex).pos;
		dAz=CalBeacon2PlaneBearing(posPlane,posBeacon);
		m_pBeaconManage->m_iAvailableBCIndex=iIndex;
		m_pBeaconManage->m_bIsBeaconAvailable=TRUE;

	}
	else{
		m_pBeaconManage->m_bIsBeaconAvailable=FALSE;
	}
	//加误差
	mwArray mwAzMav;
	double dMavTime,dq,dEllapseTime,dPrevValue,dWhiteEr;
	if(bFound){
		switch(m_pBeaconManage->GetBeaconList()->GetAt(iIndex).type){
		case AIR:			
			if(m_bNewBeacon){
				mwAzMav=m_pBeaconManage->m_dMavInitErAzAir*rand();
				m_dAzMav=mwAzMav.ExtractScalar(1);
			}
			else{
                dMavTime=m_pBeaconManage->m_dMavTimeAzAir*3600.0;
				dq=2.0/dMavTime*(m_pBeaconManage->m_dMavInitErAzAir)*(m_pBeaconManage->m_dMavInitErAzAir);
				dEllapseTime=1.0/(m_pBeaconManage->m_dFreq);
				dPrevValue=m_dAzMav;
				m_dAzMav=StepMav(dMavTime,dq,dPrevValue,dEllapseTime);
			}
			dWhiteEr=(m_pBeaconManage->m_MavWhiteErAzAir*rand()).ExtractScalar(1);
			break;

		case GROUND:
			if(m_bNewBeacon){
				mwAzMav=m_pBeaconManage->m_dMavInitErAzGround*rand();
				m_dAzMav=mwAzMav.ExtractScalar(1);
			}
			else{
                dMavTime=m_pBeaconManage->m_dMavTimeAzGround*3600.0;
				dq=2.0/dMavTime*(m_pBeaconManage->m_dMavInitErAzGround)*(m_pBeaconManage->m_dMavInitErAzGround);
				dEllapseTime=1.0/(m_pBeaconManage->m_dFreq);
				dPrevValue=m_dAzMav;
				m_dAzMav=StepMav(dMavTime,dq,dPrevValue,dEllapseTime);
			}
			dWhiteEr=(m_pBeaconManage->m_MavWhiteErAzGround*rand()).ExtractScalar(1);
			break;

		default:;
			
		}
	m_dAzMeasure=dAz+m_dAzMav+dWhiteEr;
	}
}


*/

beaconsimu.cpp - 源码说明

本页面展示了「VC&Matlab混合编程实现无线电导航指示器」中的 beaconsimu.cpp 源码文件，采用 C++ 编程语言编写，共 402 行代码。您可以在线阅读完整代码内容，也可以返回资源详情页下载完整源码包进行本地学习和开发。

虫虫开发者社区收录了大量与VC相关的技术资源，包括源代码、技术文档、电路图等，是电子工程师和嵌入式开发者的专业学习平台。

⌨️ 快捷键说明

复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?