1. ВНИМАНИЕ! В течении пары дней +- будет переезд форума на более защищённый сервер. Возможны периодические перебои в работе.

расчет координат точки на местности используя исходные данные модуля навигатора

Discussion in 'ПЕСОЧНИЦА' started by Дед 005, Jan 10, 2019.

Page 7 of 7
  1. Дед 005

    Дед 005 Форумчанин

    Mar 22, 2019 #121
    Вот изначально, я считал что алгоритм выглядит так:
    1. На основании данных взятых из эфемерид, считаем координаты спутников.
    2. На основании коэффициентов коррекции синхроимпульсов af0,af1,af2, полученных из сообщения спутников, считаем координаты спутников снова, - более точно, введя поправку, - коррекцию синхроимпульсов.
    3. На основании псевдодальностей, взятых из сообщения навигатора, и вычисленных координат спутников считаем свои координаты на местности. Методом наименьших квадратов. Вычисления повторяются, несколько раз, до тех пор пока величина ошибки не станет меньше 1 см. (пишут, что обычно это от 3 до 5 раз). Изначально координаты считаются равными нулю. При вычислениях при каждой итерации координаты получаются как накапливаемая сумма ошибок. Одновременно с этим считается dTclck – ошибка часов приемника.
    4. На основании вычисленных координат на местности и координат спутников,вычисленных ранее, исходя из выбранной модели, считаем ионосферную и тропосферную поправки для каждого спутника.
    5. По результатам вычислений делается коррекция псевдодальностей для каждого спутника Pcor[prn] = Praw[prn] + dTclck * c - dTiono * c – dRtrop; где dTclck – ошибка часов приемника.

    1. На основании скорректированных псевдодальностей, и вычисленных ранее координат спутников снова считаем свои координаты на местности. Методом наименьших квадратов. Вычисления повторяются, несколько раз, до тех пор пока величина ошибки не станет меньше 1 см. Изначально координаты считаются равными нулю. При вычислениях при каждой итерации координаты получаются как накапливаемая сумма ошибок.
    2. Результат на этом считается окончательным.
     
    Евгений_GPS likes this.
  2. Дед 005

    Дед 005 Форумчанин

    Apr 1, 2019 #122
    Применил какие нашел в литературе поправки, получился код. Только где то в самом начале что то неправильно, - потому что посчитанные координаты отличаются от реальных примерно на 100 километров. Поправки исправляют незначительно, а ошибка грубая. В программе уже есть эфемериды и псевдодальности, полученные от модуля навигатора в месте с конкретными координатами. Может кто нибудь из уважаемых геодезистов посмотрит, увидит что то и скажет что нибудь умное......... завтра. а то сегодня первое апреля. мало ли......
    Code:
    #include <stdio.h>
#include <stdlib.h>
#include <math.h>
 
 
//SV-23 23/02/19 8:00 UTC0
unsigned char stroka[120]={0x00,0xB5,0x62,0x0B,0x31,0x68,0x00,0x17,0x00,0x00,0x00,0x04,0x20,0xB2,0x00,0x00,0x40,
0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD5,0x00,
0x00,0x00,0x98,0x85,0x53,0x00,0x0C,0x00,0x00,0x00,0xC0,0x45,0xE6,0x00,0x74,0x02,
0x53,0x00,0xC7,0x20,0x33,0x00,0x54,0xC5,0xDB,0x00,0x06,0xE1,0x01,0x00,0x92,0xFF,
0x90,0x00,0xA1,0xA9,0x18,0x00,0x05,0x1C,0x0D,0x00,0x00,0x98,0x85,0x00,0x22,0xB7,
0x00,0x00,0x0F,0x51,0x55,0x00,0x26,0x19,0x00,0x00,0x95,0x4D,0x71,0x00,0xA2,0x99,
0x11,0x00,0x47,0x1D,0x7D,0x00,0x91,0xA8,0xFF,0x00,0xF8,0xFC,0x53,0x00,0x64,0x2B}
;
 
//Time gps: 37800  SV: 9
//SV:   1   Q: 7  Psr: 25109839.64387868
//SV:   7   Q: 4  Psr: 21056670.78789259
//SV:   8   Q: 6  Psr: 21555412.29389584
//SV:  30   Q: 4  Psr: 20686354.37529096
// SV1   24/02/19 10:30
unsigned char stroka1[120]={0x00,0xB5,0x62,0x0B,0x31,0x68,0x00,0x01,0x00,0x00,0x00,0x04,0x10,0x0E,0x00,0x00,0x80,
0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0C,0x00,
0x00,0x00,0x8C,0x0A,0x94,0x00,0xC1,0xFF,0x00,0x00,0x14,0xD3,0xE9,0x00,0x43,0x01,
0x94,0x00,0xFF,0x78,0x30,0x00,0xEA,0xF7,0xEB,0x00,0x04,0x45,0x01,0x00,0x45,0xD5,
0x4D,0x00,0xA1,0xF0,0x0F,0x00,0x77,0x40,0x0D,0x00,0x00,0x8C,0x0A,0x00,0xC9,0x1C,
0x00,0x00,0xC9,0x0B,0x07,0x00,0x27,0x27,0x00,0x00,0xE2,0xAD,0xB1,0x00,0x1B,0x5A,
0x1E,0x00,0x58,0x33,0xD6,0x00,0xAA,0xA6,0xFF,0x00,0x20,0x11,0x94,0x00,0x57,0x50};
 
// SV7   24/02/19 10:30
unsigned char stroka2[120]={0x00,0xB5,0x62,0x0B,0x31,0x68,0x00,0x07,0x00,0x00,0x00,0x04,0x10,0x0E,0x00,0x00,0x80,
0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xE8,0x00,
0x00,0x00,0x8C,0x0A,0x0E,0x00,0xC0,0xFF,0x00,0x00,0xE0,0x78,0x03,0x00,0x99,0x08,
0x0E,0x00,0xE6,0x53,0x34,0x00,0xB9,0x52,0xAD,0x00,0x06,0x79,0x07,0x00,0xE6,0xAB,
0x43,0x00,0xA1,0x71,0x0E,0x00,0x32,0xDD,0x0C,0x00,0x00,0x8C,0x0A,0x00,0x49,0x3A,
0x00,0x00,0x48,0xF6,0x6F,0x00,0x26,0x6B,0x00,0x00,0xB3,0xD0,0xF1,0x00,0x9B,0x78,
0x1E,0x00,0xFF,0xE4,0x1A,0x00,0xDB,0xA3,0xFF,0x00,0x44,0x01,0x0E,0x00,0x23,0xA6};
 
// SV8   24/02/19 10:30
unsigned char stroka3 [120]={0x00,0xB5,0x62,0x0B,0x31,0x68,0x00,0x08,0x00,0x00,0x00,0x04,0x10,0x0E,0x00,0x00,0x80,
0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0B,0x00,
0x00,0x00,0x8C,0x0A,0x0D,0x00,0xF4,0xFF,0x00,0x00,0x18,0x72,0xEE,0x00,0x86,0xFA,
0x0D,0x00,0x64,0x9C,0x2C,0x00,0xED,0xEA,0xF4,0x00,0x02,0x58,0xFB,0x00,0xD7,0xE4,
0x33,0x00,0xA1,0xA7,0x14,0x00,0x26,0xA7,0x0D,0x00,0x00,0x8C,0x0A,0x00,0x9D,0x0A,
0x00,0x00,0x3B,0xD2,0x92,0x00,0x27,0x0B,0x00,0x00,0xA5,0x12,0x88,0x00,0xF2,0x46,
0x18,0x00,0x7B,0x32,0x67,0x00,0xA2,0xA9,0xFF,0x00,0xD4,0xFD,0x0D,0x00,0x3B,0x53};
 
 
// SV30  24/02/19 10:30
unsigned char stroka4 [120]={0x00,0xB5,0x62,0x0B,0x31,0x68,0x00,0x1E,0x00,0x00,0x00,0x04,0x10,0x0E,0x00,0x00,0x80,
0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x08,0x00,
0x00,0x00,0x8C,0x0A,0x34,0x00,0xC7,0xFF,0x00,0x00,0xD4,0x62,0xF8,0x00,0x64,0x08,
0x34,0x00,0xE7,0xB8,0x36,0x00,0xD4,0x1B,0x3C,0x00,0x01,0x16,0x07,0x00,0x95,0x74,
0xE1,0x00,0xA1,0xE1,0x0D,0x00,0x39,0xFF,0x0C,0x00,0x00,0x8C,0x0A,0x00,0x4A,0x23,
0x00,0x00,0x10,0x48,0xA4,0x00,0x26,0xF2,0xFF,0x00,0xEE,0x89,0x5A,0x00,0x84,0x3E,
0x1E,0x00,0x4C,0xF5,0xD7,0x00,0xB8,0xA3,0xFF,0x00,0x34,0x02,0x34,0x00,0x46,0x7D};
 
unsigned char Klobuchar_RAW [81]={0x00,0xB5,0x62,0x0B,0x02,0x48,0x00,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,
0x30,0xBE,0x00,0x00,0x00,0x00,0x00,0x00,0x08,0xBD,0x00,0xF0,0x00,0x00,0xFD,0x07,
0x12,0x00,0x89,0x07,0x07,0x00,0x12,0x00,0x00,0x00,0x00,0x00,0x40,0x32,0x00,0x00,
0x00,0x32,0x00,0x00,0x80,0xB3,0x00,0x00,0x80,0xB3,0x00,0x00,0xB0,0x47,0x00,0x00,
0x00,0x00,0x00,0x00,0x40,0xC8,0x00,0x00,0x80,0xC7,0x07,0x00,0x00,0x00,0x0A,0x1F};
 
 
const double ae_WGS84 = 6378137.0;  // больша¤ (экваториальна¤) полуось общеземного эллипсоида в системе WGS-84, м
 
double t;
double c = 2.99792458E+8; // скорость света в вакууме,(2.99792458 x 108 м/с);
double X1;
double Y1;
double Z1;
double X_wgs84; // координаты спутника в системе WGS84
double Y_wgs84;
double Z_wgs84;
 
double a_bp = 0; // A_bp - это а - большая полуось
double n_o = 0;
double gp_earth = 398600500000000; // gravitational parameter of the earth
double n;
double M_sr; // M_sr - это в документе M - средняя аномалия
double t_k;
double E1;
double nash; // Ф - невозмущенный аргумент широты
double v;
double OMEGA_r; // OMEGA_r - в старой формуле называлась OMEGA
double OMEGA_zemli = 7.2921151467e-5;
double u_eph;
double r_eph;
double i_eph; // бывшие u, r, i
double sigma_u;
double sigma_r;
double sigma_i;
double a0,a1,a2,a3,b0,b1,b2,b3; // коэффициэнты модели клобучара
 
 
double E;                       // эксцентрическая аномалия
double Ti[4];                   // тропосферная задержка
double Ii[4];                   // ионосферная задержка
double dTiono;
double dTclck;
double tki[4];
double dti[4];
double Si[4];                   // поправки на эффект Саньяка
double AZ[4];
double El[4];
double GPSTime[4];              // GPSTime - время проведения измерений UTC с начала суток,с
double rhoi[4];                 // геометрическая дальность
double taui[4];
double thetai[4];
double  PR_sv[4];
double rel[3]={0,0,0};
double user[4]={0,0,0,1};       // предпологаемые координаты юзера [x y z t]
double R_total[4]={0,0,0,0};
float det_A;                    // определитель (детерминант) матрицы А
double A[4][4] = {
          {-4,-2,-7, 8},
                  { 2, 7, 4, 9},
                  { 2, 0, 6,-3},
                  { 6, 4,-10,-4}
                };
 
double sputnik[4][4] = { // [н спутника]  [X Y Z Rg]
                       {1139707.708083462,25373569.63314989,2411070.296105003,20300102.455},
                       {-6836585.938994921,19755060.70418419,-14462909.36471641,24661248.247},
                       {8502082.457108100,16110270.12902002,17986294.48759295,19481828.202},
                       {9729760.15552335,10296809.7397028,-21148765.5388340,25701752.532}
                        };
 
double matrix_4x1[4]={2,6,8,11};
double   Delta_X;
double   Delta_Y;
double   Delta_Z;
double   Delta_t0;
 
double L;  // широта
double B;  // долгота
double H;  // высота
 
 
double pi = 3.1415926535898;
void zapolnenie_matrix();   // заполнение матрицы исходными данными
void minor_4x4();           // считаем минор матрицы 4х4
void calculation_delta();   // вычисление Delta_X,Delta_Y,Delta_Z,Delta_t0
 
void pseudorange_calculation();   // вычисление координат пользователя
void Transformatik_coordinates(); // из wgs84 в геодезические L,B,H дробные
void Rtotal();
void decoder_Ephemerid();   // декодируем все переменные их эфемерид
void decoder_EPH(int x);    // декодер эфемерид от модуля навигатора по одному
                            //void decoder_EPH2(int x);
void decoder_Klobuchar();
void calk_tki_1();          // первичный расчет времени tki
void calk_tki_2();          // повторный Расчёт времени tki
void calk_dti();            // Расчёт полинома коррекции времени спутника
void calk_rhoi();           // Расчёт геометрической дальности
void calk_Si();             // Расчёт поправки на эффект Саньяка
void calk_taui();           // Находим время распространения сигнала
void calk_thetai();         // Находим угол поворота Земли за время taui
void calk_sputnik1();       // Коррекция координат спутников
void calk_Elevation();      // Находим угол места Elevationi
void calk_Azimut();         // Находим azimut для каждого спутника
void calk_Ti();             // Находим тропосферную задержку Ti
void calk_Ii();             // Находим ионосферную задержку Ii
void Klobuchar(double AZ, double El);
double GCAT(double El);
void correct_pseudorange(); // коррекция псевдодальностей
void calk_SV1_4();          // Расчёт координат спутников без поправок
void  calk2_SV1_4();        // Расчёт координат спутников c поправками
void calk_SV(int x);
void calk_SV1(int x);
void const_eph1();
float BytToSingle (unsigned char x1,unsigned char x2,unsigned char x3,unsigned char x4);
struct eph
{
    double Mo;
    double delta_n;
    double E_a;
    double square_big_axis;
    double IDOT;
    double t_oe;
    double Cuc;
    double Cus;
    double Crc;
    double Crs;
    double Cic;
    double Cis;
    double omega_0;
    double io;
    double OMEGADOT;
    double omega;
    double t_gd;
    double t_oc;
    double af2;
    double af1;
    double af0;
};
struct eph GP[32];
 
struct eph_subfrem
{
    unsigned char a;
    unsigned char b;
    unsigned char c;
};
 
struct eph_subfrem subfrem[4][11];
 
int main()
{
    decoder_Ephemerid();    // декодируем переменные их эфемерид
    decoder_Klobuchar();    // получаем коэффициэнты Клобучара
    // заполняем псевдодальности
    PR_sv[0] = 25109839.64387868;       // SV 1
    PR_sv[1] = 21056670.78789259;       // SV 7
    PR_sv[2] = 21555412.29389584;       // SV 8
    PR_sv[3] = 20686354.37529096;       // SV 30
  // Расчёт времени tki = t_наблюдений(приёмника) - t_опорнаяi(эфемерид)
     t = 37800;
     calk_tki_1();     // первичный Расчёт времени tki
     calk_SV1_4();     // Расчёт координат спутников без поправок
     pseudorange_calculation();   // вычисление координат пользователя
     Transformatik_coordinates(); // из wgs84 в геодезические L,B,H
     printf("L1 : %.15f \n",L);
     printf("B1 : %.15f \n",B);
     printf("H1 : %.15f \n",H);
//Расчёт полинома коррекции времени спутника делается в функции calk2_SV1_4
     calk_rhoi();      // Расчёт геометрической дальности
     calk_Si();        // Расчёт поправки на эффект Саньяка
     calk_taui();      // Находим время распространения сигнала
     calk_thetai();    // Находим угол поворота Земли за время taui
     calk_sputnik1();  // Корректируем координаты спутника на поворот Земли за время taui
     calk_Elevation(); // Находим угол места Elevationi
     calk_Azimut();    // Находим azimut для каждого спутника
     calk_Ii();        // Находим ионосферную поправку Ii
     calk_Ti();        // Находим тропосферную поправку Ti
     correct_pseudorange();       // коррекция псевдодальностей
     calk_tki_2();                // Заново рассчитываем tki
     calk2_SV1_4();    // Расчёт координат спутников с поправками
     pseudorange_calculation();   // вычисление координат пользователя
     Transformatik_coordinates(); // из wgs84 в геодезические L,B,H дробные
     printf("L2 : %.15f \n",L);
     printf("B2 : %.15f \n",B);
     printf("H2 : %.15f \n",H);
 
     printf("\n - * ---- end ---- * -\n"); // координаты которые нужно получить
     printf("Lreal : %.15f \n",82.954844); // 82.954844,
     printf("Breal : %.15f \n",55.105495); // 55.105495,  широта,
 
     printf("\n - * ---- end ---- * -\n"); // разность между посчитанными и реальными
     printf("delta_L : %.15f \n",L-82.954844);
     printf("delta_B : %.15f \n",B-55.105495);
 
        double DD,MM,SS,a_drb,a_ish;
         a_ish = L;
         a_drb = modf(a_ish, &DD); // DD - градусы
         a_ish = a_drb * 60;
         a_drb = modf(a_ish, &MM); // MM - инуты
         a_ish = a_drb * 60;
         a_drb = modf(a_ish, &SS); // SS - секунды
         printf("E : %.0f,%.0f,%.0f \n",DD,MM,SS);
 
         a_ish = B;
         a_drb = modf(a_ish, &DD); // DD - градусы
         a_ish = a_drb * 60;
         a_drb = modf(a_ish, &MM); // MM - инуты
         a_ish = a_drb * 60;
         a_drb = modf(a_ish, &SS); // SS - секунды
         printf("N : %.0f,%.0f,%.0f \n",DD,MM,SS);
 
return 0;
}
//////////////////////////////////////////////////
    void calk_Ii()
   {
     Klobuchar( AZ[0],El[0]);
     Ii[0] = dTiono;
     Klobuchar( AZ[1],El[1]);
     Ii[1] = dTiono;
     Klobuchar( AZ[2],El[2]);
     Ii[2] = dTiono;
     Klobuchar( AZ[3],El[3]);
     Ii[3] = dTiono;
   }
/////////////////////////////////////////////////////////////////////
//Следующие этапы расчета извлечены из «Ионосферных эффектов на GPS» Дж. А. Klobuchar. Все угловые единицы находятся в полукругах (!),
// El - высота, Az - азимут, Latu - пользовательская широта B
// Lonu пользовательская долгота L.
 void Klobuchar(double Az, double El)
 {
  double phi,Lati,Loni,Latm,Tkl,F,x,Latu,Lonu;
  Latu = B / pi;
  Lonu = L / pi;
  Az = Az / pi;
  El = El / pi;
 
  phi = 0.0137 / (El + 0.11) - 0.022;  // Earth-centered angle [sc]
  Lati = Latu + phi * cos(Az);   //  Subionospheric lattitude [sc] (1)
    if(Lati > 0.416)
  {
Lati = 0.416;
  }
if(Lati < - 0.416)
  {
Lati = - 0.416;
  }
Loni = Lonu + phi * sin(Az) / cos(Lati);  // Subionospheric longitude [sc] (1)
Latm = Lati + 0.064 * cos(Loni - 1.617);  // Geom. lat. of the iono inters. point [sc] (1)
Tkl = 4.32E+4 * Loni + t;  // Local time at sunionospheric point [sec] (2)
 
    if(Tkl > 86400.0)
  {
Tkl -= 86400;
  }
if(Tkl < 0.0)
  {
        Tkl += 86400;
      }
F = 1.0 + 16.0 * pow((0.53 - El),3);   //  Slant factor [unity]
x = 2 * pi * (t - 50400) / (b0 + b1 * Latm + b2 * pow(Latm,2) + b3 * pow(Latm,3));    // 'x' [rad]
  // Ionospheric delay correction [sec]:
   if(fabs(x) > 1.57)
    {
      dTiono = F * 5.0E-9;
    }
   else
    {
     dTiono = F * (5.0E-9 + (a0 + a1 * Latm + a2 * pow(Latm,2) + a3 * pow(Latm,3)) * (1.0 - pow(x,2) / 2 + pow(x,4) / 24)) ;
    }
 }
//////////////////////////////////////////////////////
 void pseudorange_calculation()  // вычисление координат пользователя
    {
       sputnik[0][3] = PR_sv[0];  // заполняем псевдодальности SV 1
       sputnik[1][3] = PR_sv[1];  // SV 7
       sputnik[2][3] = PR_sv[2];  // SV 8
       sputnik[3][3] = PR_sv[3];  // SV 30
 
      int f;
      for(f=0;f<10;f++)
        {
          zapolnenie_matrix();   // заполнение матрицы исходными данными
          minor_4x4();           // считаем минор матрицы 4х4
          calculation_delta();   // вычисление Delta_X,Delta_Y,Delta_Z,Delta_t0
          if((fabs(Delta_X)<=0.001)&&(fabs(Delta_Y)<=0.001)&&(fabs(Delta_Z)<=0.001))
          break;
        }
    }
//////////////////////////////////////////////////////////////////
 void  calk_SV1_4()      // Расчёт координат спутникщв на время tki
  {                      // без поправок  спутники 1,7,8,30
    calk_SV2(1);
    sputnik[0][0] = X_wgs84;
    sputnik[0][1] = Y_wgs84;
    sputnik[0][2] = Z_wgs84;
 
    calk_SV2(2);
    sputnik[1][0] = X_wgs84;
    sputnik[1][1] = Y_wgs84;
    sputnik[1][2] = Z_wgs84;
 
    calk_SV2(3);
    sputnik[2][0] = X_wgs84;
    sputnik[2][1] = Y_wgs84;
    sputnik[2][2] = Z_wgs84;
 
    calk_SV2(4);
    sputnik[3][0] = X_wgs84;
    sputnik[3][1] = Y_wgs84;
    sputnik[3][2] = Z_wgs84;
  }
/////////////////////////////////////////////////////////////////////
 void  calk2_SV1_4()     // Расчёт координат спутникщв на время tki
  {                      // с поправками  спутники 1,7,8,30
    calk_SV1(1);
    sputnik[0][0] = X_wgs84;
    sputnik[0][1] = Y_wgs84;
    sputnik[0][2] = Z_wgs84;
 
    calk_SV1(2);
    sputnik[1][0] = X_wgs84;
    sputnik[1][1] = Y_wgs84;
    sputnik[1][2] = Z_wgs84;
 
    calk_SV1(3);
    sputnik[2][0] = X_wgs84;
    sputnik[2][1] = Y_wgs84;
    sputnik[2][2] = Z_wgs84;
 
    calk_SV1(4);
    sputnik[3][0] = X_wgs84;
    sputnik[3][1] = Y_wgs84;
    sputnik[3][2] = Z_wgs84;
  }
///////////////////////////////////////////////////////////////////
  void calk_SV1(int x) // расчет положения спутника по его эфемеридам
  {                    // с применением поправок
    double E;
    a_bp = pow(GP[x].square_big_axis,2); // вычмсляем большую полуось
    n_o = sqrt(gp_earth/(pow(a_bp,3)));
    n = n_o + GP[x].delta_n;  // находим среднее движение
    ////////////////////////коррекция синхроимпульсов
       double F,d_tr, dti; //
       F = - 4.442807633E-10;                                  // (-2 * sqrt( gp_earth)) / (c * c);
        d_tr = F * GP[x].E_a * GP[x].square_big_axis * sin (E); // E_a - это бывшая e
        dti = GP[x].af0 + (GP[x].af1*(t - GP[x].t_oc)) + (GP[x].af2 * pow (t -GP[x].t_oc,2)) + d_tr - GP[x].t_gd;
 
     t_k = tki[x] - dti;
       M_sr = GP[x].Mo + (n * t_k); // определяем среднюю аномалию
 
        E1 = M_sr;  // начальное условие для вычислений уравнения Кеплера
        for (;;)    // находим эксцентрическую аномалию через уравнение Кеплера
         {
           E1 = M_sr + (GP[x].E_a * sin(E)); // Kepler_equation(x);
           if(fabs(E1 - E) > 1e-19)
             E = E1;                         // E_a - это бывшая e
           else
             break;
         }
    v = (atan((sqrt((1 + GP[x].E_a)/(1 - GP[x].E_a))) * tan(E/2)))*2;
    nash = v + GP[x].omega; // находим невозмущенный аргумент широты
 
    sigma_u = GP[x].Cus * sin(2 * nash) + GP[x].Cuc * cos(2 * nash);
    sigma_r = GP[x].Crs * sin(2 * nash) + GP[x].Crc * cos(2 * nash);
    sigma_i = GP[x].Cis * sin(2 * nash) + GP[x].Cic * cos(2 * nash);
 
    u_eph = nash + sigma_u;
    r_eph = a_bp * (1 - GP[x].E_a * cos(E)) + sigma_r;
    i_eph = GP[x].io + sigma_i + GP[x].IDOT * t_k;
 
    X1 = r_eph * cos(u_eph);
    Y1 = r_eph * sin(u_eph);
 
    OMEGA_r = GP[x].omega_0 + (GP[x].OMEGADOT - OMEGA_zemli)*t_k - OMEGA_zemli*GP[x].t_oe;
 
    X_wgs84 = X1 * cos(OMEGA_r)- Y1 * cos(i_eph) * sin(OMEGA_r);
    Y_wgs84 = X1 * sin(OMEGA_r)+ Y1 * cos(i_eph) * cos(OMEGA_r);
    Z_wgs84 = Y1 * sin(i_eph);
 }
//////////////////////////////////////////////
  void calk_SV2(int x) // расчет положения спутника по его эфемеридам
  {                    // без  использования поправок
    a_bp = pow(GP[x].square_big_axis,2); // вычмсляем большую полуось
    n_o = sqrt(gp_earth/(pow(a_bp,3)));
    n = n_o + GP[x].delta_n;  // находим среднее движение
 
        t_k =  tki[x];
        M_sr = GP[x].Mo + (n * t_k); // определяем среднюю аномалию
 
        E1 = M_sr;  // начальное условие для вычислений уравнения Кеплера
        for (;;)    // находим эксцентрическую аномалию через уравнение Кеплера
         {
           E1 = M_sr + (GP[x].E_a * sin(E)); // Kepler_equation(x);
           if(fabs(E1 - E) > 1e-19)
             E = E1;
          else
         break;
         }
    v = (atan((sqrt((1 + GP[x].E_a)/(1 - GP[x].E_a))) * tan(E/2)))*2;
    nash = v + GP[x].omega; // находим невозмущенный аргумент широты
 
    sigma_u = GP[x].Cus * sin(2 * nash) + GP[x].Cuc * cos(2 * nash);
    sigma_r = GP[x].Crs * sin(2 * nash) + GP[x].Crc * cos(2 * nash);
    sigma_i = GP[x].Cis * sin(2 * nash) + GP[x].Cic * cos(2 * nash);
 
    u_eph = nash + sigma_u;
    r_eph = a_bp * (1 - GP[x].E_a * cos(E)) + sigma_r;
    i_eph = GP[x].io + sigma_i + GP[x].IDOT * t_k;
 
    X1 = r_eph * cos(u_eph);
    Y1 = r_eph * sin(u_eph);
 
    OMEGA_r = GP[x].omega_0 + (GP[x].OMEGADOT - OMEGA_zemli)*t_k - OMEGA_zemli*GP[x].t_oe;
 
    X_wgs84 = X1 * cos(OMEGA_r)- Y1 * cos(i_eph) * sin(OMEGA_r);
    Y_wgs84 = X1 * sin(OMEGA_r)+ Y1 * cos(i_eph) * cos(OMEGA_r);
    Z_wgs84 = Y1 * sin(i_eph);
 }
/////////////////////////////////////////////////////////////
 void  calk_Elevation() // Находим угол места Elevationi для каждого спутника
  {
    double El[4],Eln;
    Elevation(sputnik[0][0],sputnik[0][1],sputnik[0][2]);
    El[0] = Eln;
    Elevation(sputnik[1][0],sputnik[1][1],sputnik[1][2]);
    El[1] = Eln;
    Elevation(sputnik[2][0],sputnik[2][1],sputnik[2][2]);
    El[2] = Eln;
    Elevation(sputnik[3][0],sputnik[3][1],sputnik[3][2]);
    El[3] = Eln;
  }
/////////////////////////////////////////////////
 // Расчёт угла возвышения (угла места) над горизонтом
  // Xu, Yu, Zu - координаты точки наблюдения, м
  // x_sv, y_sv, z_sv - координаты наблюдаемой точки, м
  // возвращаемое значение - угол возвышения, рад
void Elevation(double x_sv,double y_sv,double z_sv)
  {
double Xu = user[0];
    double Yu = user[1];
    double Zu = user[2];
   double rx, ry, cosEl, Eln;
double r[3];
r[0] = x_sv - Xu;
r[1] = y_sv - Yu;
r[2] = z_sv - Zu;
rx = pow(r[0],2) + pow(r[1],2) + pow(r[2],2);
ry = pow(Xu,2) + pow(Yu,2) + pow(Zu,2);
cosEl = (r[0] * Xu + r[1] * Yu + r[2] * Zu) / sqrt(rx * ry);
if (fabs(cosEl) > 1.0)
  {
cosEl = fabs(cosEl) / cosEl;
  }
    Eln = pi/2 - acos(cosEl);
  }
////////////////////////////////////////////////////////////
 void  calk_Azimut() // Находим azimut для каждого спутника
  {
    double AZn;
    Azimuth(sputnik[0][0],sputnik[0][1],sputnik[0][2]);
    AZ[0] = AZn;
    Azimuth(sputnik[1][0],sputnik[1][1],sputnik[1][2]);
    AZ[1] = AZn;
    Azimuth(sputnik[2][0],sputnik[2][1],sputnik[2][2]);
    AZ[2] = AZn;
    Azimuth(sputnik[3][0],sputnik[3][1],sputnik[3][2]);
    AZ[3] = AZn;
  }
///////////////////////////////////////////////////////////
// Расчёт азимута одной точки относительно другой
// Xu, Yu, Zu - координаты точки наблюдения, м
// x, y, z - координаты наблюдаемой точки, м
// возвращаемое значение - азимут, рад
  void Azimuth(double x_sv, double y_sv, double z_sv)
  {
    double Xu = user[0];
    double Yu = user[1];
    double Zu = user[2];
double xy, xyz, cosl, sinl, sint, xn1, xn2, xn3, xe1, xe2;
double z1, z2, z3, p1, p2, alpha, AZn;
xy = pow(Xu,2) + pow(Yu,2);
xyz = xy + pow(Zu,2);
xy = sqrt(xy);
xyz = sqrt(xyz);
cosl = Xu / xy;
sinl = Yu / xy;
sint = Zu / xyz;
xn1 = -sint * cosl;
xn2 = -sint * sinl;
xn3 = xy / xyz;
xe1 = - sinl;
xe2 = cosl;
z1 = x_sv - Xu;
z2 = y_sv - Yu;
z3 = z_sv - Zu;
p1 = (xn1 * z1) + (xn2 * z2) + (xn3 * z3);
p2 = (xe1 * z1) + (xe2 * z2);
alpha = pi/2 - atan2(p1, p2);
if (alpha < 0.0)
  {
AZn = alpha + 2.0 * pi;
      }
else
  {
AZn = alpha;
  }
  }
///////////////////////////////////////////////////////////
 void calk_Ti()   // расчет тропосферной поправки используя модель GCAT
   {
      Ti[0] = GCAT( El[0]);
      Ti[1] = GCAT( El[1]);
      Ti[2] = GCAT( El[2]);
      Ti[3] = GCAT( El[3]);
   }
 // Модель тропосферной задержки GCAT (GPSCode Analysis Tool)
 // H - высота приёмника над уровнем моря, км
 // El - угол возвышения спутника над горизонтом, рад
 // Возвращаемое значение - тропосферная задержка, м
   double GCAT( double El)
       {
         double Zd, Zw;
         double ht = 11.231;    // высота тропопаузы, км
         if(H < -2.0 || H > ht)
           {
             return 0.0;
           }
         Zd = 2.3 * exp(-0.116E-3 * H);
         Zw = 0.1;
         return (Zd + Zw) * 1.001 / sqrt(0.002001 + pow(sin(El),2));
       }
///////////////////////////////////////////////////////////
   void calk_taui()   // 10. Находим время распространения сигнала
  {                   //  taui = (rhoi + S) / c
    taui[0] = (rhoi[0] + Si[0]) / c;
    taui[1] = (rhoi[1] + Si[1]) / c;
    taui[2] = (rhoi[2] + Si[2]) / c;
    taui[3] = (rhoi[3] + Si[3]) / c;
  }
//////////////////////////
  void  calk_thetai()    // 11. Находим угол поворота Земли за время taui
  {                         //  thetai = OMEGAi_WGS84 * taui;
    thetai[0] = OMEGA_zemli * taui[0];
    thetai[1] = OMEGA_zemli * taui[1];
    thetai[2] = OMEGA_zemli * taui[2];
    thetai[3] = OMEGA_zemli * taui[3];
  }
////////////////////////////////////////////////////////////
 void calk_Si()        // Расчёт поправки на эффект Саньяка
 {
     //Si = (xi * y - y * xi) * OMEGAi_WGS84 / c, где OMEGAi_WGS84 - угловая скорость вращения Земли
     Si[0] = (sputnik[0][0] * user[1] - user[1] * sputnik[0][0]) * OMEGA_zemli / c;
     Si[1] = (sputnik[1][0] * user[1] - user[1] * sputnik[1][0]) * OMEGA_zemli / c;
     Si[2] = (sputnik[2][0] * user[1] - user[1] * sputnik[2][0]) * OMEGA_zemli / c;
     Si[3] = (sputnik[3][0] * user[1] - user[1] * sputnik[3][0]) * OMEGA_zemli / c;
 }
/////////////////////////////////////////////////////////////
 void  calk_rhoi()      // Расчёт геометрической дальности
  {
   // Расчёт геометрической дальности rhoi = sqrt(sqr(x - xi) + sqr(y - yi) + sqr(z - zi))
   rhoi[0] = sqrt(pow((user[0] - sputnik[0][0]),2) + pow((user[1] - sputnik[0][1]),2) + pow((user[2] - sputnik[0][2]),2));
   rhoi[1] = sqrt(pow((user[0] - sputnik[1][0]),2) + pow((user[1] - sputnik[1][1]),2) + pow((user[2] - sputnik[1][2]),2));
   rhoi[2] = sqrt(pow((user[0] - sputnik[2][0]),2) + pow((user[1] - sputnik[2][1]),2) + pow((user[2] - sputnik[2][2]),2));
   rhoi[3] = sqrt(pow((user[0] - sputnik[3][0]),2) + pow((user[1] - sputnik[3][1]),2) + pow((user[2] - sputnik[3][2]),2));
  }
////////////////////////////////////////////////////////////
  // Корректируем координаты спутника на поворот Земли за время taui
  // xi1 = xi * cos(thetai) + yi * sin(thetai);
  // yi1 = - xi * sin(thetai) + yi * cos(thetai);
  // xi = xi1
  // yi = yi1
 void  calk_sputnik1()
  {
    double sputnik1[4][4];
    sputnik1[0][0] = sputnik[0][0]*cos(thetai[0]) + sputnik[0][1]*sin(thetai[0]);
    sputnik1[1][0] = sputnik[1][0]*cos(thetai[1]) + sputnik[1][1]*sin(thetai[1]);
    sputnik1[2][0] = sputnik[2][0]*cos(thetai[2]) + sputnik[2][1]*sin(thetai[2]);
    sputnik1[3][0] = sputnik[3][0]*cos(thetai[3]) + sputnik[3][1]*sin(thetai[3]);
 
    sputnik1[0][1] = - sputnik[0][0]*sin(thetai[0]) + sputnik[0][1]*cos(thetai[0]);
    sputnik1[1][1] = - sputnik[1][0]*sin(thetai[1]) + sputnik[1][1]*cos(thetai[1]);
    sputnik1[2][1] = - sputnik[2][0]*sin(thetai[2]) + sputnik[2][1]*cos(thetai[2]);
    sputnik1[3][1] = - sputnik[3][0]*sin(thetai[3]) + sputnik[3][1]*cos(thetai[3]);
 
    sputnik[0][0] = sputnik1[0][0];
    sputnik[1][0] = sputnik1[1][0];
    sputnik[2][0] = sputnik1[2][0];
    sputnik[3][0] = sputnik1[3][0];
 
    sputnik[0][1] = sputnik1[0][1];
    sputnik[1][1] = sputnik1[1][1];
    sputnik[2][1] = sputnik1[2][1];
    sputnik[3][1] = sputnik1[3][1];
 }
///////////////////////////////////////////////////////////////
 void  calk_tki_1()    // первичный расчет времени tki
   {
     tki[1] = t - GP[1].t_oe ;   // вычисляем промежуток времени от опорной эпохи эфемерид
     if ( tki[1] > 302400)       // если t больше 302400 секунд, вычтите 604800 секунд из T.
     tki[1] = tki[1] - 604800;
     else if ( tki[1] < -302400) // Если T меньше -302400 секунд, добавьте 604800 секунд к T.
     tki[1] = tki[1] + 604800;
 
     tki[2] = t - GP[2].t_oe ;   // вычисляем промежуток времени от опорной эпохи эфемерид
     if ( tki[1] > 302400)       // если t больше 302400 секунд, вычтите 604800 секунд из T.
     tki[2] = tki[2] - 604800;
     else if ( tki[2] < -302400) // Если T меньше -302400 секунд, добавьте 604800 секунд к T.
     tki[2] = tki[2] + 604800;
 
     tki[3] = t - GP[3].t_oe ;   // вычисляем промежуток времени от опорной эпохи эфемерид
     if ( tki[3] > 302400)       // если t больше 302400 секунд, вычтите 604800 секунд из T.
     tki[3] = tki[3] - 604800;
     else if ( tki[3] < -302400) // Если T меньше -302400 секунд, добавьте 604800 секунд к T.
     tki[3] = tki[3] + 604800;
 
     tki[4] = t - GP[4].t_oe ;   // вычисляем промежуток времени от опорной эпохи эфемерид
     if ( tki[4] > 302400)       // если t больше 302400 секунд, вычтите 604800 секунд из T.
     tki[4] = tki[4] - 604800;
     else if ( tki[4] < -302400) // Если T меньше -302400 секунд, добавьте 604800 секунд к T.
     tki[4] = tki[4] + 604800;
    //  Расчёт времени прохождения сигнала от спутника к приёмнику
    //  taui  =  PR_sv / c,  где  PR_sv это псевдодальность
    double taui[4];
    taui[0] = PR_sv[0] / c;
    taui[1] = PR_sv[1] / c;
    taui[2] = PR_sv[2] / c;
    taui[3] = PR_sv[3] / c;
    // 3. Расчёт эпохи излучения сигнала спутником tki = tki - taui
    tki[1] = tki[1] - taui[0];
    tki[2] = tki[2] - taui[1];
    tki[3] = tki[3] - taui[2];
    tki[4] = tki[4] - taui[3];
 }
 ///////////////////////////////////////////////////////////
    void correct_pseudorange()  // коррекция псевдодальностей
    {
       PR_sv[0] = PR_sv[0] + (dTclck * c) - (Ii[0]* c) -  Ti[0];
       PR_sv[1] = PR_sv[1] + (dTclck * c) - (Ii[1]* c) -  Ti[1];
       PR_sv[2] = PR_sv[2] + (dTclck * c) - (Ii[2]* c) -  Ti[2];
       PR_sv[3] = PR_sv[3] + (dTclck * c) - (Ii[3]* c) -  Ti[3];
    }
 ///////////////////////////////////////////////////////////
 void  calk_tki_2()     //  повторный Расчёт времени tki
  {  // tki = t_наблюденийi (приёмника) - t_опорнаяi (эфемерид) - taui - dti
    tki[1] = t - GP[1].t_oe - taui[0] - dti[0]; // вычисляем промежуток времени от опорной эпохи эфемерид
    if ( tki[1] > 302400)       // если t больше 302400 секунд, вычтите 604800 секунд из T.
    tki[1] = tki[1] - 604800;
    else if ( tki[1] < -302400) // Если T меньше -302400 секунд, добавьте 604800 секунд к T.
    tki[1] = tki[1] + 604800;
 
    tki[2] = t - GP[2].t_oe - taui[1] - dti[1]; // вычисляем промежуток времени от опорной эпохи эфемерид
    if ( tki[2] > 302400)       // если t больше 302400 секунд, вычтите 604800 секунд из T.
    tki[2] = tki[2] - 604800;
    else if ( tki[2] < -302400) // Если T меньше -302400 секунд, добавьте 604800 секунд к T.
    tki[2] = tki[2] + 604800;
 
    tki[3] = t - GP[3].t_oe - taui[2] - dti[2]; // вычисляем промежуток времени от опорной эпохи эфемерид
    if ( tki[3] > 302400)       // если t больше 302400 секунд, вычтите 604800 секунд из T.
    tki[3] = tki[3] - 604800;
    else if ( tki[3] < -302400) // Если T меньше -302400 секунд, добавьте 604800 секунд к T.
    tki[3] = tki[3] + 604800;
 
    tki[4] = t - GP[4].t_oe - taui[3] - dti[3]; // вычисляем промежуток времени от опорной эпохи эфемерид
    if ( tki[4] > 302400)       // если t больше 302400 секунд, вычтите 604800 секунд из T.
    tki[4] = tki[4] - 604800;
    else if ( tki[4] < -302400) // Если T меньше -302400 секунд, добавьте 604800 секунд к T.
    tki[4] = tki[4] + 604800;
  }
/////////////////////////////////////////////////////////////
 void decoder_Ephemerid()  // декодируем переменные их эфемерид
   {
     int i_buff=0;
     for(i_buff=0;i_buff<121;i_buff++)   //
     {
       stroka[i_buff] = stroka1[i_buff];
     }
    decoder_EPH(1);
    for(i_buff=0;i_buff<121;i_buff++)   //
     {
       stroka[i_buff] = stroka2[i_buff];
     }
     decoder_EPH(2);
      for(i_buff=0;i_buff<121;i_buff++)   //
     {
       stroka[i_buff] = stroka3[i_buff];
     }
    decoder_EPH(3);
      for(i_buff=0;i_buff<121;i_buff++)   //
     {
       stroka[i_buff] = stroka4[i_buff];
     }
    decoder_EPH(4);
   }
////////////////////////////////////////////////////////////
  void  decoder_Klobuchar()
  {
   a0 = BytToSingle(Klobuchar_RAW[46],Klobuchar_RAW[45],Klobuchar_RAW[44],Klobuchar_RAW[43]);
   a1 = BytToSingle(Klobuchar_RAW[50],Klobuchar_RAW[49],Klobuchar_RAW[48],Klobuchar_RAW[47]);
   a2 = BytToSingle(Klobuchar_RAW[54],Klobuchar_RAW[53],Klobuchar_RAW[52],Klobuchar_RAW[51]);
   a3 = BytToSingle(Klobuchar_RAW[58],Klobuchar_RAW[57],Klobuchar_RAW[56],Klobuchar_RAW[55]);
   b0 = BytToSingle(Klobuchar_RAW[62],Klobuchar_RAW[61],Klobuchar_RAW[60],Klobuchar_RAW[59]);
   b1 = BytToSingle(Klobuchar_RAW[66],Klobuchar_RAW[65],Klobuchar_RAW[64],Klobuchar_RAW[63]);
   b2 = BytToSingle(Klobuchar_RAW[70],Klobuchar_RAW[69],Klobuchar_RAW[68],Klobuchar_RAW[67]);
   b3 = BytToSingle(Klobuchar_RAW[74],Klobuchar_RAW[73],Klobuchar_RAW[72],Klobuchar_RAW[71]);
 }
//////// подпрограмма декодирует коэффициенты Klobuchar из сообщения навигатора
  float BytToSingle (unsigned char x1,unsigned char x2,unsigned char x3,unsigned char x4)
  {
    float outval;
    unsigned char *optr;
    optr = (unsigned char*)&outval + 3;
    *optr-- = x1;
    *optr-- = x2;
    *optr-- = x3;
    *optr = x4;
    return outval;
  }
// a0  1.11759e-008  a1  7.45058e-009  a2  -5.96046e-008
// a3 -5.96046e-008  b0  90112 b1  0   b2  -196608    b3  -65536
////////////////////////////////////////////////////////
  void   decoder_EPH(int x)
  {
       unsigned long tmp_eph=0;
       long tmp_eph2=0;
 
      tmp_eph=((((((stroka[51]<<8)+stroka[57])<<8)+stroka[56])<<8)+stroka[55]); //i50,i56,i55,i54//
        if ((stroka[51] & (1<<7)) != 0 )         // if (x.f7==1)
           {
             tmp_eph2 = ( ~-tmp_eph|-0x80000000 ) + 1;
             GP[x].Mo = (tmp_eph2 * 4.6566128730774E-10) * pi;
           }
          else
           {
             GP[x].Mo = (tmp_eph * 4.6566128730774E-10) * pi;
           }
         tmp_eph=0;
 
       tmp_eph=(((stroka[53])<<8)+stroka[52]);  // i52,i51
         if ((stroka[53] & (1<<7)) != 0 )      // if (x.f7==1)
           {
             tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
             GP[x].delta_n = (tmp_eph2 * 1.1368683772162E-13) * pi;
           }
          else
           {
             GP[x].delta_n = (tmp_eph * 1.1368683772162E-13) * pi;
           }
          tmp_eph=0;
 
      tmp_eph =((((((stroka[59]<<8)+stroka[65])<<8)+stroka[64])<<8)+stroka[63]); //i58,i64,i63,i62//
      GP[x].E_a = tmp_eph* 1.1641532182693E-10;
      tmp_eph =0;
 
      tmp_eph = ((((((stroka[67]<<8)+stroka[73])<<8)+stroka[72])<<8)+stroka[71]); //i66,i72,i71,i70//
      GP[x].square_big_axis = tmp_eph*1.9073486328125E-6 ;
      tmp_eph =0;
 
      tmp_eph = ((((stroka[108])<<8)+stroka[107])>>2);  // i107,i106
         if ((stroka[108] & (1<<7)) != 0 )           // if (x.f7==1)
           {
             tmp_eph = ((((stroka[108])<<8)+stroka[107]));
             tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
             tmp_eph2=tmp_eph2>>2;
             GP[x].IDOT = (tmp_eph2 * 1.1368683772162E-13) * pi;
           }
          else
           {
             GP[x].IDOT = (tmp_eph * 1.1368683772162E-13) * pi;
           }
         tmp_eph =0;
 
      tmp_eph = (((stroka[77])<<8)+stroka[76]); //i76,i75//
      GP[x].t_oe = tmp_eph*16;                  // без дополнения до двух
      tmp_eph =0;
 
      tmp_eph =  (((stroka[61])<<8)+stroka[60]);  // i60,i59//
        if ((stroka[61] & (1<<7)) != 0 )          // if (x.f7==1)
           {
             tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
             GP[x].Cuc = tmp_eph2*1.862645149231E-9 ;
           }
          else
           {
             GP[x].Cuc = tmp_eph*1.862645149231E-9 ;
           }
         tmp_eph =0;
 
         tmp_eph = (((stroka[69])<<8)+stroka[68]);  // i68,i67,
           if ((stroka[69] & (1<<7)) != 0 )      // if (x.f7==1)
            {
                tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
                GP[x].Cus = tmp_eph2*1.862645149231E-9 ;
            }
            else
            {
                GP[x].Cus = tmp_eph*1.862645149231E-9 ;
            }
            tmp_eph =0;
 
      tmp_eph = (((stroka[97])<<8)+stroka[96]); //i96,i95,//
        if ((stroka[97] & (1<<7)) != 0 )      // if (x.f7==1)
          {
            tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
            GP[x].Crc = tmp_eph2 * 0.03125 ;
          }
        else
          {
            GP[x].Crc = tmp_eph * 0.03125 ;
          }
        tmp_eph =0;
 
     tmp_eph =  (((stroka[48])<<8)+stroka[47]); // i47,i46,//
       if ((stroka[48] & (1<<7)) != 0 )      // if (x.f7==1)
                 {
                   tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
                  GP[x].Crs = tmp_eph2 * 0.03125 ;
                 }
       else
                 {
                     GP[x].Crs = tmp_eph * 0.03125 ;
                 }
              tmp_eph =0;
 
      tmp_eph = (((stroka[81])<<8)+stroka[80]); // i80,i79,
       if ((stroka[81] & (1<<7)) != 0 )      // if (x.f7==1)
         {
           tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
           GP[x].Cic = tmp_eph2 * 1.862645149231E-9 ;
         }
       else
         {
           GP[x].Cic = tmp_eph * 1.862645149231E-9 ;
         }
       tmp_eph =0;
 
      tmp_eph = (((stroka[89])<<8)+stroka[88]); // i88,i87,
        if ((stroka[89] & (1<<7)) != 0 )      // if (x.f7==1)
           {
             tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
             GP[x].Cis = tmp_eph2*1.862645149231E-9 ;
           }
          else
           {
             GP[x].Cis = tmp_eph*1.862645149231E-9 ;
           }
         tmp_eph =0;
 
      tmp_eph = ((((((stroka[79]<<8)+stroka[85])<<8)+stroka[84])<<8)+stroka[83]); // i78, 84 83 82//
        if ((stroka[79] & (1<<7)) != 0 )      // if (x.f7==1)
           {
             tmp_eph2 = ( ~-tmp_eph|-0x80000000) + 1;
             GP[x].omega_0 = (tmp_eph2* 4.6566128730774E-10) * pi;
           }
          else
           {
             GP[x].omega_0 = (tmp_eph* 4.6566128730774E-10) * pi;
           }
        tmp_eph =0;
 
      tmp_eph = ((((((stroka[87]<<8)+stroka[93])<<8)+stroka[92])<<8)+stroka[91]); //i86,i92,i91,i90//
          if ((stroka[87] & (1<<7)) != 0 )      // if (x.f7==1)
             {
               tmp_eph2 = ( ~-tmp_eph|-0x80000000 ) + 1;
               GP[x].io = (tmp_eph2*4.6566128730774E-10) * pi;
             }
           else
             {
               GP[x].io = (tmp_eph*4.6566128730774E-10) * pi;
             }
          tmp_eph =0;
 
      tmp_eph =(((((stroka[105])<<8)+stroka[104])<<8)+stroka[103]); //i104,i103,i102
          if ((stroka[105] & (1<<7)) != 0 )      // if (x.f7==1)
             {
               tmp_eph2 = ( ~-tmp_eph|-0x800000 ) + 1;
               GP[x].OMEGADOT = (tmp_eph2* 1.1368683772162E-13) * pi;
             }
           else
             {
               GP[x].OMEGADOT = (tmp_eph* 1.1368683772162E-13) * pi;
             }
          tmp_eph =0;
 
      tmp_eph = ((((((stroka[95]<<8)+stroka[101])<<8)+stroka[100])<<8)+stroka[99]); // 94,i100,i99,i98 //
          if ((stroka[95] & (1<<7)) != 0 )      // if (x.f7==1)
             {
               tmp_eph2 = ( ~-tmp_eph|-0x80000000 ) + 1;
               GP[x].omega = (tmp_eph2*4.6566128730774E-10) * pi ;
             }
           else
             {
               GP[x].omega = (tmp_eph*4.6566128730774E-10) * pi ;
             }
          ///////////////////////
      tmp_eph = (stroka[31]); // i30,
            if ((stroka[31] & (1<<7)) != 0 )    // if (x.f7==1)
               {
                 tmp_eph2 = ( ~-tmp_eph|-0x80) + 1;
                 GP[x].t_gd = tmp_eph2*4.6566128730774E-10 ;
               }
            else
               {
                 GP[x].t_gd = tmp_eph*4.6566128730774E-10 ;
               }
            tmp_eph =0;
 
      tmp_eph = (((stroka[36])<<8)+stroka[35]); // i35,i34,
                 GP[x].t_oc = tmp_eph*16 ;      // без дополнения до двух
            tmp_eph =0;
 
      tmp_eph = (stroka[41]); // i40,
            if ((stroka[41] & (1<<7)) != 0 ) // if (x.f7==1)
               {
                 tmp_eph2 = ( ~-tmp_eph|-0x80) + 1;
                 GP[x].af2 = tmp_eph2*2.7755575615629E-17 ;
               }
           else
               {
                 GP[x].af2 = tmp_eph*2.7755575615629E-17 ;
               }
            tmp_eph =0;
 
      tmp_eph = (((stroka[40])<<8)+stroka[39]); // i39,i38,
              if ((stroka[40] & (1<<7)) != 0 ) // if (x.f7==1)
                 {
                   tmp_eph2 = ( ~-tmp_eph|-0x8000) + 1;
                   GP[x].af1 = tmp_eph2*1.1368683772162E-13 ;
                 }
              else
                 {
                   GP[x].af1 = tmp_eph*1.1368683772162E-13 ;
                 }
             tmp_eph =0;
 
     tmp_eph =((((((stroka[45])<<8)+stroka[44])<<8)+stroka[43])>>2); //i44,i43,i42
               if ((stroka[45] & (1<<7)) != 0 ) // if (x.f7==1)
                  {
                    tmp_eph =((((((stroka[45])<<8)+stroka[44])<<8)+stroka[43]));
                    tmp_eph2 = ( ~-tmp_eph|-0x800000) + 1;
                    tmp_eph2=tmp_eph2>>2;
                    GP[x].af0 = tmp_eph2 * 4.6566128730774E-10;
                  }
               else
                  {
                    GP[x].af0 = tmp_eph * 4.6566128730774E-10;
                  }
              tmp_eph =0;
     }
 // 2 в степени -43   1.1368683772162E-13   // 2 в степени -33  1.1641532182693E-10
 // 2 в степени -31   4.6566128730774E-10   // 2 в степени -29  1.862645149231E-9
 // 2 в степени -19 = 1.9073486328125E-6 = 0,00000190734
 // 2 в степени -5  = 0.03125  // 2 в степени 4 = 16  // 2 в степени -55 = 2.7755575615629E-17
