📄 fixarithmetic.java
字号:
double[] svDistrRange = new double[3];
cell_longitude = sat[0].getCell_longitude();
cell_latitude = sat[0].getCell_latitude();
cell_high = sat[0].getCell_high();
para_terminal_gpstow=sat[0].getGpsTOW();
para_gpstow = sat[0].getAcquisitiongpsTOW();
blhxyz.BLHtoXYZ(cell_latitude,cell_longitude,cell_high);
cellX=blhxyz.getXCoord() ;
cellY=blhxyz.getYCoord() ;
cellZ=blhxyz.getZCoord() ;
for (int i = 0; i < 3; i++)
{
para_acq_codephase = sat[i].getCodePhase() ;
para_acq_intcodephase = sat[i].getIntCodePhase();
para_acq_gpsbitnum = sat[i].getGpsBitNumber();
para_svid[i] = sat[i].getSatelliteID();
para_gpsweek = sat[i].getGpsWeek();
//para_gpstow = sat[i].getGpsTOW();
para_cno = sat[i].getCNo();
para_doppler = sat[i].getDoppler();
para_wholechips = sat[i].getWholeChips();
para_fracchips = sat[i].getFracChips();
para_cuc = sat[i].getEphemCuc();
para_cus = sat[i].getEphemCus();
para_crc = sat[i].getEphemCrc();
para_crs = sat[i].getEphemCrs();
para_cic = sat[i].getEphemCic();
para_cis = sat[i].getEphemCis();
para_e = sat[i].getEphemE();
para_i0 = sat[i].getEphemI0();
para_w = sat[i].getEphemW();
para_omega0 = sat[i].getEphemOmegaA0();
para_m0 = sat[i].getEphemM0();
para_omegadot = sat[i].getEphemOmegaADot();
para_deltan = sat[i].getEphemDeltaN();
para_idot = sat[i].getEphemIDot();
para_apowerhalf = sat[i].getEphemAPowerHalf();
para_ephemToc = sat[i].getEphemToc();
para_ephemAODC = sat[i].getEphemAODA();
para_ephemura = sat[i].getEphemURA();
para_ephemiodc = sat[i].getEphemIODC();
para_tgd = sat[i].getEphemTgd();
para_toe = sat[i].getEphemToe();
para_ephemaf2 = sat[i].getEphemAF2();
para_ephemaf1 = sat[i].getEphemAF1();
para_ephemaf0 = sat[i].getEphemAF0();
calSvXYZ();
svX[i] = para_X;
svY[i] = para_Y;
svZ[i] = para_Z;
calYMDHMS(para_gpsweek, para_gpstow);
//modified by hjs
svPseuRange[i]=calPseuRange( para_acq_codephase, para_acq_intcodephase , para_acq_gpsbitnum, para_gpstow,
para_ephemToc, para_tgd, para_ephemaf0, para_ephemaf1,para_ephemaf2);
//end modified
//svPseuRange[i]+=calSvSelfModify(cellX, cellY ,svX[i],svY[i]);
System.out.println("sat["+i+"]:para_x"+para_X);
System.out.println("sat["+i+"]:para_y"+para_Y);
System.out.println("sat["+i+"]:para_z"+para_Z);
//test add by hjs
double delayTime = getDelayTime(para_acq_codephase, para_acq_intcodephase, para_acq_gpsbitnum);
System.out.println("sat["+i+"]:delayTime>>" + delayTime + "*****************************************************************");
double Range = delayTime * 300000;
System.out.println("sat["+i+"]:delayTime>>" + delayTime + "*****************************************************************");
double Range1 = Math.sqrt(
Math.pow((para_X - cellX), 2) + Math.pow((para_Y - cellY), 2) +Math.pow((para_Z - cellZ), 2) );
double delaytime2 = Range1/(para_light_speed*0.001);
System.out.println("sat["+i+"]:delaytime2>>" + delaytime2 + "*****************************************************************");
double RangeTime = getRangeTime(delaytime2, para_wholechips, para_fracchips);
//double RangeTime = 0;
System.out.println("i>>" + i + "*****************************************************************");
if(i == 0)
{
RangeTime = delayTime0[1];
}
else if(i == 1)
{
RangeTime = delayTime0[1];
}
else if(i == 2)
{
RangeTime = delayTime0[2];
}
System.out.println("sat["+i+"]:RangeTime>>" + RangeTime + "*****************************************************************");
System.out.println("sat["+i+"]:para_tgd>>" + para_tgd + "*****************************************************************");
svPseuRange[i] = RangeTime - para_tgd;
//end add
}
svRealRange[0]=svPseuRange[0];
svDistrRange[0]=svRealRange[0]*para_light_speed*0.001;
for(int j=1;j<3;j++)
{
svRealRange[j]=calRealRange(svPseuRange[0] ,svPseuRange[j] ) ;
svDistrRange[j]=svRealRange[j]*para_light_speed*0.001;
}
fixcalulate.threeStarFix(svX, svY, svZ, svDistrRange, cell_latitude,
cell_longitude, cell_high);
fix_longitude = fixcalulate.getUserLongitude();
fix_latitude = fixcalulate.getUserLatitude();
fix_high = fixcalulate.getUserHigh();
fix_xcoordinate = fixcalulate.getUserXCoordinate();
fix_ycoordinate = fixcalulate.getUserYCoordinate();
fix_zcoordinate = fixcalulate.getUserZCoordinate();
fix_error = fixcalulate.getUserError();
}
// ******四颗及四颗以上卫星的定位算法*******//
else
{
fix_svnumber = svnumber;
double[] svX = new double[svnumber];
double[] svY = new double[svnumber];
double[] svZ = new double[svnumber];
double[] svPseuRange = new double[svnumber];
double[] svRealRange = new double[svnumber];
double[] svDistrRange = new double[svnumber];
cell_longitude = sat[0].getCell_longitude();
cell_latitude = sat[0].getCell_latitude();
cell_high = sat[0].getCell_high();
para_terminal_gpstow = sat[0].getGpsTOW();
para_gpstow = sat[0].getAcquisitiongpsTOW();
blhxyz.BLHtoXYZ(cell_latitude,cell_longitude,cell_high);
cellX=blhxyz.getXCoord();
cellY=blhxyz.getYCoord();
cellZ=blhxyz.getZCoord();
System.out.println("xhw---->svnumber="+svnumber);
for (int i = 0; i < svnumber; i++)
{
para_acq_codephase=sat[i].getCodePhase() ;
para_acq_intcodephase= sat[i].getIntCodePhase();
para_acq_gpsbitnum= sat[i].getGpsBitNumber();
para_svid[i] = sat[i].getSatelliteID();
para_gpsweek = sat[i].getGpsWeek();
//para_gpstow = sat[i].getGpsTOW();
para_cno = sat[i].getCNo();
para_doppler = sat[i].getDoppler();
para_wholechips = sat[i].getWholeChips();
para_fracchips = sat[i].getFracChips();
para_cuc = sat[i].getEphemCuc();
para_cus = sat[i].getEphemCus();
para_crc = sat[i].getEphemCrc();
para_crs = sat[i].getEphemCrs();
para_cic = sat[i].getEphemCic();
para_cis = sat[i].getEphemCis();
para_e = sat[i].getEphemE();
para_i0 = sat[i].getEphemI0();
para_w = sat[i].getEphemW();
para_omega0 = sat[i].getEphemOmegaA0();
para_m0 = sat[i].getEphemM0();
para_omegadot = sat[i].getEphemOmegaADot();
para_deltan = sat[i].getEphemDeltaN();
para_idot = sat[i].getEphemIDot();
para_apowerhalf = sat[i].getEphemAPowerHalf();
para_ephemToc = sat[i].getEphemToc();
para_ephemAODC = sat[i].getEphemAODA();
para_ephemura = sat[i].getEphemURA();
para_ephemiodc = sat[i].getEphemIODC();
para_tgd = sat[i].getEphemTgd();
para_toe = sat[i].getEphemToe();
para_ephemaf2 = sat[i].getEphemAF2();
para_ephemaf1 = sat[i].getEphemAF1();
para_ephemaf0 = sat[i].getEphemAF0();
// calSvXYZ();
// svX[i] = para_X;
// svY[i] = para_Y;
// svZ[i] = para_Z;
//
// calYMDHMS(para_gpsweek, para_gpstow);
// svPseuRange[i]=calPseuRange( para_acq_codephase, para_acq_intcodephase , para_acq_gpsbitnum, para_gpstow,
// para_ephemToc, para_tgd, para_ephemaf0, para_ephemaf1,para_ephemaf2);
//
// svPseuRange[i]+=calSvSelfModify(cellX, cellY ,svX[i],svY[i]);
/*******************************/
svPseuRange[i]=0;
tempPseuRagne=75;
while(true)
{
calSvXYZ(tempPseuRagne);
svPseuRange[i]=calPseuRange(cellX,cellY,cellZ,tempPseuRagne);
if(Math.abs(tempPseuRagne - svPseuRange[i])<1.0E-10)
break;
tempPseuRagne=svPseuRange[i];
}
//svPseuRange[i] = ((svPseuRange[i]/1000.0)-para_tgd)*1000.0;
//svPseuRange[i] = ((svPseuRange[i]/1000.0))*1000.0;
System.out.println("xhw---->svPseuRange["+i+"]:"+svPseuRange[i]);
//svDistrRange[i]=svPseuRange[i]*para_light_speed*0.001;
//System.out.println("xhw---->svDistrRange["+i+"]="+svDistrRange[i]);
System.out.println();
svX[i] = para_X;
svY[i] = para_Y;
svZ[i] = para_Z;
/*******************************/
}
svRealRange[0]=svPseuRange[0];
svDistrRange[0]=svRealRange[0]*para_light_speed*0.001;
for(int j=1; j < svnumber; j++)
{
svRealRange[j]=calRealRange(svPseuRange[0] ,svPseuRange[j] ) ;
svDistrRange[j]=svRealRange[j]*para_light_speed*0.001;
}
System.out.println("xhw---->use baoveThreeFix()");
fixcalulate.aboveThreeFix(svX, svY, svZ, svDistrRange,
cell_latitude, cell_longitude, cell_high, svnumber);
fix_longitude = fixcalulate.getUserLongitude();
fix_latitude = fixcalulate.getUserLatitude();
fix_high = fixcalulate.getUserHigh();
fix_xcoordinate = fixcalulate.getUserXCoordinate();
fix_ycoordinate = fixcalulate.getUserYCoordinate();
fix_zcoordinate = fixcalulate.getUserZCoordinate();
fix_error = fixcalulate.getUserError();
}
/******/
System.out.println("xhw---->get result from FixArithmetic:");
System.out.println("xhw---->FixArithmetic() Latitude:"+fix_latitude);
System.out.println("xhw---->FixArithmetic() Longitude:"+fix_longitude);
}
// 功能：根据卫星的轨道参数计算卫星的三维坐标
// 入口：无
// 出口：卫星的三维坐标（X，Y，Z）
// 作者：lrs
void calSvXYZ()
{
double n0, n;
double tk, mk;
double ek, ek1, ek2, vk, phik;
double deltau, deltar, deltai, uk, rk, ik, xk, yk, omegak;
long timeintvalue;
double measuretime;
timeintvalue = (long)(para_gpstow)/14400;
measuretime = timeintvalue*14400 + para_terminal_gpstow*0.001;
n0 = Math.sqrt((para_gm)
/ (para_apowerhalf * para_apowerhalf * para_apowerhalf));
n = n0 + para_deltan;
//tk = para_gpstow - para_toe;
tk = measuretime - para_toe;
if (tk > 302400)
tk -= 604800;
else if (tk < -302400)
tk += 604800;
mk = para_m0 + n * tk;
ek1 = mk;
do
{
ek2 = mk + para_e * Math.sin(ek1);
if (Math.abs(ek2 - ek1) <= 1.0e-10) //-12
break;
ek1 = ek2;
} while (true);
ek = ek1;
vk = Get_atan((Math.cos(ek) - para_e), Math.sqrt(1.0 - para_e * para_e)
* Math.sin(ek));
phik = vk + para_w;
deltau = para_cuc * Math.cos(2.0 * phik) + para_cus
* Math.sin(2.0 * phik);
deltar = para_crc * Math.cos(2.0 * phik) + para_crs
* Math.sin(2.0 * phik);
deltai = para_cic * Math.cos(2.0 * phik) + para_cis
* Math.sin(2.0 * phik);
uk = phik + deltau;
rk = para_apowerhalf * para_apowerhalf * (1.0 - para_e * Math.cos(ek))
+ deltar;
ik = para_i0 + deltai + para_idot * tk;
xk = rk * Math.cos(uk);
yk = rk * Math.sin(uk);
omegak = para_omega0 + (para_omegadot - para_omegae) * tk - para_omegae
* para_toe;
para_X = xk * Math.cos(omegak) - yk * Math.cos(ik) * Math.sin(omegak);
para_Y = xk * Math.sin(omegak) + yk * Math.cos(ik) * Math.cos(omegak);
para_Z = yk * Math.sin(ik);
}
// 功能：用来计算三维坐标下的角度
// 入口：坐标x与坐标y
// 出口：角度
// 作者：lrs
double Get_atan(double z, double y)
{
double x;
if (z == 0)
x = Math.PI / 2;
else
{
if (y == 0)
x = Math.PI;
else
{
x = Math.atan(Math.abs(y / z));
if ((y > 0) && (z < 0))
x = Math.PI - x;
else if ((y < 0) && (z < 0))
x = Math.PI + x;
else if ((y < 0) && (z > 0))
x = 2 * Math.PI - x;
}
}
return x;
}
// 功能：计算定位时刻的具体时间：year，month，day，hour，mintue，second
// 入口：星历中的周数weekno，和gps的系统时间gpstow
// 出口：无
// 作者：lrs
void calYMDHMS(int weekno, double gpstow)
{
int dayofw, h, m, s;
long isecs;
int dayofy, yr, ttlday, mn;
final int dinmth[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
weekno = weekno + 1024;
isecs = (long) (gpstow + 28800);// isecs=gpstow ;
dayofw = (int) isecs / 86400;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -