Java实现伪距单点定位

最近上课有点忙，所以没来得急更新最后的一节，上几期文章已经给出了读取观测值文件和广播星历文件以及计算GPS中圆轨道卫星的空间坐标（不知道的可以看之前的文章，有详细的代码），所以本期最后给出根据观测历元数据及广播星历计算测站的空间坐标（WGS-84），代码如下：

• StationPositionCalculate.java

package PosCalcluate;

import Jama.Matrix;
import ReadFile.DataClass.Ephemeris;
import ReadFile.DataClass.ObsEpoch;
import ReadFile.ReadNavData;
import ReadFile.ReadObsData;
import TimeSysTrans.GPST;
import TimeSysTrans.TimeSysTrans;
import TimeSysTrans.UTC;
import Util.CorrectionCalculate.SatelliteClockCorrection;
import org.apache.log4j.Logger;
import sun.rmi.runtime.Log;

import java.util.ArrayList;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;


/**
 * 该类实现测站位置计算
 * @author lhn
 */

public class StationPositionCalculate {
    private ReadObsData readObsData=null;
    private ReadNavData readNavData=null;
    private Map> obsEpochMap=null;
    //光速（m）
    private static final double SPEED_OF_LIGHT_MPS = 299792458.0;
    //迭代最大次数
    private static final int MAX_ITERATIONS = 200;
    //地球自转角速度
    private static final double EARTH_ROTATION_RATE_RAD_PER_SEC = 7.2921151467e-5;
    //迭代计算的精度要求
    private static final double ACCURACY_TOLERANCE = 1.0e-7;
    //迭代计算的精度要求
    private static final double ACCURACY_TOLERANCE2 = 1.0e-4;
    //创建log对象
    Logger logger=null;

    /**
     * 构造函数
     * @param navigationFile  广播星历文件
     * @param observationFile  观测值文件
     */
    public StationPositionCalculate(String navigationFile,String observationFile){
        readObsData=new ReadObsData(observationFile);
        readNavData=new ReadNavData(navigationFile);
        logger=Logger.getLogger(StationPositionCalculate.class);
    }

    /**
     * 计算整个观测值历元数据
     * @return spatial_positionMap
     */
    public Map allOfEpochPositionCalculate() throws Exception {
        Mapspatial_positionMap=null;
        obsEpochMap=readObsData.getObsEpochMap();
        if(obsEpochMap!=null){
            spatial_positionMap=positionCalculate(obsEpochMap);
        }
        return spatial_positionMap;
    }

    /**
     * 分时段计算
     * @param startTime 开始历元
     * @param endTime  结束历元
     * @return spatial_positionMap
     */
    public Map segmentOfEpochPositionCalculate(UTC startTime,UTC endTime) throws Exception {
        Mapspatial_positionMap=null;
        obsEpochMap=readObsData.getObsEpochMap();
        //开始历元
        GPST startEpoch=new GPST();
        TimeSysTrans.UTCToGPST(startTime,startEpoch);
        //结束历元
        GPST endEpoch=new GPST();
        TimeSysTrans.UTCToGPST(endTime,endEpoch);
        Map> newObsEpochMap=selectObservationEpochData(startEpoch,endEpoch,obsEpochMap);
        if(newObsEpochMap!=null){
            spatial_positionMap=positionCalculate(newObsEpochMap);
        }
        return spatial_positionMap;
    }

    /**
     * 选择指定历元间隔的观测值数据
     * @param startEpoch  开始历元
     * @param endEpoch    结束历元
     * @param obsEpochMap  原始观测值数据
     * @return newObsEpochMap
     */
    private Map> selectObservationEpochData(GPST startEpoch, GPST endEpoch, Map> obsEpochMap) {
        Map>newObsEpochMap=new LinkedHashMap>();
        //遍历原始观测值历元数据Map，根据开始和结束历元时刻挑选出新的Map
        for(Map.Entry>entry:obsEpochMap.entrySet()){
            if(!entry.getKey().isLess(startEpoch) && !entry.getKey().isGreaterThan(endEpoch)){
                newObsEpochMap.put(entry.getKey(),entry.getValue());
            }
        }
        return newObsEpochMap;
    }

    /**
     *
     * @param obsEpochMap  观测值历元数据Map
     * @return  spatial_positionMap
     */
    private Map positionCalculate(Map> obsEpochMap) throws Exception {
        Mapspatial_positionMap=new LinkedHashMap();
        //初始值设置，测站概略位置X以及接收机钟差初值
        STATION_SPATIAL_POSITION stationPosition=new STATION_SPATIAL_POSITION();
        stationPosition.setxPosition(0.0);
        stationPosition.setyPosition(0.0);
        stationPosition.setzPosition(0.0);
        //接收机钟差（s）
        double receiveClock=0.0;
        //接收机钟差（m）
        double receiveClockMeters=SPEED_OF_LIGHT_MPS*receiveClock;

        //遍历整个输入的观测值历元数据
        for(Map.Entry>obsMap:obsEpochMap.entrySet()){


            //迭代数目
            int setIterationCounter=0;
            //该历元下所有卫星的列表
            ListobsEpochesList=obsMap.getValue();
            //参数估值
            double[] parameterCorrection=new double[4];
            //迭代计算
            do{
                //有效卫星数目
                int validSatelliteNum=0;
                //系数与余数项Map
                Map>mapOfCoefficientAndConstant=new LinkedHashMap<>();
                //卫星截至高度角
                MapmapOfSatelliteElevationDegrees=new LinkedHashMap<>();
                setIterationCounter++;
                //遍历该历元下所有的卫星
                for(ObsEpoch obs:obsEpochesList){
                    if(obs.getPrn().contains("G")){
                        //查找最近的卫星星历
                        Ephemeris ephemeris=selectEphemeris(obs.getObs_t(),obs.getPrn());
                        if(ephemeris!=null && obs.getC1()!=0.0){
                            validSatelliteNum++;
                            double pseudorange=obs.getC1();
                            //卫星钟差改正
                            SatelliteClockCorrection satelliteClockCorrection=new SatelliteClockCorrection(ephemeris,obs.getObs_t());
                            double satelliteClock=satelliteClockCorrection.getSatelliteClockCorrectionMeter()/SPEED_OF_LIGHT_MPS;
                            //根据伪距观测值求出信号传播时间
                            double timeSpreadOfSignal=pseudorange/SPEED_OF_LIGHT_MPS-receiveClock+satelliteClock;
                            double newTimeSpreadOfSignal=0.0;
                            //信号传播时间的改正值
                            double changeTimeSpreadSignal=0.0;
                            //卫星的空间坐标
                            SatellitePosition satellitePosition=null;
                            //测站与卫星的几何距离
                            double distanceBetweenSatelliteAndStation=0.0;
                            //信号的发射时刻(以周内秒)
                            double transmissionSec=0.0;
                            //迭代计算卫星的信号发射的概略时刻
                            do{
                                    //信号的发射时刻
                                    transmissionSec=obs.getObs_t().getSeconds()-timeSpreadOfSignal;
                                    GPST transmissionTime=new GPST();
                                    transmissionTime.setWeeks(obs.getObs_t().getWeeks());
                                    transmissionTime.setSeconds(transmissionSec);
                                    //卫星位置计算
                                    satellitePosition=SatellitePosCal.CalSatPos(transmissionTime,ephemeris);
                                    //地球自转改正
                                    distanceBetweenSatelliteAndStation=Math.sqrt((satellitePosition.getSatellitePosition_X()-stationPosition.getxPosition())
                                            *(satellitePosition.getSatellitePosition_X()-stationPosition.getxPosition())+ (satellitePosition.getSatellitePosition_Y()-stationPosition.getyPosition())
                                            *(satellitePosition.getSatellitePosition_Y()-stationPosition.getyPosition())+(satellitePosition.getSatellitePosition_Z()-stationPosition.getzPosition())
                                            *(satellitePosition.getSatellitePosition_Z()-stationPosition.getzPosition()));
                                    //时间延迟（s）
                                    double delayOfTime=distanceBetweenSatelliteAndStation/SPEED_OF_LIGHT_MPS;
                                    //旋转矩阵
                                    double[]rotation={Math.cos(EARTH_ROTATION_RATE_RAD_PER_SEC*delayOfTime),Math.sin(EARTH_ROTATION_RATE_RAD_PER_SEC*delayOfTime),0,
                                    -Math.sin(EARTH_ROTATION_RATE_RAD_PER_SEC*delayOfTime),Math.cos(EARTH_ROTATION_RATE_RAD_PER_SEC*delayOfTime),0,
                                    0,0,1};
                                    double newSatelliteX=satellitePosition.getSatellitePosition_X()*rotation[0]+satellitePosition.getSatellitePosition_Y()*rotation[1];
                                    double newSatelliteY=satellitePosition.getSatellitePosition_X()*rotation[3]+satellitePosition.getSatellitePosition_Y()*rotation[4];
                                    double newSatelliteZ=satellitePosition.getSatellitePosition_Z();
                                    satellitePosition.setSatellitePosition_X(newSatelliteX);
                                    satellitePosition.setSatellitePosition_Y(newSatelliteY);
                                    satellitePosition.setSatellitePosition_Z(newSatelliteZ);
                                    //地球自转改正后的卫星与测站的几何距离
                                    double newDistanceBetweenSatelliteAndStation=Math.sqrt((satellitePosition.getSatellitePosition_X()-stationPosition.getxPosition())
                                            *(satellitePosition.getSatellitePosition_X()-stationPosition.getxPosition())+ (satellitePosition.getSatellitePosition_Y()-stationPosition.getyPosition())
                                            *(satellitePosition.getSatellitePosition_Y()-stationPosition.getyPosition())+(satellitePosition.getSatellitePosition_Z()-stationPosition.getzPosition())
                                            *(satellitePosition.getSatellitePosition_Z()-stationPosition.getzPosition()));
                                    //地球自转改正后的根据几何位置求出的信号传播时间
                                    newTimeSpreadOfSignal=newDistanceBetweenSatelliteAndStation/SPEED_OF_LIGHT_MPS;
                                    changeTimeSpreadSignal=Math.abs(newTimeSpreadOfSignal-timeSpreadOfSignal);
                                    timeSpreadOfSignal=newTimeSpreadOfSignal;
                                    distanceBetweenSatelliteAndStation=newDistanceBetweenSatelliteAndStation;

                            }while(changeTimeSpreadSignal>=ACCURACY_TOLERANCE);
                            //卫星截至高度角计算
                            double elevationDegrees=ElevationDegreesCalculate.getElevationDegrees(satellitePosition,stationPosition);
                            mapOfSatelliteElevationDegrees.put(obs.getPrn(),elevationDegrees);
                            //系数与余数项列表
                            ListlistOfCoefficientAndConstant=new ArrayList<>();
                            //计算X方向余弦
                            double cosineOfDirectionX=(stationPosition.getxPosition()-satellitePosition.getSatellitePosition_X())/distanceBetweenSatelliteAndStation;
                            listOfCoefficientAndConstant.add(cosineOfDirectionX);
                            //计算Y方向余弦
                            double cosineOfDirectionY=(stationPosition.getyPosition()-satellitePosition.getSatellitePosition_Y())/distanceBetweenSatelliteAndStation;
                            listOfCoefficientAndConstant.add(cosineOfDirectionY);
                            //计算Z方向余弦
                            double cosineOfDirectionZ=(stationPosition.getzPosition()-satellitePosition.getSatellitePosition_Z())/distanceBetweenSatelliteAndStation;
                            listOfCoefficientAndConstant.add(cosineOfDirectionZ);
                            listOfCoefficientAndConstant.add(1.0);
                            //电离层改正模型
                            double ionosphereCorrectionMeters=0.0;
                            if(obs.getP2()!=0.0){
                                ionosphereCorrectionMeters=1.54573*(obs.getC1()-obs.getP2());
                            }
                            //采用简化的Hopfield模型对对流层进行改正
                            double troposphereCorrectionMeters=2.47/(Math.sin(elevationDegrees)+0.0121);
                            //误差方程的余数项
                            double constantOfEquation=obs.getC1()-distanceBetweenSatelliteAndStation+SPEED_OF_LIGHT_MPS*(satelliteClock-receiveClock)-ionosphereCorrectionMeters-troposphereCorrectionMeters;
                            listOfCoefficientAndConstant.add(constantOfEquation);
                            mapOfCoefficientAndConstant.put(obs.getPrn(),listOfCoefficientAndConstant);
                        }
                    }
                }
                if(validSatelliteNum>=4){
                    //系数数组
                    double[][]arraysOfCoefficient=new double[validSatelliteNum][4];
                    //余数数组
                    double[][]arraysOfConstant=new double[validSatelliteNum][1];
                    //权值数组
                    double[][]arraysOfWeight=new double[validSatelliteNum][validSatelliteNum];
                    for (int i=0;ientry:mapOfSatelliteElevationDegrees.entrySet()){
                        arraysOfWeight[counter][counter]=Math.sin(entry.getValue())*Math.sin(entry.getValue());
                        counter++;
                    }
                    counter=0;
                    for(Map.Entry>entry:mapOfCoefficientAndConstant.entrySet()){
                        arraysOfCoefficient[counter][0]=entry.getValue().get(0);
                        arraysOfCoefficient[counter][1]=entry.getValue().get(1);
                        arraysOfCoefficient[counter][2]=entry.getValue().get(2);
                        arraysOfCoefficient[counter][3]=entry.getValue().get(3);
                        arraysOfConstant[counter][0]=entry.getValue().get(4);
                        counter++;
                    }
                    //系数矩阵
                    Matrix matrixOfCoefficient=new Matrix(arraysOfCoefficient);
                    //余数矩阵
                    Matrix matrixOfConstant=new Matrix(arraysOfConstant);
                    //权矩阵
                    Matrix matrixOfWeight=new Matrix(arraysOfWeight);
                    Matrix matrixOfValues=(matrixOfCoefficient.transpose().times(matrixOfWeight).times(matrixOfCoefficient)).inverse()
                                           .times((matrixOfCoefficient.transpose().times(matrixOfWeight).times(matrixOfConstant)));
                    double[][]arraysOfValues=matrixOfValues.getArray();
//                    System.out.println(setIterationCounter);
                    parameterCorrection[0]=arraysOfValues[0][0];
                    parameterCorrection[1]=arraysOfValues[1][0];
                    parameterCorrection[2]=arraysOfValues[2][0];
                    parameterCorrection[3]=arraysOfValues[3][0];
                    double newXPosition=stationPosition.getxPosition()+parameterCorrection[0];
                    double newYPosition=stationPosition.getyPosition()+parameterCorrection[1];
                    double newZPosition=stationPosition.getzPosition()+parameterCorrection[2];
                    stationPosition.setxPosition(newXPosition);
                    stationPosition.setyPosition(newYPosition);
                    stationPosition.setzPosition(newZPosition);
                    receiveClockMeters+=parameterCorrection[3];
                    receiveClock=receiveClockMeters/SPEED_OF_LIGHT_MPS;




                }
                else {
                    //如果有效解算卫星数量小于4颗,跳出该历元，进入下一个历元解算
                    break;
                }
                //迭代次数如果超过了最大迭代次数，即发散，则抛出异常
                if(setIterationCounter>MAX_ITERATIONS){
                    try {
                        throw new Exception("Station Position Correction calculation did not converge in "
                                + MAX_ITERATIONS + " iterations");
                    } catch (Exception e) {
                        e.printStackTrace();
                        break;
                    }
                }

            }while (parameterCorrection[0]>ACCURACY_TOLERANCE2 || parameterCorrection[1]>ACCURACY_TOLERANCE2
            || parameterCorrection[2]>ACCURACY_TOLERANCE2 || parameterCorrection[3]>ACCURACY_TOLERANCE2);//迭代条件
//            System.out.println(stationPosition.getxPosition()+" "+stationPosition.getyPosition()+" "+stationPosition.getzPosition());
            //历元处理结束，输出该历元的解算结果
            STATION_SPATIAL_POSITION finalStationPosition=new STATION_SPATIAL_POSITION();
            finalStationPosition.setxPosition(stationPosition.getxPosition());
            finalStationPosition.setyPosition(stationPosition.getyPosition());
            finalStationPosition.setzPosition(stationPosition.getzPosition());
            spatial_positionMap.put(obsMap.getKey(),finalStationPosition);
        }
        return spatial_positionMap;

    }

