?? beaconsimu.cpp
字號:
// 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;
}
}
*/?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -