MRTKLIB 0.7.14
Modernized GNSS Positioning Library
Loading...
Searching...
No Matches
mrtk_coords.c
Go to the documentation of this file.
1/*------------------------------------------------------------------------------
2 * mrtk_coords.c : coordinate transformation functions
3 *
4 * Copyright (C) 2026 H.SHIONO (MRTKLIB Project)
5 * Copyright (C) 2023-2025 Cabinet Office, Japan
6 * Copyright (C) 2024-2025 Lighthouse Technology & Consulting Co. Ltd.
7 * Copyright (C) 2023-2025 Japan Aerospace Exploration Agency
8 * Copyright (C) 2023-2025 TOSHIBA ELECTRONIC TECHNOLOGIES CORPORATION
9 * Copyright (C) 2015- Mitsubishi Electric Corp.
10 * Copyright (C) 2014 Geospatial Information Authority of Japan
11 * Copyright (C) 2014 T.SUZUKI
12 * Copyright (C) 2007-2023 T.TAKASU
13 *
14 * SPDX-License-Identifier: BSD-2-Clause
15 *----------------------------------------------------------------------------*/
16/**
17 * @file mrtk_coords.c
18 * @brief MRTKLIB Coordinates Module — Coordinate transformations and geometry.
19 *
20 * This file contains all coordinate transformation and positioning geometry
21 * functions extracted from rtkcmn.c with zero algorithmic changes (pure
22 * cut-and-paste).
23 */
24#include "mrtklib/mrtk_coords.h"
25
26#include <math.h>
27#include <string.h>
28
29#include "mrtklib/mrtk_trace.h"
30
31/*============================================================================
32 * Constants (duplicated from rtklib.h to avoid header dependency)
33 *===========================================================================*/
34
35#define PI 3.1415926535897932 /* pi */
36#define D2R (PI / 180.0) /* deg to rad */
37#define AS2R (D2R / 3600.0) /* arc sec to radian */
38#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */
39#define FE_WGS84 (1.0 / 298.257223563) /* earth flattening (WGS84) */
40#define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */
41#define CLIGHT 299792458.0 /* speed of light (m/s) */
42
43/* max satellite number — generous upper bound for stack arrays in dops() */
44#ifndef MAXSAT
45#define MAXSAT 256
46#endif
47
48/*============================================================================
49 * Forward declarations for legacy trace functions (resolved at link time)
50 *===========================================================================*/
51
52/*============================================================================
53 * Degree / DMS Conversions
54 *===========================================================================*/
55
56/* convert degree to deg-min-sec -----------------------------------------------
57 * convert degree to degree-minute-second
58 * args : double deg I degree
59 * double *dms O degree-minute-second {deg,min,sec}
60 * int ndec I number of decimals of second
61 * return : none
62 *-----------------------------------------------------------------------------*/
63extern void deg2dms(double deg, double* dms, int ndec) {
64 double sign = deg < 0.0 ? -1.0 : 1.0, a = fabs(deg);
65 double unit = pow(0.1, ndec);
66 dms[0] = floor(a);
67 a = (a - dms[0]) * 60.0;
68 dms[1] = floor(a);
69 a = (a - dms[1]) * 60.0;
70 dms[2] = floor(a / unit + 0.5) * unit;
71 if (dms[2] >= 60.0) {
72 dms[2] = 0.0;
73 dms[1] += 1.0;
74 if (dms[1] >= 60.0) {
75 dms[1] = 0.0;
76 dms[0] += 1.0;
77 }
78 }
79 dms[0] *= sign;
80}
81/* convert deg-min-sec to degree -----------------------------------------------
82 * convert degree-minute-second to degree
83 * args : double *dms I degree-minute-second {deg,min,sec}
84 * return : degree
85 *-----------------------------------------------------------------------------*/
86extern double dms2deg(const double* dms) {
87 double sign = dms[0] < 0.0 ? -1.0 : 1.0;
88 return sign * (fabs(dms[0]) + dms[1] / 60.0 + dms[2] / 3600.0);
89}
90
91/*============================================================================
92 * ECEF / Geodetic Conversions
93 *===========================================================================*/
94
95/* transform ecef to geodetic position -----------------------------------------
96 * transform ecef position to geodetic position
97 * args : double *r I ecef position {x,y,z} (m)
98 * double *pos O geodetic position {lat,lon,h} (rad,m)
99 * return : none
100 * notes : WGS84, ellipsoidal height
101 *-----------------------------------------------------------------------------*/
102extern void ecef2pos(const double* r, double* pos) {
103 double e2 = FE_WGS84 * (2.0 - FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp;
104
105 for (z = r[2], zk = 0.0; fabs(z - zk) >= 1E-4;) {
106 zk = z;
107 sinp = z / sqrt(r2 + z * z);
108 v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
109 z = r[2] + v * e2 * sinp;
110 }
111 pos[0] = r2 > 1E-12 ? atan(z / sqrt(r2)) : (r[2] > 0.0 ? PI / 2.0 : -PI / 2.0);
112 pos[1] = r2 > 1E-12 ? atan2(r[1], r[0]) : 0.0;
113 pos[2] = sqrt(r2 + z * z) - v;
114}
115/* transform geodetic to ecef position -----------------------------------------
116 * transform geodetic position to ecef position
117 * args : double *pos I geodetic position {lat,lon,h} (rad,m)
118 * double *r O ecef position {x,y,z} (m)
119 * return : none
120 * notes : WGS84, ellipsoidal height
121 *-----------------------------------------------------------------------------*/
122extern void pos2ecef(const double* pos, double* r) {
123 double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
124 double e2 = FE_WGS84 * (2.0 - FE_WGS84), v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
125
126 r[0] = (v + pos[2]) * cosp * cosl;
127 r[1] = (v + pos[2]) * cosp * sinl;
128 r[2] = (v * (1.0 - e2) + pos[2]) * sinp;
129}
130
131/*============================================================================
132 * ENU / ECEF Transformations
133 *===========================================================================*/
134
135/* ecef to local coordinate transfromation matrix ------------------------------
136 * compute ecef to local coordinate transfromation matrix
137 * args : double *pos I geodetic position {lat,lon} (rad)
138 * double *E O ecef to local coord transformation matrix (3x3)
139 * return : none
140 * notes : matirix stored by column-major order (fortran convention)
141 *-----------------------------------------------------------------------------*/
142extern void xyz2enu(const double* pos, double* E) {
143 double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
144
145 E[0] = -sinl;
146 E[3] = cosl;
147 E[6] = 0.0;
148 E[1] = -sinp * cosl;
149 E[4] = -sinp * sinl;
150 E[7] = cosp;
151 E[2] = cosp * cosl;
152 E[5] = cosp * sinl;
153 E[8] = sinp;
154}
155/* transform ecef vector to local tangental coordinate -------------------------
156 * transform ecef vector to local tangental coordinate
157 * args : double *pos I geodetic position {lat,lon} (rad)
158 * double *r I vector in ecef coordinate {x,y,z}
159 * double *e O vector in local tangental coordinate {e,n,u}
160 * return : none
161 *-----------------------------------------------------------------------------*/
162extern void ecef2enu(const double* pos, const double* r, double* e) {
163 double E[9];
164
165 xyz2enu(pos, E);
166 matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e);
167}
168/* transform local vector to ecef coordinate -----------------------------------
169 * transform local tangental coordinate vector to ecef
170 * args : double *pos I geodetic position {lat,lon} (rad)
171 * double *e I vector in local tangental coordinate {e,n,u}
172 * double *r O vector in ecef coordinate {x,y,z}
173 * return : none
174 *-----------------------------------------------------------------------------*/
175extern void enu2ecef(const double* pos, const double* e, double* r) {
176 double E[9];
177
178 xyz2enu(pos, E);
179 matmul("TN", 3, 1, 3, 1.0, E, e, 0.0, r);
180}
181/* transform covariance to local tangental coordinate --------------------------
182 * transform ecef covariance to local tangental coordinate
183 * args : double *pos I geodetic position {lat,lon} (rad)
184 * double *P I covariance in ecef coordinate
185 * double *Q O covariance in local tangental coordinate
186 * return : none
187 *-----------------------------------------------------------------------------*/
188extern void covenu(const double* pos, const double* P, double* Q) {
189 double E[9], EP[9];
190
191 xyz2enu(pos, E);
192 matmul("NN", 3, 3, 3, 1.0, E, P, 0.0, EP);
193 matmul("NT", 3, 3, 3, 1.0, EP, E, 0.0, Q);
194}
195/* transform local enu coordinate covariance to xyz-ecef -----------------------
196 * transform local enu covariance to xyz-ecef coordinate
197 * args : double *pos I geodetic position {lat,lon} (rad)
198 * double *Q I covariance in local enu coordinate
199 * double *P O covariance in xyz-ecef coordinate
200 * return : none
201 *-----------------------------------------------------------------------------*/
202extern void covecef(const double* pos, const double* Q, double* P) {
203 double E[9], EQ[9];
204
205 xyz2enu(pos, E);
206 matmul("TN", 3, 3, 3, 1.0, E, Q, 0.0, EQ);
207 matmul("NN", 3, 3, 3, 1.0, EQ, E, 0.0, P);
208}
209
210/*============================================================================
211 * ECI / ECEF Transformation
212 *===========================================================================*/
213
214/* coordinate rotation matrix ------------------------------------------------*/
215#define Rx(t, X) \
216 do { \
217 (X)[0] = 1.0; \
218 (X)[1] = (X)[2] = (X)[3] = (X)[6] = 0.0; \
219 (X)[4] = (X)[8] = cos(t); \
220 (X)[7] = sin(t); \
221 (X)[5] = -(X)[7]; \
222 } while (0)
223
224#define Ry(t, X) \
225 do { \
226 (X)[4] = 1.0; \
227 (X)[1] = (X)[3] = (X)[5] = (X)[7] = 0.0; \
228 (X)[0] = (X)[8] = cos(t); \
229 (X)[2] = sin(t); \
230 (X)[6] = -(X)[2]; \
231 } while (0)
232
233#define Rz(t, X) \
234 do { \
235 (X)[8] = 1.0; \
236 (X)[2] = (X)[5] = (X)[6] = (X)[7] = 0.0; \
237 (X)[0] = (X)[4] = cos(t); \
238 (X)[3] = sin(t); \
239 (X)[1] = -(X)[3]; \
240 } while (0)
241
242/* astronomical arguments: f={l,l',F,D,OMG} (rad) ----------------------------*/
243static void ast_args(double t, double* f) {
244 static const double fc[][5] = {/* coefficients for iau 1980 nutation */
245 {134.96340251, 1717915923.2178, 31.8792, 0.051635, -0.00024470},
246 {357.52910918, 129596581.0481, -0.5532, 0.000136, -0.00001149},
247 {93.27209062, 1739527262.8478, -12.7512, -0.001037, 0.00000417},
248 {297.85019547, 1602961601.2090, -6.3706, 0.006593, -0.00003169},
249 {125.04455501, -6962890.2665, 7.4722, 0.007702, -0.00005939}};
250 double tt[4];
251 int i, j;
252
253 for (tt[0] = t, i = 1; i < 4; i++) {
254 tt[i] = tt[i - 1] * t;
255 }
256 for (i = 0; i < 5; i++) {
257 f[i] = fc[i][0] * 3600.0;
258 for (j = 0; j < 4; j++) {
259 f[i] += fc[i][j + 1] * tt[j];
260 }
261 f[i] = fmod(f[i] * AS2R, 2.0 * PI);
262 }
263}
264/* iau 1980 nutation ---------------------------------------------------------*/
265static void nut_iau1980(double t, const double* f, double* dpsi, double* deps) {
266 static const double nut[106][10] = {{0, 0, 0, 0, 1, -6798.4, -171996, -174.2, 92025, 8.9},
267 {0, 0, 2, -2, 2, 182.6, -13187, -1.6, 5736, -3.1},
268 {0, 0, 2, 0, 2, 13.7, -2274, -0.2, 977, -0.5},
269 {0, 0, 0, 0, 2, -3399.2, 2062, 0.2, -895, 0.5},
270 {0, -1, 0, 0, 0, -365.3, -1426, 3.4, 54, -0.1},
271 {1, 0, 0, 0, 0, 27.6, 712, 0.1, -7, 0.0},
272 {0, 1, 2, -2, 2, 121.7, -517, 1.2, 224, -0.6},
273 {0, 0, 2, 0, 1, 13.6, -386, -0.4, 200, 0.0},
274 {1, 0, 2, 0, 2, 9.1, -301, 0.0, 129, -0.1},
275 {0, -1, 2, -2, 2, 365.2, 217, -0.5, -95, 0.3},
276 {-1, 0, 0, 2, 0, 31.8, 158, 0.0, -1, 0.0},
277 {0, 0, 2, -2, 1, 177.8, 129, 0.1, -70, 0.0},
278 {-1, 0, 2, 0, 2, 27.1, 123, 0.0, -53, 0.0},
279 {1, 0, 0, 0, 1, 27.7, 63, 0.1, -33, 0.0},
280 {0, 0, 0, 2, 0, 14.8, 63, 0.0, -2, 0.0},
281 {-1, 0, 2, 2, 2, 9.6, -59, 0.0, 26, 0.0},
282 {-1, 0, 0, 0, 1, -27.4, -58, -0.1, 32, 0.0},
283 {1, 0, 2, 0, 1, 9.1, -51, 0.0, 27, 0.0},
284 {-2, 0, 0, 2, 0, -205.9, -48, 0.0, 1, 0.0},
285 {-2, 0, 2, 0, 1, 1305.5, 46, 0.0, -24, 0.0},
286 {0, 0, 2, 2, 2, 7.1, -38, 0.0, 16, 0.0},
287 {2, 0, 2, 0, 2, 6.9, -31, 0.0, 13, 0.0},
288 {2, 0, 0, 0, 0, 13.8, 29, 0.0, -1, 0.0},
289 {1, 0, 2, -2, 2, 23.9, 29, 0.0, -12, 0.0},
290 {0, 0, 2, 0, 0, 13.6, 26, 0.0, -1, 0.0},
291 {0, 0, 2, -2, 0, 173.3, -22, 0.0, 0, 0.0},
292 {-1, 0, 2, 0, 1, 27.0, 21, 0.0, -10, 0.0},
293 {0, 2, 0, 0, 0, 182.6, 17, -0.1, 0, 0.0},
294 {0, 2, 2, -2, 2, 91.3, -16, 0.1, 7, 0.0},
295 {-1, 0, 0, 2, 1, 32.0, 16, 0.0, -8, 0.0},
296 {0, 1, 0, 0, 1, 386.0, -15, 0.0, 9, 0.0},
297 {1, 0, 0, -2, 1, -31.7, -13, 0.0, 7, 0.0},
298 {0, -1, 0, 0, 1, -346.6, -12, 0.0, 6, 0.0},
299 {2, 0, -2, 0, 0, -1095.2, 11, 0.0, 0, 0.0},
300 {-1, 0, 2, 2, 1, 9.5, -10, 0.0, 5, 0.0},
301 {1, 0, 2, 2, 2, 5.6, -8, 0.0, 3, 0.0},
302 {0, -1, 2, 0, 2, 14.2, -7, 0.0, 3, 0.0},
303 {0, 0, 2, 2, 1, 7.1, -7, 0.0, 3, 0.0},
304 {1, 1, 0, -2, 0, -34.8, -7, 0.0, 0, 0.0},
305 {0, 1, 2, 0, 2, 13.2, 7, 0.0, -3, 0.0},
306 {-2, 0, 0, 2, 1, -199.8, -6, 0.0, 3, 0.0},
307 {0, 0, 0, 2, 1, 14.8, -6, 0.0, 3, 0.0},
308 {2, 0, 2, -2, 2, 12.8, 6, 0.0, -3, 0.0},
309 {1, 0, 0, 2, 0, 9.6, 6, 0.0, 0, 0.0},
310 {1, 0, 2, -2, 1, 23.9, 6, 0.0, -3, 0.0},
311 {0, 0, 0, -2, 1, -14.7, -5, 0.0, 3, 0.0},
312 {0, -1, 2, -2, 1, 346.6, -5, 0.0, 3, 0.0},
313 {2, 0, 2, 0, 1, 6.9, -5, 0.0, 3, 0.0},
314 {1, -1, 0, 0, 0, 29.8, 5, 0.0, 0, 0.0},
315 {1, 0, 0, -1, 0, 411.8, -4, 0.0, 0, 0.0},
316 {0, 0, 0, 1, 0, 29.5, -4, 0.0, 0, 0.0},
317 {0, 1, 0, -2, 0, -15.4, -4, 0.0, 0, 0.0},
318 {1, 0, -2, 0, 0, -26.9, 4, 0.0, 0, 0.0},
319 {2, 0, 0, -2, 1, 212.3, 4, 0.0, -2, 0.0},
320 {0, 1, 2, -2, 1, 119.6, 4, 0.0, -2, 0.0},
321 {1, 1, 0, 0, 0, 25.6, -3, 0.0, 0, 0.0},
322 {1, -1, 0, -1, 0, -3232.9, -3, 0.0, 0, 0.0},
323 {-1, -1, 2, 2, 2, 9.8, -3, 0.0, 1, 0.0},
324 {0, -1, 2, 2, 2, 7.2, -3, 0.0, 1, 0.0},
325 {1, -1, 2, 0, 2, 9.4, -3, 0.0, 1, 0.0},
326 {3, 0, 2, 0, 2, 5.5, -3, 0.0, 1, 0.0},
327 {-2, 0, 2, 0, 2, 1615.7, -3, 0.0, 1, 0.0},
328 {1, 0, 2, 0, 0, 9.1, 3, 0.0, 0, 0.0},
329 {-1, 0, 2, 4, 2, 5.8, -2, 0.0, 1, 0.0},
330 {1, 0, 0, 0, 2, 27.8, -2, 0.0, 1, 0.0},
331 {-1, 0, 2, -2, 1, -32.6, -2, 0.0, 1, 0.0},
332 {0, -2, 2, -2, 1, 6786.3, -2, 0.0, 1, 0.0},
333 {-2, 0, 0, 0, 1, -13.7, -2, 0.0, 1, 0.0},
334 {2, 0, 0, 0, 1, 13.8, 2, 0.0, -1, 0.0},
335 {3, 0, 0, 0, 0, 9.2, 2, 0.0, 0, 0.0},
336 {1, 1, 2, 0, 2, 8.9, 2, 0.0, -1, 0.0},
337 {0, 0, 2, 1, 2, 9.3, 2, 0.0, -1, 0.0},
338 {1, 0, 0, 2, 1, 9.6, -1, 0.0, 0, 0.0},
339 {1, 0, 2, 2, 1, 5.6, -1, 0.0, 1, 0.0},
340 {1, 1, 0, -2, 1, -34.7, -1, 0.0, 0, 0.0},
341 {0, 1, 0, 2, 0, 14.2, -1, 0.0, 0, 0.0},
342 {0, 1, 2, -2, 0, 117.5, -1, 0.0, 0, 0.0},
343 {0, 1, -2, 2, 0, -329.8, -1, 0.0, 0, 0.0},
344 {1, 0, -2, 2, 0, 23.8, -1, 0.0, 0, 0.0},
345 {1, 0, -2, -2, 0, -9.5, -1, 0.0, 0, 0.0},
346 {1, 0, 2, -2, 0, 32.8, -1, 0.0, 0, 0.0},
347 {1, 0, 0, -4, 0, -10.1, -1, 0.0, 0, 0.0},
348 {2, 0, 0, -4, 0, -15.9, -1, 0.0, 0, 0.0},
349 {0, 0, 2, 4, 2, 4.8, -1, 0.0, 0, 0.0},
350 {0, 0, 2, -1, 2, 25.4, -1, 0.0, 0, 0.0},
351 {-2, 0, 2, 4, 2, 7.3, -1, 0.0, 1, 0.0},
352 {2, 0, 2, 2, 2, 4.7, -1, 0.0, 0, 0.0},
353 {0, -1, 2, 0, 1, 14.2, -1, 0.0, 0, 0.0},
354 {0, 0, -2, 0, 1, -13.6, -1, 0.0, 0, 0.0},
355 {0, 0, 4, -2, 2, 12.7, 1, 0.0, 0, 0.0},
356 {0, 1, 0, 0, 2, 409.2, 1, 0.0, 0, 0.0},
357 {1, 1, 2, -2, 2, 22.5, 1, 0.0, -1, 0.0},
358 {3, 0, 2, -2, 2, 8.7, 1, 0.0, 0, 0.0},
359 {-2, 0, 2, 2, 2, 14.6, 1, 0.0, -1, 0.0},
360 {-1, 0, 0, 0, 2, -27.3, 1, 0.0, -1, 0.0},
361 {0, 0, -2, 2, 1, -169.0, 1, 0.0, 0, 0.0},
362 {0, 1, 2, 0, 1, 13.1, 1, 0.0, 0, 0.0},
363 {-1, 0, 4, 0, 2, 9.1, 1, 0.0, 0, 0.0},
364 {2, 1, 0, -2, 0, 131.7, 1, 0.0, 0, 0.0},
365 {2, 0, 0, 2, 0, 7.1, 1, 0.0, 0, 0.0},
366 {2, 0, 2, -2, 1, 12.8, 1, 0.0, -1, 0.0},
367 {2, 0, -2, 0, 1, -943.2, 1, 0.0, 0, 0.0},
368 {1, -1, 0, -2, 0, -29.3, 1, 0.0, 0, 0.0},
369 {-1, 0, 0, 1, 1, -388.3, 1, 0.0, 0, 0.0},
370 {-1, -1, 0, 2, 1, 35.0, 1, 0.0, 0, 0.0},
371 {0, 1, 0, 1, 0, 27.3, 1, 0.0, 0, 0.0}};
372 double ang;
373 int i, j;
374
375 *dpsi = *deps = 0.0;
376
377 for (i = 0; i < 106; i++) {
378 ang = 0.0;
379 for (j = 0; j < 5; j++) {
380 ang += nut[i][j] * f[j];
381 }
382 *dpsi += (nut[i][6] + nut[i][7] * t) * sin(ang);
383 *deps += (nut[i][8] + nut[i][9] * t) * cos(ang);
384 }
385 *dpsi *= 1E-4 * AS2R; /* 0.1 mas -> rad */
386 *deps *= 1E-4 * AS2R;
387}
388/* eci to ecef transformation matrix -------------------------------------------
389 * compute eci to ecef transformation matrix
390 * args : gtime_t tutc I time in utc
391 * double *erpv I erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d)
392 * double *U O eci to ecef transformation matrix (3 x 3)
393 * double *gmst IO greenwich mean sidereal time (rad)
394 * (NULL: no output)
395 * return : none
396 * note : see ref [3] chap 5
397 * not thread-safe
398 *-----------------------------------------------------------------------------*/
399extern void eci2ecef(gtime_t tutc, const double* erpv, double* U, double* gmst) {
400 const double ep2000[] = {2000, 1, 1, 12, 0, 0};
401 static gtime_t tutc_;
402 static double U_[9], gmst_;
403 gtime_t tgps;
404 double eps, ze, th, z, t, t2, t3, dpsi, deps, gast, f[5];
405 double R1[9], R2[9], R3[9], R[9], W[9], N[9], P[9], NP[9];
406 int i;
407
408 trace(NULL, 4, "eci2ecef: tutc=%s\n", time_str(tutc, 3));
409
410 if (fabs(timediff(tutc, tutc_)) < 0.01) { /* read cache */
411 for (i = 0; i < 9; i++) {
412 U[i] = U_[i];
413 }
414 if (gmst) {
415 *gmst = gmst_;
416 }
417 return;
418 }
419 tutc_ = tutc;
420
421 /* terrestrial time */
422 tgps = utc2gpst(tutc_);
423 t = (timediff(tgps, epoch2time(ep2000)) + 19.0 + 32.184) / 86400.0 / 36525.0;
424 t2 = t * t;
425 t3 = t2 * t;
426
427 /* astronomical arguments */
428 ast_args(t, f);
429
430 /* iau 1976 precession */
431 ze = (2306.2181 * t + 0.30188 * t2 + 0.017998 * t3) * AS2R;
432 th = (2004.3109 * t - 0.42665 * t2 - 0.041833 * t3) * AS2R;
433 z = (2306.2181 * t + 1.09468 * t2 + 0.018203 * t3) * AS2R;
434 eps = (84381.448 - 46.8150 * t - 0.00059 * t2 + 0.001813 * t3) * AS2R;
435 Rz(-z, R1);
436 Ry(th, R2);
437 Rz(-ze, R3);
438 matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, R);
439 matmul("NN", 3, 3, 3, 1.0, R, R3, 0.0, P); /* P=Rz(-z)*Ry(th)*Rz(-ze) */
440
441 /* iau 1980 nutation */
442 nut_iau1980(t, f, &dpsi, &deps);
443 Rx(-eps - deps, R1);
444 Rz(-dpsi, R2);
445 Rx(eps, R3);
446 matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, R);
447 matmul("NN", 3, 3, 3, 1.0, R, R3, 0.0, N); /* N=Rx(-eps)*Rz(-dspi)*Rx(eps) */
448
449 /* greenwich aparent sidereal time (rad) */
450 gmst_ = utc2gmst(tutc_, erpv[2]);
451 gast = gmst_ + dpsi * cos(eps);
452 gast += (0.00264 * sin(f[4]) + 0.000063 * sin(2.0 * f[4])) * AS2R;
453
454 /* eci to ecef transformation matrix */
455 Ry(-erpv[0], R1);
456 Rx(-erpv[1], R2);
457 Rz(gast, R3);
458 matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, W);
459 matmul("NN", 3, 3, 3, 1.0, W, R3, 0.0, R); /* W=Ry(-xp)*Rx(-yp) */
460 matmul("NN", 3, 3, 3, 1.0, N, P, 0.0, NP);
461 matmul("NN", 3, 3, 3, 1.0, R, NP, 0.0, U_); /* U=W*Rz(gast)*N*P */
462
463 for (i = 0; i < 9; i++) {
464 U[i] = U_[i];
465 }
466 if (gmst) {
467 *gmst = gmst_;
468 }
469
470 trace(NULL, 5, "gmst=%.12f gast=%.12f\n", gmst_, gast);
471 trace(NULL, 5, "P=\n");
472 tracemat(NULL, 5, P, 3, 3, 15, 12);
473 trace(NULL, 5, "N=\n");
474 tracemat(NULL, 5, N, 3, 3, 15, 12);
475 trace(NULL, 5, "W=\n");
476 tracemat(NULL, 5, W, 3, 3, 15, 12);
477 trace(NULL, 5, "U=\n");
478 tracemat(NULL, 5, U, 3, 3, 15, 12);
479}
480
481/*============================================================================
482 * Positioning Geometry
483 *===========================================================================*/
484
485/* geometric distance ----------------------------------------------------------
486 * compute geometric distance and receiver-to-satellite unit vector
487 * args : double *rs I satellilte position (ecef at transmission) (m)
488 * double *rr I receiver position (ecef at reception) (m)
489 * double *e O line-of-sight vector (ecef)
490 * return : geometric distance (m) (0>:error/no satellite position)
491 * notes : distance includes sagnac effect correction
492 *-----------------------------------------------------------------------------*/
493extern double geodist(const double* rs, const double* rr, double* e) {
494 double r;
495 int i;
496
497 if (norm(rs, 3) < RE_WGS84) {
498 return -1.0;
499 }
500 for (i = 0; i < 3; i++) {
501 e[i] = rs[i] - rr[i];
502 }
503 r = norm(e, 3);
504 for (i = 0; i < 3; i++) {
505 e[i] /= r;
506 }
507 return r + OMGE * (rs[0] * rr[1] - rs[1] * rr[0]) / CLIGHT;
508}
509/* satellite azimuth/elevation angle -------------------------------------------
510 * compute satellite azimuth/elevation angle
511 * args : double *pos I geodetic position {lat,lon,h} (rad,m)
512 * double *e I receiver-to-satellilte unit vevtor (ecef)
513 * double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
514 * (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
515 * return : elevation angle (rad)
516 *-----------------------------------------------------------------------------*/
517extern double satazel(const double* pos, const double* e, double* azel) {
518 double az = 0.0, el = PI / 2.0, enu[3];
519
520 if (pos[2] > -RE_WGS84) {
521 ecef2enu(pos, e, enu);
522 az = dot(enu, enu, 2) < 1E-12 ? 0.0 : atan2(enu[0], enu[1]);
523 if (az < 0.0) {
524 az += 2 * PI;
525 }
526 el = asin(enu[2]);
527 }
528 if (azel) {
529 azel[0] = az;
530 azel[1] = el;
531 }
532 return el;
533}
534/* compute dops ----------------------------------------------------------------
535 * compute DOP (dilution of precision)
536 * args : int ns I number of satellites
537 * double *azel I satellite azimuth/elevation angle (rad)
538 * double elmin I elevation cutoff angle (rad)
539 * double *dop O DOPs {GDOP,PDOP,HDOP,VDOP}
540 * return : none
541 * notes : dop[0]-[3] return 0 in case of dop computation error
542 *-----------------------------------------------------------------------------*/
543#define SQRT(x) ((x) < 0.0 || (x) != (x) ? 0.0 : sqrt(x))
544
545extern void dops(int ns, const double* azel, double elmin, double* dop) {
546 double H[4 * MAXSAT], Q[16], cosel, sinel;
547 int i, n;
548
549 for (i = 0; i < 4; i++) {
550 dop[i] = 0.0;
551 }
552 for (i = n = 0; i < ns && i < MAXSAT; i++) {
553 if (azel[1 + i * 2] < elmin || azel[1 + i * 2] <= 0.0) {
554 continue;
555 }
556 cosel = cos(azel[1 + i * 2]);
557 sinel = sin(azel[1 + i * 2]);
558 H[4 * n] = cosel * sin(azel[i * 2]);
559 H[1 + 4 * n] = cosel * cos(azel[i * 2]);
560 H[2 + 4 * n] = sinel;
561 H[3 + 4 * n++] = 1.0;
562 }
563 if (n < 4) {
564 return;
565 }
566
567 matmul("NT", 4, 4, n, 1.0, H, H, 0.0, Q);
568 if (!matinv(Q, 4)) {
569 dop[0] = SQRT(Q[0] + Q[5] + Q[10] + Q[15]); /* GDOP */
570 dop[1] = SQRT(Q[0] + Q[5] + Q[10]); /* PDOP */
571 dop[2] = SQRT(Q[0] + Q[5]); /* HDOP */
572 dop[3] = SQRT(Q[10]); /* VDOP */
573 }
574}
geodist
double geodist(const double *rs, const double *rr, double *e)
Compute geometric distance and receiver-to-satellite unit vector.
Definition mrtk_coords.c:493
enu2ecef
void enu2ecef(const double *pos, const double *e, double *r)
Transform local tangential coordinate vector to ECEF.
Definition mrtk_coords.c:175
pos2ecef
void pos2ecef(const double *pos, double *r)
Transform geodetic position to ECEF position.
Definition mrtk_coords.c:122
satazel
double satazel(const double *pos, const double *e, double *azel)
Compute satellite azimuth/elevation angle.
Definition mrtk_coords.c:517
CLIGHT
#define CLIGHT
Definition mrtk_coords.c:41
FE_WGS84
#define FE_WGS84
Definition mrtk_coords.c:39
PI
#define PI
Definition mrtk_coords.c:35
xyz2enu
void xyz2enu(const double *pos, double *E)
Compute ECEF to local coordinate transformation matrix.
Definition mrtk_coords.c:142
covecef
void covecef(const double *pos, const double *Q, double *P)
Transform local ENU covariance to ECEF coordinate.
Definition mrtk_coords.c:202
MAXSAT
#define MAXSAT
Definition mrtk_coords.c:45
covenu
void covenu(const double *pos, const double *P, double *Q)
Transform ECEF covariance to local tangential coordinate.
Definition mrtk_coords.c:188
eci2ecef
void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst)
Compute ECI to ECEF transformation matrix.
Definition mrtk_coords.c:399
ast_args
static void ast_args(double t, double *f)
Definition mrtk_coords.c:243
RE_WGS84
#define RE_WGS84
Definition mrtk_coords.c:38
AS2R
#define AS2R
Definition mrtk_coords.c:37
Rz
#define Rz(t, X)
Definition mrtk_coords.c:233
OMGE
#define OMGE
Definition mrtk_coords.c:40
Rx
#define Rx(t, X)
Definition mrtk_coords.c:215
deg2dms
void deg2dms(double deg, double *dms, int ndec)
Convert degree to degree-minute-second.
Definition mrtk_coords.c:63
dops
void dops(int ns, const double *azel, double elmin, double *dop)
Compute DOP (dilution of precision).
Definition mrtk_coords.c:545
SQRT
#define SQRT(x)
Definition mrtk_coords.c:543
ecef2pos
void ecef2pos(const double *r, double *pos)
Transform ECEF position to geodetic position.
Definition mrtk_coords.c:102
nut_iau1980
static void nut_iau1980(double t, const double *f, double *dpsi, double *deps)
Definition mrtk_coords.c:265
Ry
#define Ry(t, X)
Definition mrtk_coords.c:224
dms2deg
double dms2deg(const double *dms)
Convert degree-minute-second to degree.
Definition mrtk_coords.c:86
ecef2enu
void ecef2enu(const double *pos, const double *r, double *e)
Transform ECEF vector to local tangential coordinate.
Definition mrtk_coords.c:162
mrtk_coords.h
MRTKLIB Coordinates Module — Coordinate transformations and geometry.
matinv
int matinv(double *A, int n)
Inverse of matrix (A = A^-1).
Definition mrtk_mat.c:421
norm
double norm(const double *a, int n)
Euclidean norm of vector.
Definition mrtk_mat.c:165
matmul
void matmul(const char *tr, int n, int k, int m, double alpha, const double *A, const double *B, double beta, double *C)
Multiply matrix by matrix (C = alpha*A*B + beta*C).
Definition mrtk_mat.c:310
dot
double dot(const double *a, const double *b, int n)
Inner product of vectors.
Definition mrtk_mat.c:151
NP
#define NP(opt)
Definition mrtk_ppp.c:212
epoch2time
gtime_t epoch2time(const double *ep)
Convert calendar day/time to gtime_t struct.
Definition mrtk_time.c:113
utc2gmst
double utc2gmst(gtime_t t, double ut1_utc)
Convert UTC to Greenwich Mean Sidereal Time.
Definition mrtk_time.c:485
timediff
double timediff(gtime_t t1, gtime_t t2)
Compute time difference (t1 - t2).
Definition mrtk_time.c:279
time_str
char * time_str(gtime_t t, int n)
Get time string (not reentrant).
Definition mrtk_time.c:530
utc2gpst
gtime_t utc2gpst(gtime_t t)
Convert UTC to GPS time considering leap seconds.
Definition mrtk_time.c:440
tracemat
void tracemat(mrtk_ctx_t *ctx, int level, const double *A, int n, int m, int p, int q)
Output trace matrix.
Definition mrtk_trace.c:106
trace
void trace(mrtk_ctx_t *ctx, int level, const char *format,...)
Output trace message.
Definition mrtk_trace.c:78
mrtk_trace.h
MRTKLIB Trace Module — debug trace and logging functions.
gtime_t
GNSS time representation.
Definition mrtk_time.h:49