///////////////////////////////////////////////////
   void calculation_delta()
      {
        Delta_X = A[0][0]*matrix_4x1[0]+A[0][1]*matrix_4x1[1]+A[0][2]*matrix_4x1[2]+A[0][3]*matrix_4x1[3];
        Delta_Y = A[1][0]*matrix_4x1[0]+A[1][1]*matrix_4x1[1]+A[1][2]*matrix_4x1[2]+A[1][3]*matrix_4x1[3];
        Delta_Z = A[2][0]*matrix_4x1[0]+A[2][1]*matrix_4x1[1]+A[2][2]*matrix_4x1[2]+A[2][3]*matrix_4x1[3];
        Delta_t0 = A[3][0]*matrix_4x1[0]+A[3][1]*matrix_4x1[1]+A[3][2]*matrix_4x1[2]+A[3][3]*matrix_4x1[3];
        user[0]=user[0]+Delta_X;
        user[1]=user[1]+Delta_Y;
        user[2]=user[2]+Delta_Z;
        dTclck = Delta_t0;
      }
//////////////////////////////////////////////////
   void zapolnenie_matrix()
    {
      int i;
      int j;
      Rtotal();         // вычисляется Rtotal
      for(i=0;i<4;i++)
        {
           matrix_4x1=sputnik[3]-R_total;
           for(j=0;j<4;j++)
             {
               if(j==3)
                  A[j] =  c;
               else
                  A[j]=(user[j]-sputnik[j])/R_total;
             }
        }
    }
/////////////////////////////////////
    void Rtotal()
    {
      int i;
      for(i=0;i<4;i++)
      {
        R_total=sqrt(pow((sputnik[0]-user[0]),2)+pow((sputnik[1]-user[1]),2)+pow((sputnik[2]-user[2]),2));
      }
    }
