G
G
gibsonen2018-01-08 17:53:46
satellite navigation
gibsonen, 2018-01-08 17:53:46

Method that finds receiver coordinates, error?

Too big delta is obtained, in relation to the header file:
Here is a method that finds the coordinates of the receiver:

public class CalcPosition {
    private static final Integer c = 299792458;
    private static final Double eRot = 7.2921151467e-5;

    public static Matrix getPosition(List<DataObs> obs, List<SatelliteObject> nav, LocalDateTime dt) throws NullPointerException {

        //нахожу по дате проделанные наблюдения
        DataObs rinexObs = RinexObs.searchTimeObs(obs, dt);
        //получаю данные наблюдения спутников
        List<Observation> observations = rinexObs != null ? rinexObs.getObservations() : null;
        List<BigDecimal> pr = new ArrayList<>();
        List<Matrix> xSv = new ArrayList<>();
        Matrix xMatr = null;
        if (observations != null) {
            for (Observation observation : observations) {
                //нахожу id спутника по наблюдению
                Integer id = RinexObs.getId(observation);
                //ищу в навигационных данных(nav) дату dt и спутник с id
                SatelliteObject navigation = Nav.getNavTimeID(nav, dt, id);
                //в файле присутствует тип спутника R, но я использую только G, если спутник найден
                if (navigation != null && !observation.getSatelliteTypeId().getType().equals("R")) {
                    //получаю PRN, EPOCH,SV CLK
                    SatellitePRN sPRN = navigation.getSatellitePRN();
                    //из наблюдения получая C1
                    BigDecimal c1 = observation.getC1();
                    //получаю время (оно будет равно dt)
                    LocalDateTime epoch = rinexObs.getTime();
                    Long obsTimeMod = (long) (3600 * (24 + epoch.getHour()) + 60 * epoch.getMinute() + epoch.getSecond());
                    BigDecimal tmp = c1.divide(BigDecimal.valueOf(c),11, RoundingMode.HALF_UP);
                    //time = obsTimeMod - C1/299792458; - по формуле
                    Double time = obsTimeMod - tmp.doubleValue();
                    //здесь по времени time и навигационных данных navigation получаю координаты спутник, Вычисление координат на момент излучения
                    ComponentsSatellite cSat = TaskCoordinates.getCoorSat(navigation, null, time);
                    //получаю время из навигационных данных
                    LocalDateTime t = navigation.getSatellitePRN().getTime();
                    Integer toc = 3600 * t.getHour() + 60 * t.getMinute() + t.getSecond();
                    Double tocTime = time - toc;
                    BigDecimal tmp1 = sPRN.getVal("сдвиг часов").add(sPRN.getVal("скорость ухода").multiply(BigDecimal.valueOf(tocTime)));
                    BigDecimal tmp2 = tmp1.add(sPRN.getVal("ускорение ухода").multiply(BigDecimal.valueOf(Math.pow(tocTime, 2))));
                    BigDecimal tmp3 = tmp2.subtract(navigation.getBroadcastOrbitSix().getTgd());
                    BigDecimal dltT = tmp3.add(BigDecimal.valueOf(cSat.getDltTr()));

                    //Коррекция псевдодальности, компенсирующая уход часов НКА, т.е приводящая измерения к диной
                    //шкале времени - шкале времени GPS
                    tmp1 = dltT.multiply(BigDecimal.valueOf(c));
                    tmp2 = c1.add(tmp1);
                    //заношу это в лист для сохранения
                    pr.add(tmp2);
                    //Коррекция координат НКА, учитывающая вращение Земли за время от момента излучения, на
                    //который посчитаны координаты НКА, до Момента приём сигнала
                    BigDecimal deltaT = tmp2.divide(BigDecimal.valueOf(c), RoundingMode.HALF_UP);
                    BigDecimal aoR = deltaT.negate().multiply(BigDecimal.valueOf(eRot));
                    //инициалиизруем и обнуляем матрицу mor
                    double[][] mor = getZeroMatr(3, 3);
                    mor[0][0] = Math.cos(aoR.doubleValue());
                    mor[1][1] = mor[0][0];
                    mor[0][1] = Math.sin(aoR.doubleValue());
                    mor[1][0] = -mor[0][1];
                    mor[2][2] = 1.0;
                    Matrix matrix = new Matrix(mor);
                    Matrix transMor = matrix.transpose();
                    double[][] coordSat = getVector(cSat.getSatelliteCoordinates());
                    Matrix coordSatVector = new Matrix(coordSat);

                    xSv.add(coordSatVector.times(transMor));
                }
                //Анализ данных на достаточность для обработки
                if (xSv.size() == 4) {
                    break;
                }
            }
            if (xSv.size() < 4)
                return null;
            if (!xSv.isEmpty()) {
                //Первое решение НЗ
                // Формирование начальных значений
                double[][] x = getZeroMatr(4, 1);
                xMatr = new Matrix(x);
                double[][] r = getZeroMatr(4, 1);
                double[][] h = getZeroMatr(4, 4);
                double[][] dPr = getZeroMatr(4, 1);
                double[][] xSt = getZeroMatr(1, 3);
                xSt[0][0] = -2407751;
                xSt[0][1] = -4706536.65;
                xSt[0][2] = 3557571.41;
                for (int i = 0; i < 10; i++) {
                    //Формирование матрицы наблюдений и разницы измеренных и расчётных значений
                    for (int z = 0; z < 4; z++) {
                        r[z][0] = Math.sqrt(Math.pow(getDouble(xSv, z, 0) - xMatr.get(0, 0), 2) + Math.pow(getDouble(xSv, z, 1) - xMatr.get(1, 0), 2) + Math.pow(getDouble(xSv, z, 2) - xMatr.get(2, 0), 2));
                        xSv.get(z).get(0, 0);
                        h[z][0] = (getDouble(xSv, z, 0) - xMatr.get(0, 0)) / r[z][0];
                        h[z][1] = (getDouble(xSv, z, 1) - xMatr.get(1, 0)) / r[z][0];
                        h[z][2] = (getDouble(xSv, z, 2) - xMatr.get(2, 0)) / r[z][0];
                        h[z][3] = 1;
                        dPr[z][0] = pr.get(z).doubleValue() - r[z][0] - xMatr.get(3, 0);
                    }
                    //Вычисление приращения координат
                    //d_X = (H'*H)^-1*H'*d_PR;
                    Matrix matrixH = new Matrix(h);
                    //H'
                    Matrix hTrans = matrixH.transpose();
                    //H'*H
                    Matrix hTransH = hTrans.times(matrixH);
                    //(H'*H)^-1
                    Matrix invHtransH = hTransH.inverse();
                    //(H'*H)^-1*H'
                    Matrix mulInvTransH = invHtransH.times(hTrans);
                    Matrix dPrMatr = new Matrix(dPr);
                    //(H'*H)^-1*H'*d_PR
                    Matrix dX = mulInvTransH.times(dPrMatr);
                    //X = X - dX;
                    xMatr = xMatr.minus(dX);
                }
            }
        } else {
            System.out.println("Не нашло наблюдений спутника");
        }
        return xMatr;
    }

Answer the question

In order to leave comments, you need to log in

Didn't find what you were looking for?

Ask your question

Ask a Question

731 491 924 answers to any question