    /**
     * 根据观测历元时刻和相应的卫星prn选择最近的卫星星历
     * @param gpst
     * @param prn
     * @return
     */
    private Ephemeris selectEphemeris(GPST gpst,String prn){
        ListephemerisList=readNavData.getEphemerisList();
        Ephemeris ephemeris=null;
        GPST temp=new GPST();
        for(Ephemeris e:ephemerisList){
            if(e.getPrn().equals(prn)){
                temp=e.getGpst();
                ephemeris=e;
                break;
            }
        }
        for(Ephemeris e:ephemerisList){
            if(e.getPrn().equals(prn)){
                GPST gpst_sub=gpst.sub(e.getGpst());
                if(gpst_sub.isLess(temp)){
                    temp=gpst_sub;
                    ephemeris=e;
                }

            }
        }
        if(ephemeris!=null){
            if(Math.abs((ephemeris.getGpst().sub(gpst)).getSeconds())<=3600.0){
                return ephemeris;
            }else {
                return null;
            }
        }else {
            return null;
        }

    }
}

• ElevationDegreesCalculate.java

package PosCalcluate;

import java.math.MathContext;

/**
 * 卫星截至高度角计算
 */
public class ElevationDegreesCalculate {
    //长半轴(m)
    private static final double SEMI_MAJOR_AXIS=6378137.0;
    //第一偏心率
    private static final double theFirstOfEccentricityRation=6.69437999013E-3;


