Слишком большое дельта получается, по отношению к заголовочному файлу:
Вот метод, который находит координаты приемника: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;
}
