Вот изначально, я считал что алгоритм выглядит так: На основании данных взятых из эфемерид, считаем координаты спутников. На основании коэффициентов коррекции синхроимпульсов af0,af1,af2, полученных из сообщения спутников, считаем координаты спутников снова, - более точно, введя поправку, - коррекцию синхроимпульсов. На основании псевдодальностей, взятых из сообщения навигатора, и вычисленных координат спутников считаем свои координаты на местности. Методом наименьших квадратов. Вычисления повторяются, несколько раз, до тех пор пока величина ошибки не станет меньше 1 см. (пишут, что обычно это от 3 до 5 раз). Изначально координаты считаются равными нулю. При вычислениях при каждой итерации координаты получаются как накапливаемая сумма ошибок. Одновременно с этим считается dTclck – ошибка часов приемника. На основании вычисленных координат на местности и координат спутников,вычисленных ранее, исходя из выбранной модели, считаем ионосферную и тропосферную поправки для каждого спутника. По результатам вычислений делается коррекция псевдодальностей для каждого спутника Pcor[prn] = Praw[prn] + dTclck * c - dTiono * c – dRtrop; где dTclck – ошибка часов приемника. На основании скорректированных псевдодальностей, и вычисленных ранее координат спутников снова считаем свои координаты на местности. Методом наименьших квадратов. Вычисления повторяются, несколько раз, до тех пор пока величина ошибки не станет меньше 1 см. Изначально координаты считаются равными нулю. При вычислениях при каждой итерации координаты получаются как накапливаемая сумма ошибок. Результат на этом считается окончательным.
Применил какие нашел в литературе поправки, получился код. Только где то в самом начале что то неправильно, - потому что посчитанные координаты отличаются от реальных примерно на 100 километров. Поправки исправляют незначительно, а ошибка грубая. В программе уже есть эфемериды и псевдодальности, полученные от модуля навигатора в месте с конкретными координатами. Может кто нибудь из уважаемых геодезистов посмотрит, увидит что то и скажет что нибудь умное......... завтра. а то сегодня первое апреля. мало ли...... Код: #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; }
Пять тысяч просмотров........ и ни одного комментария...... хоть бы кто написал. - что нибудь типа, - "ерунда полная", или может быть по делу что то.....может ошибку кто то увидел. или бы своим рабочим проектом поделился....
ACCURATE ALGORITHMS TO TRANSFORM GEOCENTRIC TO GEODETIC COORDINATES использовался в IERS TECHNICAL NOTE 21 (IERS Conventions (1992)) Вот неплохой алгоритм из методички Валеры и Тамары Луповок Сравните с тем, что у вас
Нашел книгу «основы космической геодезии» В.А. Луповка, Т.К. Луповка, посмотрел рекомендованный алгоритм, - страницы 78, 77 , реализовал алгоритм, - сравнил с тем что был ранее, по «основам» получилось наверное грамотнее, - но цифры из примера, приблизительно схожи. Код: #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); } --- Сообщения объединены, 8 ноя 2019, Оригинальное время сообщения: 8 ноя 2019 --- хочется периодически уточнять место нахождения робота.
модуль выдает готовый результат. точность бытовая. Следующим этапом если до него получится дорасти, Хочется вводить поправки с базовой станции.
Ну это было давно. А сейчас их б/у на ebay валом, за сотни $: https://www.ebay.com/itm/10MHz-FE-5680A-Rubidium-Atomic-Frequency-Standard-/252895330212 Даже новый за полторы штуки можно купить: https://www.thinksrs.com/products/PRS10.htm
Оффтоп (Move your mouse to the spoiler area to reveal the content) Это приблизительно как Emlid Reach супротив Ляйки или Тримбл.
Добрый день. Вот что то снова посмотрел на эти же формулы, и как то не ясно стало, - вот t = tsv - дельта tsv, .........................а в расчете этой самой дельта используется эта же t ........... поясните пожалуйста.................... вроде раньше я что то по другому воспринимал.....
хотя................. если расположить формулы в обратном порядке, то все нормально будет...... видимо слишком много вчера выкушал на ночь горячего шоколада...........
Добрый день. вот такой появился вопрос, - считает моя программа координаты, - и при этом они постоянно "плывут", и причина мне не понятна. Есть ощущение, что что то важное в алгоритме я упустил....... поясняю: вот запускаю я расчеты несколько десятков раз, для удобства написал программу, которая считает расстояние от истинных моих координат, до тех, что посчитала программа (расчет упрощенный, - для плоскости ). и вот выборка из десятков расчетов координат: время спутники дистанция 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 здесь видно, что с каждым разом, расчетная точка сдвигается все ближе, - потом проходит ближайшую и начинает отдаляться через какое то время один из спутников уходит, и с новым созвездием все повторяется........ подскажите по возможности, - что не так?