    public static double getElevationDegrees(SatellitePosition satellitePosition,STATION_SPATIAL_POSITION stationPosition){
        double longitude=Math.atan2(stationPosition.getyPosition(),stationPosition.getxPosition());
        double scalarOfStation=Math.sqrt(stationPosition.getxPosition()*stationPosition.getxPosition()+
                stationPosition.getyPosition()*stationPosition.getyPosition()+
                stationPosition.getzPosition()*stationPosition.getzPosition());
        double theFirstOfEccentricityRationWithoutSquare=Math.sqrt(theFirstOfEccentricityRation);
        double seta=Math.atan2(stationPosition.getzPosition(),Math.sqrt(stationPosition.getxPosition()*stationPosition.getxPosition()
                     +stationPosition.getyPosition()*stationPosition.getyPosition()));
        double latitude=Math.asin(Math.sqrt((SEMI_MAJOR_AXIS*SEMI_MAJOR_AXIS-scalarOfStation*scalarOfStation*Math.cos(seta)*Math.cos(seta))/
                (SEMI_MAJOR_AXIS*SEMI_MAJOR_AXIS-scalarOfStation*scalarOfStation*Math.cos(seta)*Math.cos(seta)*theFirstOfEccentricityRationWithoutSquare*theFirstOfEccentricityRationWithoutSquare)));
        double newLatitude=Math.atan(Math.tan(seta)*(1+SEMI_MAJOR_AXIS*theFirstOfEccentricityRationWithoutSquare*theFirstOfEccentricityRationWithoutSquare
        *Math.sin(latitude)/satellitePosition.getSatellitePosition_Z()/Math.sqrt(1-theFirstOfEccentricityRationWithoutSquare*theFirstOfEccentricityRationWithoutSquare*
        Math.sin(latitude)*Math.sin(latitude))));
        while (Math.abs(newLatitude-latitude)>0.01*Math.PI/180.0/3600.0){
            latitude=newLatitude;
            newLatitude=Math.atan(Math.tan(seta)*(1+SEMI_MAJOR_AXIS*theFirstOfEccentricityRationWithoutSquare*theFirstOfEccentricityRationWithoutSquare*Math.sin(latitude)
            /stationPosition.getzPosition()/Math.sqrt(1-theFirstOfEccentricityRationWithoutSquare*theFirstOfEccentricityRationWithoutSquare*Math.sin(latitude)*Math.sin(latitude))));
        }
        double H=scalarOfStation*Math.cos(seta)/Math.cos(latitude)-(SEMI_MAJOR_AXIS/Math.sqrt(1-theFirstOfEccentricityRationWithoutSquare*theFirstOfEccentricityRationWithoutSquare
        *Math.sin(latitude)*Math.sin(latitude)));
        double differenceBetweenStationAndSatellite_X=satellitePosition.getSatellitePosition_X()-stationPosition.getxPosition();
        double differenceBetweenStationAndSatellite_Y=satellitePosition.getSatellitePosition_Y()-stationPosition.getyPosition();
        double differenceBetweenStationAndSatellite_Z=satellitePosition.getSatellitePosition_Z()-stationPosition.getzPosition();
        double NB=-Math.sin(latitude)*Math.cos(longitude)*differenceBetweenStationAndSatellite_X-Math.sin(latitude)*Math.sin(longitude)*differenceBetweenStationAndSatellite_Y
                +Math.cos(latitude)*differenceBetweenStationAndSatellite_Z;
        double EB=-Math.sin(longitude)*differenceBetweenStationAndSatellite_X+Math.cos(longitude)*differenceBetweenStationAndSatellite_Y;
        double UB=Math.cos(latitude)* Math.cos(longitude)*differenceBetweenStationAndSatellite_X+Math.cos(latitude)*Math.sin(longitude)*differenceBetweenStationAndSatellite_Y
                +Math.sin(latitude)*differenceBetweenStationAndSatellite_Z;
        double SAB=Math.sqrt(NB*NB+EB*EB+UB*UB);
        return Math.asin(UB/SAB);
    }
}

主要问题是对卫星发射概略时刻的迭代，以及在间接平差中，观测值非线性方程通过泰勒公式展开保留一阶项忽略高阶项时高阶项误差的迭代计算。在误差改正中需要对地球自转改正、电离层改正以及对流层改正。对于对流层改正通常采用的是简化的霍夫菲尔德模型，电离层采用的是双频伪距观测值组合模型。

计算部分结果如下：

测站的参考坐标为X:-2148744.8493  Y:4426641.8937 z: 4044656.2430,大部分历元计算结果与参考坐标相差在米级，有些历元可能因为观测条件出现问题而偏差几百米，所以可以剔除该历元。