///////////////////////////////////////////////////////////////////
       // Алгоритм вычисления обратной матрицы с помощью алгебраических
       // дополнений: метод присоединённой (союзной) матрицы.
   void minor_4x4()              // считаем минор матрицы 4х4
             {
               int ii;
               int jj;
               float mn[4][4];
 
                mn[0][0] = A[1][1]*A[2][2]*A[3][3] + A[1][2]*A[2][3]*A[3][1] + A[1][3]*A[2][1]*A[3][2]
                - A[1][3]*A[2][2]*A[3][1] - A[1][1]*A[2][3]*A[3][2] - A[1][2]*A[2][1]*A[3][3];
 
                mn[0][1] = A[1][0]*A[2][2]*A[3][3] + A[1][2]*A[2][3]*A[3][0] + A[1][3]*A[2][0]*A[3][2]
                - A[1][3]*A[2][2]*A[3][0] - A[1][0]*A[2][3]*A[3][2] - A[1][2]*A[2][0]*A[3][3];
 
                mn[0][2] = A[1][0]*A[2][1]*A[3][3] + A[1][1]*A[2][3]*A[3][0] + A[1][3]*A[2][0]*A[3][1]
                - A[1][3]*A[2][1]*A[3][0] - A[1][0]*A[2][3]*A[3][1] - A[1][1]*A[2][0]*A[3][3];
 
                mn[0][3] = A[1][0]*A[2][1]*A[3][2] + A[1][1]*A[2][2]*A[3][0] + A[1][2]*A[2][0]*A[3][1]
                - A[1][2]*A[2][1]*A[3][0] - A[1][0]*A[2][2]*A[3][1] - A[1][1]*A[2][0]*A[3][2];
 
               mn[1][0] =  A[0][1]*A[2][2]*A[3][3] + A[0][2]*A[2][3]*A[3][1] + A[0][3]*A[2][1]*A[3][2]
                - A[0][3]*A[2][2]*A[3][1] - A[0][2]*A[2][1]*A[3][3] - A[0][1]*A[2][3]*A[3][2];
 
               mn[1][1] =  A[0][0]*A[2][2]*A[3][3] + A[0][2]*A[2][3]*A[3][0] + A[0][3]*A[2][0]*A[3][2]
                - A[0][3]*A[2][2]*A[3][0] - A[0][2]*A[2][0]*A[3][3] - A[0][0]*A[2][3]*A[3][2];
 
               mn[1][2] =  A[0][0]*A[2][1]*A[3][3] + A[0][1]*A[2][3]*A[3][0] + A[0][3]*A[2][0]*A[3][1]
                - A[0][3]*A[2][1]*A[3][0] - A[0][1]*A[2][0]*A[3][3] - A[0][0]*A[2][3]*A[3][1];
 
               mn[1][3] =  A[0][0]*A[2][1]*A[3][2] + A[0][1]*A[2][2]*A[3][0] + A[0][2]*A[2][0]*A[3][1]
                - A[0][2]*A[2][1]*A[3][0] - A[0][1]*A[2][0]*A[3][2] - A[0][0]*A[2][2]*A[3][1];
 
               mn[2][0] =  A[0][1]*A[1][2]*A[3][3] + A[0][2]*A[1][3]*A[3][1] + A[0][3]*A[1][1]*A[3][2]
                - A[0][3]*A[1][2]*A[3][1] - A[0][2]*A[1][1]*A[3][3] - A[0][1]*A[1][3]*A[3][2];
 
               mn[2][1] =  A[0][0]*A[1][2]*A[3][3] + A[0][2]*A[1][3]*A[3][0] + A[0][3]*A[1][0]*A[3][2]
                - A[0][3]*A[1][2]*A[3][0] - A[0][2]*A[1][0]*A[3][3] - A[0][0]*A[1][3]*A[3][2];
 
               mn[2][2] =  A[0][0]*A[1][1]*A[3][3] + A[0][1]*A[1][3]*A[3][0] + A[0][3]*A[1][0]*A[3][1]
                - A[0][3]*A[1][1]*A[3][0] - A[0][1]*A[1][0]*A[3][3] - A[0][0]*A[1][3]*A[3][1];
 
               mn[2][3] =  A[0][0]*A[1][1]*A[3][2] + A[0][1]*A[1][2]*A[3][0] + A[0][2]*A[1][0]*A[3][1]
                - A[0][2]*A[1][1]*A[3][0] - A[0][1]*A[1][0]*A[3][2] - A[0][0]*A[1][2]*A[3][1];
 
               mn[3][0] =  A[0][1]*A[1][2]*A[2][3] + A[0][2]*A[1][3]*A[2][1] + A[0][3]*A[1][1]*A[2][2]
                - A[0][3]*A[1][2]*A[2][1] - A[0][2]*A[1][1]*A[2][3] - A[0][1]*A[1][3]*A[2][2];
 
               mn[3][1] =  A[0][0]*A[1][2]*A[2][3] + A[0][2]*A[1][3]*A[2][0] + A[0][3]*A[1][0]*A[2][2]
                - A[0][3]*A[1][2]*A[2][0] - A[0][2]*A[1][0]*A[2][3] - A[0][0]*A[1][3]*A[2][2];
 
               mn[3][2] = A[0][0]*A[1][1]*A[2][3] + A[0][1]*A[1][3]*A[2][0] + A[0][3]*A[1][0]*A[2][1]
                - A[0][3]*A[1][1]*A[2][0] - A[0][1]*A[1][0]*A[2][3] - A[0][0]*A[1][3]*A[2][1];
 
               mn[3][3] = A[0][0]*A[1][1]*A[2][2] + A[0][1]*A[1][2]*A[2][0] + A[0][2]*A[1][0]*A[2][1]
                - A[0][2]*A[1][1]*A[2][0] - A[0][1]*A[1][0]*A[2][2] - A[0][0]*A[1][2]*A[2][1];
 
               det_A = A[0][0]*mn[0][0] - A[0][1]*mn[0][1] + A[0][2]*mn[0][2]- A[0][3]*mn[0][3];
 
               for(ii=0;ii<4;ii++)     //  Находим матрицу алгебраических дополнений
                  for(jj=0;jj<4;jj++)
                       mn[ii][jj]=pow((-1),(ii+jj+2))*mn[ii][jj];
 
               for(ii=0;ii<4;ii++)     // Находим транспонированную матрицу алгебраических дополнений
                  for(jj=0;jj<4;jj++)
                       A[jj][ii]=mn[ii][jj];
 
               for(ii=0;ii<4;ii++)
                  for(jj=0;jj<4;jj++)
                       A[jj][ii]=A[jj][ii] / det_A ;
 
             }
///////////////////////////////////////////////////////////
  void Transformatik_coordinates() // Преобразование декартовых координат {x,y,z} в геодезические {H,L,B
    {
           double    x =user[0];
           double    y =user[1];
           double    z =user[2];
           double D,La,r;
 
          D = sqrtl(pow(x,2) + pow(y,2)); // вычисляем вспомогательную величину D
 
           if (D ==0)
            {
              L = 0;
              B = (pi/2)*(z/fabsl(z));
            }
           if (D >0)
            {
              La = asinl (y/D);
              if (y<0 && x>0)
                 {
                   L = 2*pi - La;
                 }
              if (y<0 && x<0)
                 {
                   L = 2*pi + La;
                 }
              if (y>0 && x<0)
                 {
                   L = pi - La;
                 }
              if (y>0 && x>0)
                 {
                   L = La;
                 }
               r = sqrtl(powl(x,2)+powl(y,2)+powl(z,2)); // вспомогательная величина r
               B = asinl(z/r);                           // геодезическая широта B
               if (z==0)
                 {
                   B=0;
                 }
            }
         double a_kr = 1/298.257223563;  // коэффициэнт сжатия элли
         H = D*cosl(B)+z*sinl(B)- 6378137* sqrtl(1-powl(a_kr,2)*(sinl(B)*sinl(B))); // Найдем геодезическую высоту H
         B = B * (180/pi);  // переведем радианы в градусы
         L = L * (180/pi);  // геодезическая долгота в градусах
      }
/////////////////////////////////////////////////////////////
     void Transformatik_coordinates_2() // Преобразование декартовых координат {x,y,z} в геодезические {H,L,B
           {
             double    x =user[0];
             double    y =user[1];
             double    z =user[2];
             double D,La,r,ce,p1,e1;
             double a_kr = 1/298.257223563;      // коэффициэнт сжатия эллипсоида
             //Малая полуось (полярный радиус) 6356863 м  6356863.019
             //Большая полуось (экваториальный радиус) 6378245 м  6378245.000
             //Полярное сжатие (отношение разницы полуосей к большой полуоси)1/298,3
 
             D = sqrtl(powl(x,2) + powl(y,2));   // вычисляем вспомогательную величину D
             if (D ==0)
              {
                L = 0;
                B = (pi/2)*(z/fabsl(z));   // H = не вычисляем за ненадобностью
              }
             if (D >0)
              {
                La = asinl (y/D);
                if (y<0 && x>0)
                   {
                     L = 2*pi - La;
                   }
                if (y<0 && x<0)
                   {
                     L = 2*pi + La;
                   }
                if (y>0 && x<0)
                   {
                     L = pi - La;
                   }
                if (y>0 && x>0)
                   {
                     L = La;
                   }
                 if (z==0)
                   {
                     B = 0;
                     H = D - 6378245.000; // D - Большая полуось эллипсоида
                   }
           r = sqrtl(powl(x,2)+powl(y,2)+powl(z,2)); // вспомогательная величина r
           ce = asinl(z/r);
           e1 = ((2 * a_kr) - (pow(a_kr,2))); // квадрат эксцентриситета эллипсоида
           p1 = (e1 * a_kr) / (2 * r);
 
               double s1,s2,be1;
               s1 =0; // начальное условие для вычислений уравнения
                for (;;)   //
                   {
                     be1 = ce + s1; //
                     s2 = asinl((p1 * sin(2*be1)) / (sqrtl(1 - e1 * (sin(be1) * sin(be1)))));
                     if(fabs(s2 -s1) > 0.000001)
                     s1 = s2;
                     else
                     break;
                   }
                B = be1;        // геодезическая широта B
               }
 
          H = D*cosl(B)+z*sinl(B)- 6378245*sqrtl(1-e1*(sinl(B)*sinl(B))); // Найдем геодезическую высоту H
          B = B * (180/pi);  // переведем радианы в градусы
          L = L * (180/pi);  // геодезическая долгота в градусах
       }
 ////////////////////////////////////////////////
void const_eph1() // таблица данных из тестового примера
 {                // в программе не используется
    GP[1].Mo = 1.8314411304;
    GP[1].delta_n = 0.0000000036905108674;
    GP[1].E_a = 0.007922023884; // эксцентриситет; бывшая e
    GP[1].square_big_axis = 5153.7570324; // Квадратный корень большой полуоси;
    GP[1].IDOT = 2.07151148583E-11;
    GP[1].t_oe = 324000;
    GP[1].Cuc = 0.00000068731606007;
    GP[1].Cus = 0.000010745599866;
    GP[1].Crc = 179.59375;
    GP[1].Crs = 0;
    GP[1].Cic = 0.000000106117077351;
    GP[1].Cis = 0.00000011548399925;
    GP[1].omega_0 = -0.70057438567;
    GP[1].io = 0.98096702533;
    GP[1].OMEGADOT = -0.000000007603888161;
    GP[1].omega = -0.85377327792;
 }
     

    Attached Files:

  3. Дед 005

    Дед 005 Форумчанин

    Oct 31, 2019 #123
    Пять тысяч просмотров........ и ни одного комментария...... хоть бы кто написал. - что нибудь типа, - "ерунда полная", или может быть по делу что то.....может ошибку кто то увидел. или бы своим рабочим проектом поделился....
     
  4. trir

    trir Форумчанин

    Oct 31, 2019 #124
    просто никто этим не занимается - не очень понятно зачем это нужно...
     
    Ламаград likes this.
  5. stout

    stout Форумчанин

    Oct 31, 2019 #125
    ACCURATE ALGORITHMS TO TRANSFORM GEOCENTRIC TO GEODETIC COORDINATES использовался в IERS TECHNICAL NOTE 21 (IERS Conventions (1992))
    Borkowski, 1989.png
    Вот неплохой алгоритм из методички Валеры и Тамары Луповок
    Луповка.png
    Сравните с тем, что у вас
     
  6. Дед 005

    Дед 005 Форумчанин

    Nov 8, 2019 #126
    Нашел книгу «основы космической геодезии» В.А. Луповка, Т.К. Луповка, посмотрел рекомендованный алгоритм, - страницы 78, 77 , реализовал алгоритм, - сравнил с тем что был ранее, по «основам» получилось наверное грамотнее, - но цифры из примера, приблизительно схожи.
    Code:
    #include <stdio.h>
#include <stdlib.h>
 
double pi = 3.1415926535898;
double L;  // широта
double B;  // долгота
double H;  // высота
     //  параметры эллипсоида вращения в WGS84
double      a = 6378137.00000;     // большая полуось эллипсоида метров
double      b = 6356752.31424518;  // малая полуось эллипсоида метров
double      e = 0.081819190842622; // первый эксцентриситет меридианного эллипса,
double      f = 1/298.257223563;   // сжатие эллипсоида
        //
double    X = 2616905.988;         // исходные данные из примера
double    Y = 5135967.188;
double    Z = 3003938.098;
// в примере по этим исходным данным получается такой результат:
       // B =  27,5255635201896 градуса
       // L =  63,0000000181943 градуса
       // H =  121871,656812661 м
 
double    D,e2,c;
int       quarter_number;         // номер четверти (квадранта)
 
void Transformatik_coordinates(); // из wgs84 в геодезические L,B,H
void algoritm_Lupovka ();         // второй вариант расчета
void one_solution();              // расчет для низких и средних широт
void two_solution();              // расчет для высоких широт
 
int main()
{
  Transformatik_coordinates(); // из wgs84 в геодезические L,B,H
  algoritm_Lupovka ();         // из wgs84 в геодезические L,B,H
  return 0;
}
//////////////////////////////////////////////////////////
 void algoritm_Lupovka()
  {
   D = sqrtl(pow(X,2) + pow(Y,2)); // вычисляем вспомогательную величину D
     if (D >=0)
     {
       one_solution(); // расчет для низких и средних широт
     }
    else
     {
       two_solution(); // расчет для высоких широт
     }
    B = B * (180/pi);  // переведем радианы в градусы
    L = L * (180/pi);  // геодезическая долгота в градусах
    printf("Ln : %.15f \n",L);
    printf("Bn : %.15f \n",B);
    printf("Hn : %.15f \n",H);
  }
  ////////////////////////////////////////////////////////
 void one_solution()    // расчет для низких и средних широт
  {
   double  N,t0,K,e2,c,p,ti,ti1;
 
    t0 = Z/D;
    e2 = f * (2 - f);
    K = 1/(1 - e2);
    c = a/(1-f);
    p = (c * e2)/D;
 
    ti = 0;    // начальное условие для вычислений
    for (;;)
      {
        ti1 = t0 + ((p * ti)/(sqrtl(K + pow(ti,2))));
        if(fabs(ti1 - ti) > 1e-19)
         ti = ti1;
        else
        break;
      }
    N = (c * sqrtl(1 + pow(ti1,2))) / ( sqrtl(K + pow(ti1,2)));
    B = atanl(ti1);
    H = D * sqrtl(1 + pow(ti1,2)) - N;
    L = atanl(Y / X);
  }
 
 void two_solution()   // расчет для высоких широт
  {
  double  N_shtrih,t0_shtrih,K_shtrih,p_shtrih,ti_shtrih,ti1_shtrih;
 
    t0_shtrih = D/Z;
    e2 = f * (2 - f);
    K_shtrih = 1 - e2;
    c = a/(1-f);
    p_shtrih = (-a * e2)/Z;
 
   ti_shtrih = 0;  // начальное условие для вычислений
    for (;;)
      {
        ti1_shtrih = t0_shtrih + ((p_shtrih * ti_shtrih)/(sqrtl(K_shtrih + pow(ti_shtrih,2))));
        if(fabs(ti1_shtrih - ti_shtrih) > 1e-19)
        ti_shtrih = ti1_shtrih;
        else
        break;
      }
   N_shtrih  = (a * sqrtl(1 + pow(ti1_shtrih,2))) / ( sqrtl(K_shtrih + pow(ti1_shtrih,2)));
   B = 90 - atanl(ti1_shtrih);
   H = Z * sqrtl(1 + pow(ti1_shtrih,2)) - (K_shtrih * N_shtrih);
   L = atanl(Y / X);
  }
//////////////////////////////////////////////////////////
 
   void Transformatik_coordinates() // Преобразование декартовых координат {x,y,z} в геодезические {H,L,B
    {
      double  x = 2616905.988;
      double  y = 5135967.188;
      double  z = 3003938.098;
      double D,La,r;
 
      D = sqrtl(pow(x,2) + pow(y,2)); // вычисляем вспомогательную величину D
          if (D ==0)
           {
             L = 0;
             B = (pi/2)*(z/fabsl(z));
           }
      if (D >0)
       {
           La = asinl (y/D);
           if (y<0 && x>0)
            {
              L = 2*pi - La;
            }
           if (y<0 && x<0)
            {
              L = 2*pi + La;
            }
           if (y>0 && x<0)
            {
              L = pi - La;
            }
           if (y>0 && x>0)
            {
               L = La;
            }
     r = sqrtl(powl(x,2)+powl(y,2)+powl(z,2)); // вспомогательная величина r
     B = asinl(z/r);                           // геодезическая широта B
           if (z==0)
            {
               B=0;
            }
        }
    double a_kr = 1/298.257223563;  // коэффициэнт сжатия эллипсоида
    H = D*cosl(B)+z*sinl(B)- 6378137* sqrtl(1-powl(a_kr,2)*(sinl(B)*sinl(B))); // Найдем геодезическую высоту H
    B = B * (180/pi);  // переведем радианы в градусы
    L = L * (180/pi);  // геодезическая долгота в градусах
    printf("Ln : %.15f \n",L);
    printf("Bn : %.15f \n",B);
    printf("Hn : %.15f \n",H);
    }
    --- Сообщения объединены, Nov 8, 2019, Оригинальное время сообщения: Nov 8, 2019 ---
    хочется периодически уточнять место нахождения робота.
     
  7. trir

    trir Форумчанин

    Nov 8, 2019 #127
    что это за модуль, который отдаёт сырые данные, но не даёт готовый результат?
     
  8. Дед 005

    Дед 005 Форумчанин

    Nov 8, 2019 #128
    модуль выдает готовый результат. точность бытовая. Следующим этапом если до него получится дорасти, Хочется вводить поправки с базовой станции.
     
  9. Sergey Astakhov

    Sergey Astakhov Форумчанин

    Nov 8, 2019 #129
    Ну это было давно.
    А сейчас их б/у на ebay валом, за сотни $: https://www.ebay.com/itm/10MHz-FE-5680A-Rubidium-Atomic-Frequency-Standard-/252895330212
    Даже новый за полторы штуки можно купить: https://www.thinksrs.com/products/PRS10.htm
     
  10. stout

    stout Форумчанин

    Nov 8, 2019 #130
    Spoiler

    Это приблизительно как Emlid Reach супротив Ляйки или Тримбл.

     
  11. Дед 005

    Дед 005 Форумчанин

    Dec 11, 2019 #131
    системеое время.jpg

    Добрый день. Вот что то снова посмотрел на эти же формулы, и как то не ясно стало, - вот t = tsv - дельта tsv, .........................а в расчете этой самой дельта используется эта же t ........... поясните пожалуйста.................... вроде раньше я что то по другому воспринимал.....
     
  12. Дед 005

    Дед 005 Форумчанин

    Dec 12, 2019 #132
    хотя................. если расположить формулы в обратном порядке, то все нормально будет...... видимо слишком много вчера выкушал на ночь горячего шоколада...........
     
  13. Дед 005

    Дед 005 Форумчанин

    Dec 22, 2019 #133
    Добрый день. вот такой появился вопрос, - считает моя программа координаты, - и при этом они постоянно "плывут", и причина мне не понятна. Есть ощущение, что что то важное в алгоритме я упустил....... поясняю: вот запускаю я расчеты несколько десятков раз, для удобства написал программу, которая считает расстояние от истинных моих координат, до тех, что посчитала программа (расчет упрощенный, - для плоскости ). и вот выборка из десятков расчетов координат:
    время спутники дистанция
    time - 21666.0 | 12 10 25 32 | Distance : 44.907 kilometrov
    time - 21988.0 | 12 10 25 32 | Distance : 24.452 kilometrov
    time - 22020.0 | 12 10 25 32 | Distance : 24.197 kilometrov
    time - 22052.0 | 12 10 25 32 | Distance : 24.269 kilometrov
    time - 22084.0 | 12 10 25 32 | Distance : 24.588 kilometrov
    time - 22662.0 | 12 10 25 32 | Distance : 50.569 kilometrov
    time - 23241.0 | 12 10 25 32 | Distance : 86.234 kilometrov
    time - 24494.0 | 12 10 25 32 | Distance : 225.363 kilometrov
    time - 25362.0 | 12 10 25 32 | Distance : 492.917 kilometrov

    time - 25587.0 | 10 25 32 31 | Distance : 284.597 kilometrov
    time - 25844.0 | 12 25 32 31 | Distance : 104.051 kilometrov
    time - 27708.0 | 12 25 32 31 | Distance : 65.780 kilometrov
    time - 28350.0 | 12 25 32 31 | Distance : 67.459 kilometrov
    здесь видно, что с каждым разом, расчетная точка сдвигается все ближе, - потом проходит ближайшую и начинает отдаляться через какое то время один из спутников уходит, и с новым созвездием все повторяется........ подскажите по возможности, - что не так?
     
  14. Евгений_GPS

    Евгений_GPS

    Apr 8, 2025 #134
    Актуальное время можно взять из полученного RAW(X)- сообщения
    --- Сообщения объединены, Apr 8, 2025, Оригинальное время сообщения: Apr 8, 2025 ---
    Смущает слово ГЛОНАСС наверху. Принимаете сигнал GPS? Это ничего, можно считать по этой методичке?
    --- Сообщения объединены, Apr 8, 2025 ---

    Что является исходными данными для расчёта по этой методичке? Могу я найти координаты приёмника по полученным псевдодальностям из RAW(X) сообщения? Как привязать к этому эфемериды?
    --- Сообщения объединены, Apr 8, 2025 ---
    Это ещё ничего...На StackOverflow уже бы аккаунт забанили за то, что посмел спросить такое, чего "высокорейтинговые правщики" не знают.)))
     
Page 7 of 7
  1. This site uses cookies to help personalise content, tailor your experience and to keep you logged in if you register.
    By continuing to use this site, you are consenting to our use of cookies.
    Accept Learn More...
    Dismiss Notice
  1. This site uses cookies to help personalise content, tailor your experience and to keep you logged in if you register.
    By continuing to use this site, you are consenting to our use of cookies.
    Accept Learn More...
    Dismiss Notice