gnss-js 1.27.0 → 1.29.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/dist/chunk-4KMWWCUR.js +120 -0
- package/dist/{chunk-4XGKCVE3.js → chunk-5WHKMKIB.js} +827 -2
- package/dist/{chunk-QVNNHACQ.js → chunk-6GTS5MVI.js} +0 -118
- package/dist/index.cjs +847 -19
- package/dist/index.d.cts +3 -3
- package/dist/index.d.ts +3 -3
- package/dist/index.js +22 -4
- package/dist/positioning.cjs +900 -1
- package/dist/positioning.d.cts +231 -2
- package/dist/positioning.d.ts +231 -2
- package/dist/positioning.js +18 -1
- package/dist/rinex.d.cts +2 -49
- package/dist/rinex.d.ts +2 -49
- package/dist/rinex.js +5 -3
- package/dist/sp3-BAq8l39b.d.cts +70 -0
- package/dist/sp3-BAq8l39b.d.ts +70 -0
- package/package.json +1 -1
- package/dist/ionex-DaW5x9nq.d.cts +0 -23
- package/dist/ionex-DaW5x9nq.d.ts +0 -23
package/dist/positioning.cjs
CHANGED
|
@@ -22,15 +22,23 @@ var positioning_exports = {};
|
|
|
22
22
|
__export(positioning_exports, {
|
|
23
23
|
IONO_L1_M_PER_TECU: () => IONO_L1_M_PER_TECU,
|
|
24
24
|
RtkFloatEngine: () => RtkFloatEngine,
|
|
25
|
+
buildPppAntenna: () => buildPppAntenna,
|
|
26
|
+
createPppCorrections: () => createPppCorrections,
|
|
25
27
|
gimSlantIonoDelayL1: () => gimSlantIonoDelayL1,
|
|
26
28
|
gimVerticalTec: () => gimVerticalTec,
|
|
29
|
+
gmst: () => gmst,
|
|
27
30
|
ionoFree: () => ionoFree,
|
|
28
31
|
klobucharDelay: () => klobucharDelay,
|
|
29
32
|
lambdaReduction: () => lambdaReduction,
|
|
30
33
|
lambdaSearch: () => lambdaSearch,
|
|
34
|
+
moonEcef: () => moonEcef,
|
|
35
|
+
niellMapping: () => niellMapping,
|
|
31
36
|
satClockCorrection: () => satClockCorrection,
|
|
37
|
+
solidEarthTide: () => solidEarthTide,
|
|
32
38
|
solveDgnss: () => solveDgnss,
|
|
39
|
+
solvePpp: () => solvePpp,
|
|
33
40
|
solveSpp: () => solveSpp,
|
|
41
|
+
sunEcef: () => sunEcef,
|
|
34
42
|
toRtkEpoch: () => toRtkEpoch
|
|
35
43
|
});
|
|
36
44
|
module.exports = __toCommonJS(positioning_exports);
|
|
@@ -41,6 +49,9 @@ var WGS84_FLATTENING = 1 / 298.257223563;
|
|
|
41
49
|
var WGS84_ECCENTRICITY_SQUARED = 0.006694379990197;
|
|
42
50
|
|
|
43
51
|
// src/coordinates/ecef.ts
|
|
52
|
+
function clampUnit(x) {
|
|
53
|
+
return x < -1 ? -1 : x > 1 ? 1 : x;
|
|
54
|
+
}
|
|
44
55
|
function ecefToGeodetic(x, y, z) {
|
|
45
56
|
const lon = Math.atan2(y, x);
|
|
46
57
|
const p = Math.sqrt(x * x + y * y);
|
|
@@ -64,6 +75,24 @@ function ecefToGeodetic(x, y, z) {
|
|
|
64
75
|
const alt = p / Math.cos(lat) - N;
|
|
65
76
|
return [lat, lon, alt];
|
|
66
77
|
}
|
|
78
|
+
function getEnuDifference(x, y, z, xRef, yRef, zRef) {
|
|
79
|
+
const [latRef, lonRef] = ecefToGeodetic(xRef, yRef, zRef);
|
|
80
|
+
const deltaX = x - xRef;
|
|
81
|
+
const deltaY = y - yRef;
|
|
82
|
+
const deltaZ = z - zRef;
|
|
83
|
+
const deltaE = -Math.sin(lonRef) * deltaX + Math.cos(lonRef) * deltaY;
|
|
84
|
+
const deltaN = -Math.cos(lonRef) * Math.sin(latRef) * deltaX - Math.sin(lonRef) * Math.sin(latRef) * deltaY + Math.cos(latRef) * deltaZ;
|
|
85
|
+
const deltaU = Math.cos(lonRef) * Math.cos(latRef) * deltaX + Math.sin(lonRef) * Math.cos(latRef) * deltaY + Math.sin(latRef) * deltaZ;
|
|
86
|
+
return [deltaE, deltaN, deltaU];
|
|
87
|
+
}
|
|
88
|
+
function getAer(x, y, z, xRef, yRef, zRef) {
|
|
89
|
+
const slant = Math.sqrt((x - xRef) ** 2 + (y - yRef) ** 2 + (z - zRef) ** 2);
|
|
90
|
+
if (!slant) return [0, 0, 0];
|
|
91
|
+
const [deltaE, deltaN, deltaU] = getEnuDifference(x, y, z, xRef, yRef, zRef);
|
|
92
|
+
const elevation = Math.asin(clampUnit(deltaU / slant));
|
|
93
|
+
const azimuth = Math.atan2(deltaE, deltaN);
|
|
94
|
+
return [elevation, azimuth, slant];
|
|
95
|
+
}
|
|
67
96
|
|
|
68
97
|
// src/constants/time.ts
|
|
69
98
|
var MILLISECONDS_IN_SECOND = 1e3;
|
|
@@ -1518,6 +1547,867 @@ var RtkFloatEngine = class {
|
|
|
1518
1547
|
}
|
|
1519
1548
|
};
|
|
1520
1549
|
|
|
1550
|
+
// src/rinex/sp3.ts
|
|
1551
|
+
function sp3Position(sp3, prn, tMs, order = 9) {
|
|
1552
|
+
const samples = sp3.satellites[prn];
|
|
1553
|
+
const { epochs } = sp3;
|
|
1554
|
+
const n = epochs.length;
|
|
1555
|
+
if (!samples || n < order) return null;
|
|
1556
|
+
if (tMs < epochs[0] || tMs > epochs[n - 1]) return null;
|
|
1557
|
+
let lo = 0;
|
|
1558
|
+
let hi = n - 1;
|
|
1559
|
+
while (lo < hi) {
|
|
1560
|
+
const mid = lo + hi + 1 >> 1;
|
|
1561
|
+
if (epochs[mid] <= tMs) lo = mid;
|
|
1562
|
+
else hi = mid - 1;
|
|
1563
|
+
}
|
|
1564
|
+
let start = lo - (order >> 1) + (order % 2 === 0 ? 1 : 0);
|
|
1565
|
+
start = Math.max(0, Math.min(n - order, start));
|
|
1566
|
+
for (let i = start; i < start + order; i++) {
|
|
1567
|
+
if (!samples[i]) return null;
|
|
1568
|
+
}
|
|
1569
|
+
const t0 = epochs[start];
|
|
1570
|
+
const t = (tMs - t0) / 1e3;
|
|
1571
|
+
let x = 0;
|
|
1572
|
+
let y = 0;
|
|
1573
|
+
let z = 0;
|
|
1574
|
+
for (let i = 0; i < order; i++) {
|
|
1575
|
+
const ti = (epochs[start + i] - t0) / 1e3;
|
|
1576
|
+
let w = 1;
|
|
1577
|
+
for (let j = 0; j < order; j++) {
|
|
1578
|
+
if (j === i) continue;
|
|
1579
|
+
const tj = (epochs[start + j] - t0) / 1e3;
|
|
1580
|
+
w *= (t - tj) / (ti - tj);
|
|
1581
|
+
}
|
|
1582
|
+
const s = samples[start + i];
|
|
1583
|
+
x += w * s.x;
|
|
1584
|
+
y += w * s.y;
|
|
1585
|
+
z += w * s.z;
|
|
1586
|
+
}
|
|
1587
|
+
let clk = null;
|
|
1588
|
+
const a = samples[lo];
|
|
1589
|
+
const b = samples[Math.min(lo + 1, n - 1)];
|
|
1590
|
+
if (a?.clk != null && b?.clk != null) {
|
|
1591
|
+
const span = epochs[Math.min(lo + 1, n - 1)] - epochs[lo];
|
|
1592
|
+
const f = span > 0 ? (tMs - epochs[lo]) / span : 0;
|
|
1593
|
+
clk = a.clk * (1 - f) + b.clk * f;
|
|
1594
|
+
} else if (a?.clk != null) {
|
|
1595
|
+
clk = a.clk;
|
|
1596
|
+
}
|
|
1597
|
+
return { x, y, z, clk };
|
|
1598
|
+
}
|
|
1599
|
+
|
|
1600
|
+
// src/positioning/ppp-tropo.ts
|
|
1601
|
+
var HYD_AVG_A = [
|
|
1602
|
+
0.0012769934,
|
|
1603
|
+
1268323e-9,
|
|
1604
|
+
0.0012465397,
|
|
1605
|
+
0.0012196049,
|
|
1606
|
+
0.0012045996
|
|
1607
|
+
];
|
|
1608
|
+
var HYD_AVG_B = [
|
|
1609
|
+
0.0029153695,
|
|
1610
|
+
0.0029152299,
|
|
1611
|
+
0.0029288445,
|
|
1612
|
+
0.0029022565,
|
|
1613
|
+
0.0029024912
|
|
1614
|
+
];
|
|
1615
|
+
var HYD_AVG_C = [
|
|
1616
|
+
0.062610505,
|
|
1617
|
+
0.062837393,
|
|
1618
|
+
0.063721774,
|
|
1619
|
+
0.063824265,
|
|
1620
|
+
0.064258455
|
|
1621
|
+
];
|
|
1622
|
+
var HYD_AMP_A = [0, 12709626e-12, 26523662e-12, 34000452e-12, 41202191e-12];
|
|
1623
|
+
var HYD_AMP_B = [0, 21414979e-12, 30160779e-12, 72562722e-12, 11723375e-11];
|
|
1624
|
+
var HYD_AMP_C = [0, 901284e-10, 43497037e-12, 84795348e-11, 0.0017037206];
|
|
1625
|
+
var HT_A = 253e-7;
|
|
1626
|
+
var HT_B = 549e-5;
|
|
1627
|
+
var HT_C = 114e-5;
|
|
1628
|
+
var WET_A = [
|
|
1629
|
+
58021897e-11,
|
|
1630
|
+
56794847e-11,
|
|
1631
|
+
58118019e-11,
|
|
1632
|
+
59727542e-11,
|
|
1633
|
+
61641693e-11
|
|
1634
|
+
];
|
|
1635
|
+
var WET_B = [
|
|
1636
|
+
0.0014275268,
|
|
1637
|
+
0.0015138625,
|
|
1638
|
+
0.0014572752,
|
|
1639
|
+
0.0015007428,
|
|
1640
|
+
0.0017599082
|
|
1641
|
+
];
|
|
1642
|
+
var WET_C = [
|
|
1643
|
+
0.043472961,
|
|
1644
|
+
0.04672951,
|
|
1645
|
+
0.043908931,
|
|
1646
|
+
0.044626982,
|
|
1647
|
+
0.054736038
|
|
1648
|
+
];
|
|
1649
|
+
var LATS = [15, 30, 45, 60, 75];
|
|
1650
|
+
function interpLat(table, absLatDeg) {
|
|
1651
|
+
if (absLatDeg <= 15) return table[0];
|
|
1652
|
+
if (absLatDeg >= 75) return table[4];
|
|
1653
|
+
let i = 0;
|
|
1654
|
+
while (i < 4 && absLatDeg > LATS[i + 1]) i++;
|
|
1655
|
+
const t = (absLatDeg - LATS[i]) / (LATS[i + 1] - LATS[i]);
|
|
1656
|
+
return table[i] + t * (table[i + 1] - table[i]);
|
|
1657
|
+
}
|
|
1658
|
+
function marini(sinEl, a, b, c) {
|
|
1659
|
+
const num = 1 + a / (1 + b / (1 + c));
|
|
1660
|
+
const den = sinEl + a / (sinEl + b / (sinEl + c));
|
|
1661
|
+
return num / den;
|
|
1662
|
+
}
|
|
1663
|
+
function dayOfYear(timeMs) {
|
|
1664
|
+
const d = new Date(timeMs);
|
|
1665
|
+
const start = Date.UTC(d.getUTCFullYear(), 0, 1);
|
|
1666
|
+
return Math.floor((timeMs - start) / 864e5) + 1;
|
|
1667
|
+
}
|
|
1668
|
+
function niellMapping(elRad, latRad, heightM, timeMs) {
|
|
1669
|
+
const sinEl = Math.sin(Math.max(elRad, 1e-3));
|
|
1670
|
+
const absLat = Math.abs(latRad * 180 / Math.PI);
|
|
1671
|
+
const hKm = heightM / 1e3;
|
|
1672
|
+
let doy = dayOfYear(timeMs);
|
|
1673
|
+
if (latRad < 0) doy += 365.25 / 2;
|
|
1674
|
+
const yfrac = Math.cos(2 * Math.PI * (doy - 28) / 365.25);
|
|
1675
|
+
const ah = interpLat(HYD_AVG_A, absLat) - interpLat(HYD_AMP_A, absLat) * yfrac;
|
|
1676
|
+
const bh = interpLat(HYD_AVG_B, absLat) - interpLat(HYD_AMP_B, absLat) * yfrac;
|
|
1677
|
+
const ch = interpLat(HYD_AVG_C, absLat) - interpLat(HYD_AMP_C, absLat) * yfrac;
|
|
1678
|
+
const mHt = marini(sinEl, HT_A, HT_B, HT_C);
|
|
1679
|
+
const dmdh = 1 / sinEl - mHt;
|
|
1680
|
+
const mh = marini(sinEl, ah, bh, ch) + dmdh * hKm;
|
|
1681
|
+
const aw = interpLat(WET_A, absLat);
|
|
1682
|
+
const bw = interpLat(WET_B, absLat);
|
|
1683
|
+
const cw = interpLat(WET_C, absLat);
|
|
1684
|
+
const mw = marini(sinEl, aw, bw, cw);
|
|
1685
|
+
const h = heightM < 0 ? 0 : heightM > 1e4 ? 1e4 : heightM;
|
|
1686
|
+
const pres = 1013.25 * Math.pow(1 - 22557e-9 * h, 5.2568);
|
|
1687
|
+
const zhd = 22768e-7 * pres / (1 - 266e-5 * Math.cos(2 * latRad) - 28e-5 * (h / 1e3));
|
|
1688
|
+
return { zhd, mh, mw };
|
|
1689
|
+
}
|
|
1690
|
+
|
|
1691
|
+
// src/positioning/ppp-astro.ts
|
|
1692
|
+
var AU = 149597870700;
|
|
1693
|
+
var GPS_UTC_LEAP = 18;
|
|
1694
|
+
var J2000_MS = Date.UTC(2e3, 0, 1, 12, 0, 0);
|
|
1695
|
+
var DEG = Math.PI / 180;
|
|
1696
|
+
function daysSinceJ2000(timeMs) {
|
|
1697
|
+
return (timeMs - GPS_UTC_LEAP * 1e3 - J2000_MS) / 864e5;
|
|
1698
|
+
}
|
|
1699
|
+
function gmst(timeMs) {
|
|
1700
|
+
const d = daysSinceJ2000(timeMs);
|
|
1701
|
+
let deg = 280.46061837 + 360.98564736629 * d;
|
|
1702
|
+
deg %= 360;
|
|
1703
|
+
if (deg < 0) deg += 360;
|
|
1704
|
+
return deg * DEG;
|
|
1705
|
+
}
|
|
1706
|
+
function eciToEcef(v, theta) {
|
|
1707
|
+
const c = Math.cos(theta);
|
|
1708
|
+
const s = Math.sin(theta);
|
|
1709
|
+
return [v[0] * c + v[1] * s, -v[0] * s + v[1] * c, v[2]];
|
|
1710
|
+
}
|
|
1711
|
+
function sunEcef(timeMs) {
|
|
1712
|
+
const n = daysSinceJ2000(timeMs);
|
|
1713
|
+
const L = (280.46 + 0.9856474 * n) * DEG;
|
|
1714
|
+
const g = (357.528 + 0.9856003 * n) * DEG;
|
|
1715
|
+
const lambda = L + (1.915 * Math.sin(g) + 0.02 * Math.sin(2 * g)) * DEG;
|
|
1716
|
+
const eps = (23.439 - 4e-7 * n) * DEG;
|
|
1717
|
+
const R = (1.00014 - 0.01671 * Math.cos(g) - 14e-5 * Math.cos(2 * g)) * AU;
|
|
1718
|
+
const eci = [
|
|
1719
|
+
R * Math.cos(lambda),
|
|
1720
|
+
R * Math.cos(eps) * Math.sin(lambda),
|
|
1721
|
+
R * Math.sin(eps) * Math.sin(lambda)
|
|
1722
|
+
];
|
|
1723
|
+
return eciToEcef(eci, gmst(timeMs));
|
|
1724
|
+
}
|
|
1725
|
+
function moonEcef(timeMs) {
|
|
1726
|
+
const T = daysSinceJ2000(timeMs) / 36525;
|
|
1727
|
+
const L0 = 218.31617 + 481267.88088 * T - 1.3972 * T;
|
|
1728
|
+
const l = 134.96292 + 477198.86753 * T;
|
|
1729
|
+
const lp = 357.52543 + 35999.04944 * T;
|
|
1730
|
+
const F = 93.27283 + 483202.01873 * T;
|
|
1731
|
+
const D = 297.85027 + 445267.11135 * T;
|
|
1732
|
+
const s = (x) => Math.sin(x * DEG);
|
|
1733
|
+
const c = (x) => Math.cos(x * DEG);
|
|
1734
|
+
const lon = L0 + 6.28875 * s(l) + 1.274018 * s(2 * D - l) + 0.658309 * s(2 * D) + 0.213616 * s(2 * l) - 0.185596 * s(lp) - 0.114336 * s(2 * F) + 0.058793 * s(2 * D - 2 * l) + 0.057212 * s(2 * D - lp - l) + 0.05332 * s(2 * D + l);
|
|
1735
|
+
const lat = 5.128189 * s(F) + 0.280606 * s(l + F) + 0.277693 * s(l - F) + 0.173238 * s(2 * D - F) + 0.055413 * s(2 * D + F - l) + 0.046272 * s(2 * D - F - l);
|
|
1736
|
+
const r = (385e3 - 20905 * c(l) - 3699 * c(2 * D - l) - 2956 * c(2 * D) - 570 * c(2 * l)) * 1e3;
|
|
1737
|
+
const eps = (23.4393 - 4e-7 * daysSinceJ2000(timeMs)) * DEG;
|
|
1738
|
+
const lonR = lon * DEG;
|
|
1739
|
+
const latR = lat * DEG;
|
|
1740
|
+
const xe = r * Math.cos(latR) * Math.cos(lonR);
|
|
1741
|
+
const ye = r * Math.cos(latR) * Math.sin(lonR);
|
|
1742
|
+
const ze = r * Math.sin(latR);
|
|
1743
|
+
const eci = [
|
|
1744
|
+
xe,
|
|
1745
|
+
ye * Math.cos(eps) - ze * Math.sin(eps),
|
|
1746
|
+
ye * Math.sin(eps) + ze * Math.cos(eps)
|
|
1747
|
+
];
|
|
1748
|
+
return eciToEcef(eci, gmst(timeMs));
|
|
1749
|
+
}
|
|
1750
|
+
var H2 = 0.6078;
|
|
1751
|
+
var L2 = 0.0847;
|
|
1752
|
+
var RE = 6378137;
|
|
1753
|
+
var GM_SUN_RATIO = 332946.0482;
|
|
1754
|
+
var GM_MOON_RATIO = 0.0123000371;
|
|
1755
|
+
function solidEarthTide(rcv, sun, moon) {
|
|
1756
|
+
const rNorm = Math.hypot(rcv[0], rcv[1], rcv[2]);
|
|
1757
|
+
const rHat = [
|
|
1758
|
+
rcv[0] / rNorm,
|
|
1759
|
+
rcv[1] / rNorm,
|
|
1760
|
+
rcv[2] / rNorm
|
|
1761
|
+
];
|
|
1762
|
+
const disp = [0, 0, 0];
|
|
1763
|
+
for (const [body, ratio] of [
|
|
1764
|
+
[sun, GM_SUN_RATIO],
|
|
1765
|
+
[moon, GM_MOON_RATIO]
|
|
1766
|
+
]) {
|
|
1767
|
+
const rj = Math.hypot(body[0], body[1], body[2]);
|
|
1768
|
+
const jHat = [
|
|
1769
|
+
body[0] / rj,
|
|
1770
|
+
body[1] / rj,
|
|
1771
|
+
body[2] / rj
|
|
1772
|
+
];
|
|
1773
|
+
const dot2 = rHat[0] * jHat[0] + rHat[1] * jHat[1] + rHat[2] * jHat[2];
|
|
1774
|
+
const factor = ratio * RE * RE * RE * RE / (rj * rj * rj);
|
|
1775
|
+
const radialScale = H2 * (1.5 * dot2 * dot2 - 0.5);
|
|
1776
|
+
const tanScale = 3 * L2 * dot2;
|
|
1777
|
+
for (let k = 0; k < 3; k++) {
|
|
1778
|
+
disp[k] += factor * (radialScale * rHat[k] + tanScale * (jHat[k] - dot2 * rHat[k]));
|
|
1779
|
+
}
|
|
1780
|
+
}
|
|
1781
|
+
return disp;
|
|
1782
|
+
}
|
|
1783
|
+
|
|
1784
|
+
// src/positioning/ppp-antenna.ts
|
|
1785
|
+
function parseAntexTime(s) {
|
|
1786
|
+
const p = s.trim().split(/\s+/).map(Number);
|
|
1787
|
+
if (p.length < 3 || p.some((v) => Number.isNaN(v))) return null;
|
|
1788
|
+
return Date.UTC(
|
|
1789
|
+
p[0],
|
|
1790
|
+
(p[1] || 1) - 1,
|
|
1791
|
+
p[2] || 1,
|
|
1792
|
+
p[3] ?? 0,
|
|
1793
|
+
p[4] ?? 0,
|
|
1794
|
+
Math.floor(p[5] ?? 0)
|
|
1795
|
+
);
|
|
1796
|
+
}
|
|
1797
|
+
function freqOf(entry, code) {
|
|
1798
|
+
return entry.frequencies.find((f) => f.frequency === code);
|
|
1799
|
+
}
|
|
1800
|
+
function combineIf(fA, fB, f1, f2) {
|
|
1801
|
+
const g = f1 * f1 / (f1 * f1 - f2 * f2);
|
|
1802
|
+
const mm = 1e-3;
|
|
1803
|
+
const pco = [
|
|
1804
|
+
(g * fA.pcoN - (g - 1) * fB.pcoN) * mm,
|
|
1805
|
+
(g * fA.pcoE - (g - 1) * fB.pcoE) * mm,
|
|
1806
|
+
(g * fA.pcoU - (g - 1) * fB.pcoU) * mm
|
|
1807
|
+
];
|
|
1808
|
+
const nz = Math.min(fA.pcvNoazi.length, fB.pcvNoazi.length);
|
|
1809
|
+
const pcvNoazi = new Array(nz);
|
|
1810
|
+
for (let i = 0; i < nz; i++) {
|
|
1811
|
+
pcvNoazi[i] = (g * fA.pcvNoazi[i] - (g - 1) * fB.pcvNoazi[i]) * mm;
|
|
1812
|
+
}
|
|
1813
|
+
return { pco, pcvZen1Deg: 0, pcvDzenDeg: 0, pcvNoazi };
|
|
1814
|
+
}
|
|
1815
|
+
function buildPppAntenna(antex) {
|
|
1816
|
+
const sats = /* @__PURE__ */ new Map();
|
|
1817
|
+
const rcvs = /* @__PURE__ */ new Map();
|
|
1818
|
+
for (const a of antex.antennas) {
|
|
1819
|
+
if (a.isSatellite) {
|
|
1820
|
+
const list = sats.get(a.serialNo) ?? [];
|
|
1821
|
+
list.push(a);
|
|
1822
|
+
sats.set(a.serialNo, list);
|
|
1823
|
+
} else if (!rcvs.has(a.type)) {
|
|
1824
|
+
rcvs.set(a.type, a);
|
|
1825
|
+
}
|
|
1826
|
+
}
|
|
1827
|
+
const withZen = (entry, off) => ({
|
|
1828
|
+
...off,
|
|
1829
|
+
pcvZen1Deg: entry.zen1,
|
|
1830
|
+
pcvDzenDeg: entry.dzen
|
|
1831
|
+
});
|
|
1832
|
+
return {
|
|
1833
|
+
satOffset(prn, dateMs, band1, band2, f1, f2) {
|
|
1834
|
+
const list = sats.get(prn);
|
|
1835
|
+
if (!list) return null;
|
|
1836
|
+
const entry = list.find((e) => {
|
|
1837
|
+
const from = parseAntexTime(e.validFrom);
|
|
1838
|
+
const until = parseAntexTime(e.validUntil);
|
|
1839
|
+
return (from == null || dateMs >= from) && (until == null || dateMs <= until);
|
|
1840
|
+
});
|
|
1841
|
+
if (!entry) return null;
|
|
1842
|
+
const a = freqOf(entry, band1);
|
|
1843
|
+
const b = freqOf(entry, band2);
|
|
1844
|
+
if (!a || !b) return null;
|
|
1845
|
+
return withZen(entry, combineIf(a, b, f1, f2));
|
|
1846
|
+
},
|
|
1847
|
+
rcvOffset(antType, band1, band2, f1, f2) {
|
|
1848
|
+
const entry = rcvs.get(antType);
|
|
1849
|
+
if (!entry) return null;
|
|
1850
|
+
const a = freqOf(entry, band1);
|
|
1851
|
+
const b = freqOf(entry, band2);
|
|
1852
|
+
if (!a || !b) return null;
|
|
1853
|
+
return withZen(entry, combineIf(a, b, f1, f2));
|
|
1854
|
+
}
|
|
1855
|
+
};
|
|
1856
|
+
}
|
|
1857
|
+
function cross(a, b) {
|
|
1858
|
+
return [
|
|
1859
|
+
a[1] * b[2] - a[2] * b[1],
|
|
1860
|
+
a[2] * b[0] - a[0] * b[2],
|
|
1861
|
+
a[0] * b[1] - a[1] * b[0]
|
|
1862
|
+
];
|
|
1863
|
+
}
|
|
1864
|
+
function norm(a) {
|
|
1865
|
+
const n = Math.hypot(a[0], a[1], a[2]) || 1;
|
|
1866
|
+
return [a[0] / n, a[1] / n, a[2] / n];
|
|
1867
|
+
}
|
|
1868
|
+
function dot(a, b) {
|
|
1869
|
+
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
|
|
1870
|
+
}
|
|
1871
|
+
function satBodyFrame(satEcef, sunEcef2) {
|
|
1872
|
+
const z = norm([-satEcef[0], -satEcef[1], -satEcef[2]]);
|
|
1873
|
+
const eSun = norm([
|
|
1874
|
+
sunEcef2[0] - satEcef[0],
|
|
1875
|
+
sunEcef2[1] - satEcef[1],
|
|
1876
|
+
sunEcef2[2] - satEcef[2]
|
|
1877
|
+
]);
|
|
1878
|
+
const y = norm(cross(z, eSun));
|
|
1879
|
+
const x = norm(cross(y, z));
|
|
1880
|
+
return { x, y, z };
|
|
1881
|
+
}
|
|
1882
|
+
function satPcoToEcef(pcoBody, frame) {
|
|
1883
|
+
return [
|
|
1884
|
+
pcoBody[0] * frame.x[0] + pcoBody[1] * frame.y[0] + pcoBody[2] * frame.z[0],
|
|
1885
|
+
pcoBody[0] * frame.x[1] + pcoBody[1] * frame.y[1] + pcoBody[2] * frame.z[1],
|
|
1886
|
+
pcoBody[0] * frame.x[2] + pcoBody[1] * frame.y[2] + pcoBody[2] * frame.z[2]
|
|
1887
|
+
];
|
|
1888
|
+
}
|
|
1889
|
+
function enuToEcef(enu, latRad, lonRad) {
|
|
1890
|
+
const sl = Math.sin(latRad);
|
|
1891
|
+
const cl = Math.cos(latRad);
|
|
1892
|
+
const so = Math.sin(lonRad);
|
|
1893
|
+
const co = Math.cos(lonRad);
|
|
1894
|
+
const [e, n, u] = enu;
|
|
1895
|
+
return [
|
|
1896
|
+
-so * e - sl * co * n + cl * co * u,
|
|
1897
|
+
co * e - sl * so * n + cl * so * u,
|
|
1898
|
+
cl * n + sl * u
|
|
1899
|
+
];
|
|
1900
|
+
}
|
|
1901
|
+
function interpPcv(off, angleDeg) {
|
|
1902
|
+
const { pcvZen1Deg, pcvDzenDeg, pcvNoazi } = off;
|
|
1903
|
+
if (pcvDzenDeg <= 0 || pcvNoazi.length < 2) return 0;
|
|
1904
|
+
const x = (angleDeg - pcvZen1Deg) / pcvDzenDeg;
|
|
1905
|
+
if (x <= 0) return pcvNoazi[0];
|
|
1906
|
+
if (x >= pcvNoazi.length - 1) return pcvNoazi[pcvNoazi.length - 1];
|
|
1907
|
+
const i = Math.floor(x);
|
|
1908
|
+
const t = x - i;
|
|
1909
|
+
return pcvNoazi[i] * (1 - t) + pcvNoazi[i + 1] * t;
|
|
1910
|
+
}
|
|
1911
|
+
function phaseWindup(prn, losRcvToSat, satFrame, latRad, lonRad, state) {
|
|
1912
|
+
const k = [
|
|
1913
|
+
-losRcvToSat[0],
|
|
1914
|
+
-losRcvToSat[1],
|
|
1915
|
+
-losRcvToSat[2]
|
|
1916
|
+
];
|
|
1917
|
+
const kx = satFrame.x;
|
|
1918
|
+
const ky = satFrame.y;
|
|
1919
|
+
const kdotx = dot(k, kx);
|
|
1920
|
+
const kcrossY = cross(k, ky);
|
|
1921
|
+
const dS = [
|
|
1922
|
+
kx[0] - k[0] * kdotx - kcrossY[0],
|
|
1923
|
+
kx[1] - k[1] * kdotx - kcrossY[1],
|
|
1924
|
+
kx[2] - k[2] * kdotx - kcrossY[2]
|
|
1925
|
+
];
|
|
1926
|
+
const north = enuToEcef([0, 1, 0], latRad, lonRad);
|
|
1927
|
+
const east = enuToEcef([1, 0, 0], latRad, lonRad);
|
|
1928
|
+
const kdotn = dot(k, north);
|
|
1929
|
+
const kcrossE = cross(k, east);
|
|
1930
|
+
const dR = [
|
|
1931
|
+
north[0] - k[0] * kdotn + kcrossE[0],
|
|
1932
|
+
north[1] - k[1] * kdotn + kcrossE[1],
|
|
1933
|
+
north[2] - k[2] * kdotn + kcrossE[2]
|
|
1934
|
+
];
|
|
1935
|
+
const nS = Math.hypot(dS[0], dS[1], dS[2]) || 1;
|
|
1936
|
+
const nR = Math.hypot(dR[0], dR[1], dR[2]) || 1;
|
|
1937
|
+
let cosPhi = dot(dS, dR) / (nS * nR);
|
|
1938
|
+
cosPhi = Math.max(-1, Math.min(1, cosPhi));
|
|
1939
|
+
const zeta = cross(dS, dR);
|
|
1940
|
+
const sign = dot(k, zeta) >= 0 ? 1 : -1;
|
|
1941
|
+
const dphi = sign * Math.acos(cosPhi) / (2 * Math.PI);
|
|
1942
|
+
const prev = state.get(prn);
|
|
1943
|
+
const wu = prev == null ? dphi : dphi + Math.round(prev - dphi);
|
|
1944
|
+
state.set(prn, wu);
|
|
1945
|
+
return wu;
|
|
1946
|
+
}
|
|
1947
|
+
|
|
1948
|
+
// src/positioning/ppp-corrections.ts
|
|
1949
|
+
function applyCorrections(corrections, geom, ctx) {
|
|
1950
|
+
if (corrections.evaluate) return corrections.evaluate(geom, ctx);
|
|
1951
|
+
return { rangeM: 0, phaseWindupM: 0 };
|
|
1952
|
+
}
|
|
1953
|
+
function createPppCorrections(cfg) {
|
|
1954
|
+
const useSat = cfg.satPco ?? false;
|
|
1955
|
+
const useRcv = cfg.rcvPco ?? true;
|
|
1956
|
+
const useTides = cfg.tides ?? true;
|
|
1957
|
+
const useWindup = cfg.windup ?? true;
|
|
1958
|
+
const antDelta = cfg.antDeltaEnu ?? [0, 0, 0];
|
|
1959
|
+
const windupState = /* @__PURE__ */ new Map();
|
|
1960
|
+
return {
|
|
1961
|
+
epochContext(timeMs, rcvEcef) {
|
|
1962
|
+
const sun = sunEcef(timeMs);
|
|
1963
|
+
const moon = moonEcef(timeMs);
|
|
1964
|
+
const tideDisp = useTides ? solidEarthTide(rcvEcef, sun, moon) : [0, 0, 0];
|
|
1965
|
+
return { timeMs, sun, moon, tideDisp };
|
|
1966
|
+
},
|
|
1967
|
+
evaluate(geom, ctx) {
|
|
1968
|
+
let rangeM = 0;
|
|
1969
|
+
let phaseWindupM = 0;
|
|
1970
|
+
const e = geom.los;
|
|
1971
|
+
const [latDeg, lonDeg] = ecefToGeodetic(
|
|
1972
|
+
geom.rcvEcef[0],
|
|
1973
|
+
geom.rcvEcef[1],
|
|
1974
|
+
geom.rcvEcef[2]
|
|
1975
|
+
);
|
|
1976
|
+
const latRad = latDeg * Math.PI / 180;
|
|
1977
|
+
const lonRad = lonDeg * Math.PI / 180;
|
|
1978
|
+
const frame = ctx ? satBodyFrame(geom.satEcef, ctx.sun) : null;
|
|
1979
|
+
if (useSat && frame && geom.band1 && geom.band2) {
|
|
1980
|
+
const off = cfg.antenna.satOffset(
|
|
1981
|
+
geom.prn,
|
|
1982
|
+
ctx.timeMs,
|
|
1983
|
+
geom.band1,
|
|
1984
|
+
geom.band2,
|
|
1985
|
+
geom.f1,
|
|
1986
|
+
geom.f2
|
|
1987
|
+
);
|
|
1988
|
+
if (off) {
|
|
1989
|
+
const pcoEcef = satPcoToEcef(off.pco, frame);
|
|
1990
|
+
rangeM += dot(e, pcoEcef);
|
|
1991
|
+
const satR = Math.hypot(
|
|
1992
|
+
geom.satEcef[0],
|
|
1993
|
+
geom.satEcef[1],
|
|
1994
|
+
geom.satEcef[2]
|
|
1995
|
+
);
|
|
1996
|
+
const cosNadir = (e[0] * geom.satEcef[0] + e[1] * geom.satEcef[1] + e[2] * geom.satEcef[2]) / satR;
|
|
1997
|
+
const nadirDeg = Math.acos(Math.max(-1, Math.min(1, cosNadir))) * 180 / Math.PI;
|
|
1998
|
+
rangeM += interpPcv(off, nadirDeg);
|
|
1999
|
+
}
|
|
2000
|
+
}
|
|
2001
|
+
if (useRcv && geom.band1 && geom.band2) {
|
|
2002
|
+
const off = cfg.antenna.rcvOffset(
|
|
2003
|
+
cfg.rcvAntType,
|
|
2004
|
+
geom.band1,
|
|
2005
|
+
geom.band2,
|
|
2006
|
+
geom.f1,
|
|
2007
|
+
geom.f2
|
|
2008
|
+
);
|
|
2009
|
+
if (off) {
|
|
2010
|
+
const total = [
|
|
2011
|
+
off.pco[0] + antDelta[0],
|
|
2012
|
+
off.pco[1] + antDelta[1],
|
|
2013
|
+
off.pco[2] + antDelta[2]
|
|
2014
|
+
];
|
|
2015
|
+
const ecef = enuToEcef(total, latRad, lonRad);
|
|
2016
|
+
rangeM -= dot(e, ecef);
|
|
2017
|
+
const zenithDeg = 90 - geom.elRad * 180 / Math.PI;
|
|
2018
|
+
rangeM += interpPcv(off, zenithDeg);
|
|
2019
|
+
}
|
|
2020
|
+
}
|
|
2021
|
+
if (useTides && ctx) {
|
|
2022
|
+
rangeM -= dot(e, ctx.tideDisp);
|
|
2023
|
+
}
|
|
2024
|
+
if (useWindup && frame) {
|
|
2025
|
+
const wu = phaseWindup(geom.prn, e, frame, latRad, lonRad, windupState);
|
|
2026
|
+
const lambdaIf = 299792458 / (geom.f1 + geom.f2);
|
|
2027
|
+
phaseWindupM = wu * lambdaIf;
|
|
2028
|
+
}
|
|
2029
|
+
return { rangeM, phaseWindupM };
|
|
2030
|
+
}
|
|
2031
|
+
};
|
|
2032
|
+
}
|
|
2033
|
+
|
|
2034
|
+
// src/positioning/ppp.ts
|
|
2035
|
+
function matVec(P, h) {
|
|
2036
|
+
const n = P.length;
|
|
2037
|
+
const out = new Array(n).fill(0);
|
|
2038
|
+
for (let i = 0; i < n; i++) {
|
|
2039
|
+
let s = 0;
|
|
2040
|
+
const Pi = P[i];
|
|
2041
|
+
for (let j = 0; j < n; j++) s += Pi[j] * h[j];
|
|
2042
|
+
out[i] = s;
|
|
2043
|
+
}
|
|
2044
|
+
return out;
|
|
2045
|
+
}
|
|
2046
|
+
function ekfUpdateScalar(x, P, h, innovation, r, rejectK = 0) {
|
|
2047
|
+
const n = x.length;
|
|
2048
|
+
const Ph = matVec(P, h);
|
|
2049
|
+
let hPh = 0;
|
|
2050
|
+
for (let i = 0; i < n; i++) hPh += h[i] * Ph[i];
|
|
2051
|
+
const s = hPh + r;
|
|
2052
|
+
if (s <= 0) return false;
|
|
2053
|
+
if (rejectK > 0 && innovation * innovation > rejectK * rejectK * s) {
|
|
2054
|
+
return false;
|
|
2055
|
+
}
|
|
2056
|
+
const K = Ph.map((v) => v / s);
|
|
2057
|
+
for (let i = 0; i < n; i++) x[i] += K[i] * innovation;
|
|
2058
|
+
for (let i = 0; i < n; i++) {
|
|
2059
|
+
for (let j = 0; j < n; j++) {
|
|
2060
|
+
P[i][j] -= K[i] * Ph[j];
|
|
2061
|
+
}
|
|
2062
|
+
}
|
|
2063
|
+
for (let i = 0; i < n; i++) {
|
|
2064
|
+
for (let j = i + 1; j < n; j++) {
|
|
2065
|
+
const m = 0.5 * (P[i][j] + P[j][i]);
|
|
2066
|
+
P[i][j] = m;
|
|
2067
|
+
P[j][i] = m;
|
|
2068
|
+
}
|
|
2069
|
+
}
|
|
2070
|
+
return true;
|
|
2071
|
+
}
|
|
2072
|
+
function ifCoeffs(f1, f2) {
|
|
2073
|
+
const g = f1 * f1 / (f1 * f1 - f2 * f2);
|
|
2074
|
+
const lambdaIf = C_LIGHT / (f1 + f2);
|
|
2075
|
+
return { g, lambdaIf };
|
|
2076
|
+
}
|
|
2077
|
+
function sagnacRotate(x, y, z, travelS) {
|
|
2078
|
+
const theta = OMEGA_E * travelS;
|
|
2079
|
+
const c = Math.cos(theta);
|
|
2080
|
+
const s = Math.sin(theta);
|
|
2081
|
+
return [x * c + y * s, -x * s + y * c, z];
|
|
2082
|
+
}
|
|
2083
|
+
function satStateAtEmission(sp3, prn, recvTimeMs, rcv) {
|
|
2084
|
+
let travelS = 0.075;
|
|
2085
|
+
let sx = 0;
|
|
2086
|
+
let sy = 0;
|
|
2087
|
+
let sz = 0;
|
|
2088
|
+
let clkS = null;
|
|
2089
|
+
let velDotPos = 0;
|
|
2090
|
+
for (let iter = 0; iter < 3; iter++) {
|
|
2091
|
+
const tEmit = recvTimeMs - travelS * 1e3;
|
|
2092
|
+
const p = sp3Position(sp3, prn, tEmit);
|
|
2093
|
+
if (!p || p.clk == null) return null;
|
|
2094
|
+
const pPlus = sp3Position(sp3, prn, tEmit + 500);
|
|
2095
|
+
const pMinus = sp3Position(sp3, prn, tEmit - 500);
|
|
2096
|
+
if (pPlus && pMinus) {
|
|
2097
|
+
const vx = (pPlus.x - pMinus.x) / 1;
|
|
2098
|
+
const vy = (pPlus.y - pMinus.y) / 1;
|
|
2099
|
+
const vz = (pPlus.z - pMinus.z) / 1;
|
|
2100
|
+
velDotPos = p.x * vx + p.y * vy + p.z * vz;
|
|
2101
|
+
}
|
|
2102
|
+
clkS = p.clk;
|
|
2103
|
+
const [rx, ry, rz] = sagnacRotate(p.x, p.y, p.z, travelS);
|
|
2104
|
+
sx = rx;
|
|
2105
|
+
sy = ry;
|
|
2106
|
+
sz = rz;
|
|
2107
|
+
const dx = sx - rcv[0];
|
|
2108
|
+
const dy = sy - rcv[1];
|
|
2109
|
+
const dz = sz - rcv[2];
|
|
2110
|
+
const range = Math.hypot(dx, dy, dz);
|
|
2111
|
+
travelS = range / C_LIGHT;
|
|
2112
|
+
}
|
|
2113
|
+
if (clkS == null) return null;
|
|
2114
|
+
const relS = -2 * velDotPos / (C_LIGHT * C_LIGHT);
|
|
2115
|
+
const clkM = C_LIGHT * (clkS + relS);
|
|
2116
|
+
return { state: { x: sx, y: sy, z: sz, clkM }, travelS };
|
|
2117
|
+
}
|
|
2118
|
+
var IDX_X = 0;
|
|
2119
|
+
var IDX_Y = 1;
|
|
2120
|
+
var IDX_Z = 2;
|
|
2121
|
+
var IDX_ZWD = 3;
|
|
2122
|
+
var NBASE = 4;
|
|
2123
|
+
var CLK_VAR = 1e10;
|
|
2124
|
+
function solvePpp(epochs, sp3, opts) {
|
|
2125
|
+
const elevMask = (opts.elevationMaskDeg ?? 10) * Math.PI / 180;
|
|
2126
|
+
const codeSigma = opts.codeSigma ?? 3;
|
|
2127
|
+
const phaseSigma = opts.phaseSigma ?? 0.01;
|
|
2128
|
+
const ztdQ = opts.ztdProcessNoise ?? 1e-8;
|
|
2129
|
+
const corrections = opts.corrections;
|
|
2130
|
+
const x = [
|
|
2131
|
+
opts.aprioriPos[0],
|
|
2132
|
+
opts.aprioriPos[1],
|
|
2133
|
+
opts.aprioriPos[2],
|
|
2134
|
+
0.1
|
|
2135
|
+
// zenith wet delay (m) a priori
|
|
2136
|
+
];
|
|
2137
|
+
const P = [];
|
|
2138
|
+
for (let i = 0; i < NBASE; i++) {
|
|
2139
|
+
P.push(new Array(NBASE).fill(0));
|
|
2140
|
+
}
|
|
2141
|
+
P[IDX_X][IDX_X] = 100 * 100;
|
|
2142
|
+
P[IDX_Y][IDX_Y] = 100 * 100;
|
|
2143
|
+
P[IDX_Z][IDX_Z] = 100 * 100;
|
|
2144
|
+
P[IDX_ZWD][IDX_ZWD] = 0.5 * 0.5;
|
|
2145
|
+
const clkIdx = /* @__PURE__ */ new Map();
|
|
2146
|
+
const clockSeeded = /* @__PURE__ */ new Set();
|
|
2147
|
+
const ambIdx = /* @__PURE__ */ new Map();
|
|
2148
|
+
const lastSeen = /* @__PURE__ */ new Map();
|
|
2149
|
+
const mwState = /* @__PURE__ */ new Map();
|
|
2150
|
+
const detectSlip = (o, ei) => {
|
|
2151
|
+
const lamW = C_LIGHT / (o.f1 - o.f2);
|
|
2152
|
+
const lw = o.l1 - o.l2;
|
|
2153
|
+
const pn = (o.f1 * o.c1 + o.f2 * o.c2) / (o.f1 + o.f2);
|
|
2154
|
+
const mw = lw - pn / lamW;
|
|
2155
|
+
const st = mwState.get(o.prn);
|
|
2156
|
+
let slip = o.slip;
|
|
2157
|
+
if (!st || ei - st.lastEi > 2)
|
|
2158
|
+
slip = true;
|
|
2159
|
+
else if (Math.abs(mw - st.mean) > 4) slip = true;
|
|
2160
|
+
if (slip) mwState.set(o.prn, { mean: mw, n: 1, lastEi: ei });
|
|
2161
|
+
else {
|
|
2162
|
+
const n = st.n + 1;
|
|
2163
|
+
mwState.set(o.prn, {
|
|
2164
|
+
mean: st.mean + (mw - st.mean) / Math.min(n, 100),
|
|
2165
|
+
n,
|
|
2166
|
+
lastEi: ei
|
|
2167
|
+
});
|
|
2168
|
+
}
|
|
2169
|
+
return slip;
|
|
2170
|
+
};
|
|
2171
|
+
const growState = (initVal, initVar) => {
|
|
2172
|
+
const idx = x.length;
|
|
2173
|
+
x.push(initVal);
|
|
2174
|
+
for (const row of P) row.push(0);
|
|
2175
|
+
P.push(new Array(x.length).fill(0));
|
|
2176
|
+
P[idx][idx] = initVar;
|
|
2177
|
+
return idx;
|
|
2178
|
+
};
|
|
2179
|
+
const dropState = (idx) => {
|
|
2180
|
+
x.splice(idx, 1);
|
|
2181
|
+
P.splice(idx, 1);
|
|
2182
|
+
for (const row of P) row.splice(idx, 1);
|
|
2183
|
+
for (const [prn, i] of ambIdx) {
|
|
2184
|
+
if (i === idx) ambIdx.delete(prn);
|
|
2185
|
+
else if (i > idx) ambIdx.set(prn, i - 1);
|
|
2186
|
+
}
|
|
2187
|
+
for (const [sys, i] of clkIdx) {
|
|
2188
|
+
if (i > idx) clkIdx.set(sys, i - 1);
|
|
2189
|
+
}
|
|
2190
|
+
};
|
|
2191
|
+
const clockOf = (sys) => {
|
|
2192
|
+
let i = clkIdx.get(sys);
|
|
2193
|
+
if (i === void 0) {
|
|
2194
|
+
i = growState(0, CLK_VAR);
|
|
2195
|
+
clkIdx.set(sys, i);
|
|
2196
|
+
}
|
|
2197
|
+
return i;
|
|
2198
|
+
};
|
|
2199
|
+
const series = [];
|
|
2200
|
+
const truth = opts.groundTruth;
|
|
2201
|
+
const t0 = epochs[0]?.timeMs ?? 0;
|
|
2202
|
+
let convergenceSec = null;
|
|
2203
|
+
let convergedStreak = 0;
|
|
2204
|
+
for (let ei = 0; ei < epochs.length; ei++) {
|
|
2205
|
+
const epoch = epochs[ei];
|
|
2206
|
+
for (const ci of clkIdx.values()) P[ci][ci] = CLK_VAR;
|
|
2207
|
+
P[IDX_ZWD][IDX_ZWD] += ztdQ * (ei > 0 ? 1 : 0);
|
|
2208
|
+
const rcv = [x[IDX_X], x[IDX_Y], x[IDX_Z]];
|
|
2209
|
+
const [latDeg, , heightM] = ecefToGeodetic(rcv[0], rcv[1], rcv[2]);
|
|
2210
|
+
const latRad = latDeg * Math.PI / 180;
|
|
2211
|
+
const epochCtx = corrections?.epochContext?.(epoch.timeMs, rcv) ?? null;
|
|
2212
|
+
const vis = [];
|
|
2213
|
+
for (const o of epoch.obs) {
|
|
2214
|
+
if (o.c1 === 0 || o.c2 === 0 || o.l1 === 0 || o.l2 === 0) continue;
|
|
2215
|
+
const slip = detectSlip(o, ei);
|
|
2216
|
+
const sat = satStateAtEmission(sp3, o.prn, epoch.timeMs, rcv);
|
|
2217
|
+
if (!sat) continue;
|
|
2218
|
+
const [elRad, azRad] = getAer(
|
|
2219
|
+
sat.state.x,
|
|
2220
|
+
sat.state.y,
|
|
2221
|
+
sat.state.z,
|
|
2222
|
+
rcv[0],
|
|
2223
|
+
rcv[1],
|
|
2224
|
+
rcv[2]
|
|
2225
|
+
);
|
|
2226
|
+
if (elRad < elevMask) continue;
|
|
2227
|
+
const dx = sat.state.x - rcv[0];
|
|
2228
|
+
const dy = sat.state.y - rcv[1];
|
|
2229
|
+
const dz = sat.state.z - rcv[2];
|
|
2230
|
+
const range = Math.hypot(dx, dy, dz);
|
|
2231
|
+
const e = [dx / range, dy / range, dz / range];
|
|
2232
|
+
const { g } = ifCoeffs(o.f1, o.f2);
|
|
2233
|
+
const lam1 = C_LIGHT / o.f1;
|
|
2234
|
+
const lam2 = C_LIGHT / o.f2;
|
|
2235
|
+
if (o.c1 === 0 || o.c2 === 0 || o.l1 === 0 || o.l2 === 0) continue;
|
|
2236
|
+
const pIf = g * o.c1 - (g - 1) * o.c2;
|
|
2237
|
+
const lIf = g * (o.l1 * lam1) - (g - 1) * (o.l2 * lam2);
|
|
2238
|
+
if (!Number.isFinite(pIf) || !Number.isFinite(lIf)) continue;
|
|
2239
|
+
const trop = opts.troposphere ?? true ? niellMapping(elRad, latRad, heightM, epoch.timeMs) : { zhd: 0, mh: 0, mw: 0 };
|
|
2240
|
+
const { zhd, mh, mw } = trop;
|
|
2241
|
+
const geom = {
|
|
2242
|
+
prn: o.prn,
|
|
2243
|
+
satEcef: [sat.state.x, sat.state.y, sat.state.z],
|
|
2244
|
+
rcvEcef: rcv,
|
|
2245
|
+
los: e,
|
|
2246
|
+
elRad,
|
|
2247
|
+
azRad,
|
|
2248
|
+
f1: o.f1,
|
|
2249
|
+
f2: o.f2,
|
|
2250
|
+
band1: o.band1,
|
|
2251
|
+
band2: o.band2,
|
|
2252
|
+
g
|
|
2253
|
+
};
|
|
2254
|
+
const corr = corrections ? applyCorrections(corrections, geom, epochCtx) : { rangeM: 0, phaseWindupM: 0 };
|
|
2255
|
+
vis.push({
|
|
2256
|
+
prn: o.prn,
|
|
2257
|
+
e,
|
|
2258
|
+
elRad,
|
|
2259
|
+
pIf,
|
|
2260
|
+
lIf,
|
|
2261
|
+
satClkM: sat.state.clkM,
|
|
2262
|
+
range,
|
|
2263
|
+
tropoFixed: zhd * mh + corr.rangeM,
|
|
2264
|
+
mw,
|
|
2265
|
+
windupM: corr.phaseWindupM,
|
|
2266
|
+
slip
|
|
2267
|
+
});
|
|
2268
|
+
}
|
|
2269
|
+
const zwd0 = x[IDX_ZWD];
|
|
2270
|
+
const bySys = /* @__PURE__ */ new Map();
|
|
2271
|
+
for (const v of vis) {
|
|
2272
|
+
const sys = v.prn[0];
|
|
2273
|
+
const r = v.pIf - (v.range - v.satClkM + v.tropoFixed + zwd0 * v.mw);
|
|
2274
|
+
const arr = bySys.get(sys);
|
|
2275
|
+
if (arr) arr.push(r);
|
|
2276
|
+
else bySys.set(sys, [r]);
|
|
2277
|
+
}
|
|
2278
|
+
for (const [sys, res] of bySys) {
|
|
2279
|
+
const ci = clockOf(sys);
|
|
2280
|
+
if (!clockSeeded.has(sys)) {
|
|
2281
|
+
res.sort((a, b) => a - b);
|
|
2282
|
+
x[ci] = res[Math.floor(res.length / 2)];
|
|
2283
|
+
clockSeeded.add(sys);
|
|
2284
|
+
}
|
|
2285
|
+
P[ci][ci] = CLK_VAR;
|
|
2286
|
+
}
|
|
2287
|
+
let nSats = 0;
|
|
2288
|
+
let sumPhaseResSq = 0;
|
|
2289
|
+
let nPhaseRes = 0;
|
|
2290
|
+
for (const v of vis) {
|
|
2291
|
+
const ci = clockOf(v.prn[0]);
|
|
2292
|
+
const common = v.range + x[ci] - v.satClkM + v.tropoFixed + zwd0 * v.mw;
|
|
2293
|
+
let ai = ambIdx.get(v.prn);
|
|
2294
|
+
if (ai === void 0 || v.slip) {
|
|
2295
|
+
const bInit = v.lIf - (common + v.windupM);
|
|
2296
|
+
if (ai === void 0) {
|
|
2297
|
+
ai = growState(bInit, 100 * 100);
|
|
2298
|
+
} else {
|
|
2299
|
+
x[ai] = bInit;
|
|
2300
|
+
for (let j = 0; j < P.length; j++) {
|
|
2301
|
+
P[ai][j] = 0;
|
|
2302
|
+
P[j][ai] = 0;
|
|
2303
|
+
}
|
|
2304
|
+
P[ai][ai] = 100 * 100;
|
|
2305
|
+
}
|
|
2306
|
+
ambIdx.set(v.prn, ai);
|
|
2307
|
+
}
|
|
2308
|
+
const n = x.length;
|
|
2309
|
+
const sinEl = Math.max(Math.sin(v.elRad), 0.1);
|
|
2310
|
+
const rCode = (codeSigma / sinEl) ** 2;
|
|
2311
|
+
const rPhase = (phaseSigma / sinEl) ** 2;
|
|
2312
|
+
{
|
|
2313
|
+
const h = new Array(n).fill(0);
|
|
2314
|
+
h[IDX_X] = -v.e[0];
|
|
2315
|
+
h[IDX_Y] = -v.e[1];
|
|
2316
|
+
h[IDX_Z] = -v.e[2];
|
|
2317
|
+
h[ci] = 1;
|
|
2318
|
+
h[IDX_ZWD] = v.mw;
|
|
2319
|
+
const innov = v.pIf - common;
|
|
2320
|
+
if (Math.abs(innov) < 1e6) ekfUpdateScalar(x, P, h, innov, rCode, 4);
|
|
2321
|
+
}
|
|
2322
|
+
{
|
|
2323
|
+
const commonPh = v.range + x[ci] - v.satClkM + v.tropoFixed + x[IDX_ZWD] * v.mw;
|
|
2324
|
+
const innov = v.lIf - (commonPh + v.windupM + x[ai]);
|
|
2325
|
+
if (Math.abs(innov) > 0.15) {
|
|
2326
|
+
x[ai] = v.lIf - (commonPh + v.windupM);
|
|
2327
|
+
for (let j = 0; j < P.length; j++) {
|
|
2328
|
+
P[ai][j] = 0;
|
|
2329
|
+
P[j][ai] = 0;
|
|
2330
|
+
}
|
|
2331
|
+
P[ai][ai] = 100 * 100;
|
|
2332
|
+
} else {
|
|
2333
|
+
const h = new Array(n).fill(0);
|
|
2334
|
+
h[IDX_X] = -v.e[0];
|
|
2335
|
+
h[IDX_Y] = -v.e[1];
|
|
2336
|
+
h[IDX_Z] = -v.e[2];
|
|
2337
|
+
h[ci] = 1;
|
|
2338
|
+
h[IDX_ZWD] = v.mw;
|
|
2339
|
+
h[ai] = 1;
|
|
2340
|
+
if (ekfUpdateScalar(x, P, h, innov, rPhase, 4)) {
|
|
2341
|
+
sumPhaseResSq += innov * innov;
|
|
2342
|
+
nPhaseRes++;
|
|
2343
|
+
}
|
|
2344
|
+
}
|
|
2345
|
+
}
|
|
2346
|
+
lastSeen.set(v.prn, ei);
|
|
2347
|
+
nSats++;
|
|
2348
|
+
}
|
|
2349
|
+
for (const [prn, seen] of [...lastSeen]) {
|
|
2350
|
+
if (ei - seen > 20) {
|
|
2351
|
+
const idx = ambIdx.get(prn);
|
|
2352
|
+
if (idx !== void 0) dropState(idx);
|
|
2353
|
+
lastSeen.delete(prn);
|
|
2354
|
+
}
|
|
2355
|
+
}
|
|
2356
|
+
const pos = [x[IDX_X], x[IDX_Y], x[IDX_Z]];
|
|
2357
|
+
let enu = null;
|
|
2358
|
+
let error3d = null;
|
|
2359
|
+
if (truth) {
|
|
2360
|
+
enu = getEnuDifference(
|
|
2361
|
+
pos[0],
|
|
2362
|
+
pos[1],
|
|
2363
|
+
pos[2],
|
|
2364
|
+
truth[0],
|
|
2365
|
+
truth[1],
|
|
2366
|
+
truth[2]
|
|
2367
|
+
);
|
|
2368
|
+
error3d = Math.hypot(
|
|
2369
|
+
pos[0] - truth[0],
|
|
2370
|
+
pos[1] - truth[1],
|
|
2371
|
+
pos[2] - truth[2]
|
|
2372
|
+
);
|
|
2373
|
+
if (error3d < 0.1) {
|
|
2374
|
+
convergedStreak++;
|
|
2375
|
+
if (convergedStreak >= 10 && convergenceSec == null) {
|
|
2376
|
+
convergenceSec = (epoch.timeMs - t0) / 1e3;
|
|
2377
|
+
}
|
|
2378
|
+
} else {
|
|
2379
|
+
convergedStreak = 0;
|
|
2380
|
+
convergenceSec = null;
|
|
2381
|
+
}
|
|
2382
|
+
}
|
|
2383
|
+
series.push({
|
|
2384
|
+
timeMs: epoch.timeMs,
|
|
2385
|
+
enu,
|
|
2386
|
+
error3d,
|
|
2387
|
+
position: pos,
|
|
2388
|
+
nSats,
|
|
2389
|
+
ztdWet: x[IDX_ZWD],
|
|
2390
|
+
phaseResRms: nPhaseRes > 0 ? Math.sqrt(sumPhaseResSq / nPhaseRes) : 0
|
|
2391
|
+
});
|
|
2392
|
+
}
|
|
2393
|
+
const finalPos = [x[IDX_X], x[IDX_Y], x[IDX_Z]];
|
|
2394
|
+
const llh = ecefToGeodetic(finalPos[0], finalPos[1], finalPos[2]);
|
|
2395
|
+
const finalError3d = truth ? Math.hypot(
|
|
2396
|
+
finalPos[0] - truth[0],
|
|
2397
|
+
finalPos[1] - truth[1],
|
|
2398
|
+
finalPos[2] - truth[2]
|
|
2399
|
+
) : null;
|
|
2400
|
+
return {
|
|
2401
|
+
position: finalPos,
|
|
2402
|
+
llh,
|
|
2403
|
+
series,
|
|
2404
|
+
ztdWet: x[IDX_ZWD],
|
|
2405
|
+
epochsUsed: epochs.length,
|
|
2406
|
+
convergenceSec,
|
|
2407
|
+
finalError3d
|
|
2408
|
+
};
|
|
2409
|
+
}
|
|
2410
|
+
|
|
1521
2411
|
// src/positioning/index.ts
|
|
1522
2412
|
var GM_GPS2 = 3986005e8;
|
|
1523
2413
|
var F_REL = -4442807633e-19;
|
|
@@ -1658,7 +2548,8 @@ function solveSpp(pseudoranges, ephemerides, timeMs, opts = {}) {
|
|
|
1658
2548
|
let ionoM = 0;
|
|
1659
2549
|
if ((gim || iono) && positionSane) {
|
|
1660
2550
|
let l1M = null;
|
|
1661
|
-
if (gim)
|
|
2551
|
+
if (gim)
|
|
2552
|
+
l1M = gimSlantIonoDelayL1(gim, rxLat, rxLon, azim, elev, timeMs);
|
|
1662
2553
|
if (l1M === null && iono) {
|
|
1663
2554
|
l1M = C_LIGHT * klobucharDelay(iono, rxLat, rxLon, azim, elev, gpsTow);
|
|
1664
2555
|
}
|
|
@@ -1748,14 +2639,22 @@ function solveSpp(pseudoranges, ephemerides, timeMs, opts = {}) {
|
|
|
1748
2639
|
0 && (module.exports = {
|
|
1749
2640
|
IONO_L1_M_PER_TECU,
|
|
1750
2641
|
RtkFloatEngine,
|
|
2642
|
+
buildPppAntenna,
|
|
2643
|
+
createPppCorrections,
|
|
1751
2644
|
gimSlantIonoDelayL1,
|
|
1752
2645
|
gimVerticalTec,
|
|
2646
|
+
gmst,
|
|
1753
2647
|
ionoFree,
|
|
1754
2648
|
klobucharDelay,
|
|
1755
2649
|
lambdaReduction,
|
|
1756
2650
|
lambdaSearch,
|
|
2651
|
+
moonEcef,
|
|
2652
|
+
niellMapping,
|
|
1757
2653
|
satClockCorrection,
|
|
2654
|
+
solidEarthTide,
|
|
1758
2655
|
solveDgnss,
|
|
2656
|
+
solvePpp,
|
|
1759
2657
|
solveSpp,
|
|
2658
|
+
sunEcef,
|
|
1760
2659
|
toRtkEpoch
|
|
1761
2660
|
});
|