📄 fixarithmetic.java
字号:
package algorithm;
import java.text.*; //将文本显示功能的包装入
import java.io.*;
import GPSMAIN.*;
public class FixArithmetic
{
//test use
double []delayTime0 = new double[3];
// **********下面是计算卫星的坐标和终端测量参数所需要的一些变量************///
public static final double para_gm = 3.986005e+14; // 地球的引力常数
public static final double para_omegae = 7.2921151467e-5; // 地球的自转角速度
public static final double para_light_speed = 299792458;//
/*xhw*/
public static final double para_pi = 3.1415926535898;//PI
// *****最后计算定位的年、月、日、时、分、秒****//
private int fix_year;
private int fix_month;
private int fix_day;
private int fix_hour;
private int fix_minute;
private int fix_second;
// ********定位的经纬度、高度、三维坐标及定位的精度********//
private int fix_svnumber; // 定位的卫星颗数，用于调试
private double fix_longitude;
private double fix_latitude;
private double fix_high;
private double fix_xcoordinate;
private double fix_ycoordinate;
private double fix_zcoordinate;
private double fix_error;
// ********小区的标识号、经纬度及高度********//
double cell_longitude;
double cell_latitude;
double cell_high;
char cell_id;
// 下面数据是终端的测量数据
private long para_terminal_gpstow; // GPS星期ms数
private int para_cno; // GPS卫星信噪比 单位dB/Hz
private double para_doppler; // GPS信号的多普勒频偏 单位HZ
private int para_wholechips; // 伪距测量的整码片数 单位chip
private double para_fracchips; // 伪距测量的分码片数 单位chip
// 下面是从supl接收过来的数据
private double para_cuc; // 纬度幅角余弦改正项 单位rad
private double para_cus; // 纬度幅角正弦改正项 单位rad
private double para_crc; // 轨道半径余弦改正项 单位m
private double para_crs; // 轨道半径正弦改正项 单位m
private double para_cic; // 轨道倾角余弦改正项 单位rad
private double para_cis; // 轨道倾角正弦改正项 单位rad
private double para_e; // 偏心率e
private double para_i0; // 轨道倾角i0 单位半周
private double para_w; // 近地点点角距ω 单位半周
private double para_omega0; // 升交点赤经Ω0 单位半周
private double para_m0; // 参考时间的平近点角M0 单位半周
private double para_omegadot; // 升交点赤径变化率Ω0 单位半周/s
private double para_deltan; // 平均角速度改正0n 单位半周/s
private double para_idot; // 倾角变化率0i 单位半周/s
private double para_apowerhalf; // 长半轴的平方根 单位m的根方
private long para_ephemToc; // 卫星钟差数据参考时刻，单位S
private long para_ephemAODC; // GPS卫星时钟数据龄期 单位s
// private double para_gpstow; // GPS星期秒数
private int para_gpsweek; // GPS星期数
private int[] para_svid = new int[10]; // GPS卫星编号
private int para_ephemura; // GPS卫星测距精度
private long para_ephemiodc; // GPS卫星星历数据龄期
private double para_tgd; // 群延迟 单位s
private long para_toe; // 轨道参数参考时间t0e 单位S
private double para_ephemaf2; // 卫星钟差二次改正, 单位s/(s*s)
private double para_ephemaf1; // 卫星钟差一次改正, 单位s/s
private double para_ephemaf0; // 卫星钟差改正 单位s
private double para_gpstow; //SUPL中Acquisition Assist中的GPS TOW GPS星期秒数 单位 0.08s
private int para_acq_codephase ;//SUPL中Acquisition Assist中的GPS CODE PHASE 单位 1chip
private int para_acq_intcodephase ;//SUPL中Acquisition Assist中的GPS INTGER CODE PHASE 单位 1ms
private int para_acq_gpsbitnum ; //SUPL中Acquisition Assist中的GPS Bit number 单位 20ms
// 下面是定位所需要的参数
private double para_X; // 卫星的X坐标 单位m
private double para_Y; // 卫星的Y坐标 单位m
private double para_Z; // 卫星的Z坐标 单位m
private double para_pseurange; // 对卫星的测量伪局
// 功能：卫星定位的算法
// 入口：四颗卫星的星历参数、终端测量参数
// 出口：
// 作者：lrs
public FixArithmetic(int svnumber, Satellite[] sat)
{
FixCalulate fixcalulate = new FixCalulate(); // 首先实例化定位算法对象
BLHXYZ blhxyz=new BLHXYZ();
double cellX,cellY,cellZ;
/**xhw*/
double tempPseuRagne;
System.out.println("xhw---->in FixArithmetic constructor");
// ******一颗卫星的定位算法*******//
if (1 == svnumber)
{
fix_svnumber = svnumber;
double[] svX = new double[4];
double[] svY = new double[4];
double[] svZ = new double[4];
double[] svPseuRange = new double[4];
double[] svRealRange = new double[4];
double[] svDistrRange = new double[4];
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 < 4; 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]);
}
svRealRange[0]=svPseuRange[0];
svDistrRange[0]=svRealRange[0]*para_light_speed*0.001;
for(int j=1;j<4;j++)
{
svRealRange[j]=calRealRange(svPseuRange[0] ,svPseuRange[j] ) ;
svDistrRange[j]=svRealRange[j]*para_light_speed*0.001;
}
fixcalulate.aboveThreeFix(svX, svY, svZ, svDistrRange,
cell_latitude, cell_longitude, cell_high, 4);
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 if (2 == svnumber)
{
fix_svnumber = svnumber;
double[] svX = new double[4];
double[] svY = new double[4];
double[] svZ = new double[4];
double[] svPseuRange = new double[4];
double[] svRealRange = new double[4];
double[] svDistrRange = new double[4];
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 < 4; 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]);
}
svRealRange[0]=svPseuRange[0];
svDistrRange[0]=svRealRange[0]*para_light_speed*0.001;
for(int j=1;j<4;j++)
{
svRealRange[j]=calRealRange(svPseuRange[0] ,svPseuRange[j] ) ;
svDistrRange[j]=svRealRange[j]*para_light_speed*0.001;
}
fixcalulate.twoStarFixB(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 if (3 == svnumber)
{
System.out.println("xhw---->use three satellite ...");
fix_svnumber = svnumber;
double[] svX = new double[3];
double[] svY = new double[3];
double[] svZ = new double[3];
double[] svPseuRange = new double[3];
double[] svRealRange = new double[3];
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -