gnss-js 1.23.0 → 1.25.0

package/dist/positioning.cjs

@@ -20,9 +20,14 @@ var __toCommonJS = (mod) => __copyProps(__defProp({}, "__esModule", { value: tru
 // src/positioning/index.ts
 var positioning_exports = {};
 __export(positioning_exports, {
+  IONO_L1_M_PER_TECU: () => IONO_L1_M_PER_TECU,
   RtkFloatEngine: () => RtkFloatEngine,
+  gimSlantIonoDelayL1: () => gimSlantIonoDelayL1,
+  gimVerticalTec: () => gimVerticalTec,
   ionoFree: () => ionoFree,
   klobucharDelay: () => klobucharDelay,
+  lambdaReduction: () => lambdaReduction,
+  lambdaSearch: () => lambdaSearch,
   satClockCorrection: () => satClockCorrection,
   solveDgnss: () => solveDgnss,
   solveSpp: () => solveSpp,
@@ -462,6 +467,252 @@ function klobucharDelay(coeffs, latRad, lonRad, azRad, elRad, gpsTowSec) {
   return F * (5e-9 + amp * (1 - x2 / 2 + x2 * x2 / 24));
 }
 
+// src/positioning/gim.ts
+var R_E = 6371e3;
+var H_ION = 45e4;
+var F_L1 = 157542e4;
+var IONO_L1_M_PER_TECU = 403e15 / (F_L1 * F_L1);
+function tecAtMap(grid, mapIdx, latDeg, lonDeg) {
+  const m = grid.maps[mapIdx];
+  const { lats, lons } = grid;
+  const w = lons.length;
+  const fi = (latDeg - lats[0]) / (lats[lats.length - 1] - lats[0]) * (lats.length - 1);
+  const fj = (lonDeg - lons[0]) / (lons[w - 1] - lons[0]) * (w - 1);
+  const i0 = Math.max(0, Math.min(lats.length - 2, Math.floor(fi)));
+  const j0 = Math.max(0, Math.min(w - 2, Math.floor(fj)));
+  const di = Math.max(0, Math.min(1, fi - i0));
+  const dj = Math.max(0, Math.min(1, fj - j0));
+  return m[i0 * w + j0] * (1 - di) * (1 - dj) + m[i0 * w + j0 + 1] * (1 - di) * dj + m[(i0 + 1) * w + j0] * di * (1 - dj) + m[(i0 + 1) * w + j0 + 1] * di * dj;
+}
+function gimVerticalTec(grid, timeMs, latDeg, lonDeg) {
+  const e = grid.epochs;
+  if (e.length === 0 || timeMs < e[0] || timeMs > e[e.length - 1]) return null;
+  let i = 0;
+  while (i < e.length - 2 && e[i + 1] <= timeMs) i++;
+  const span = e[i + 1] - e[i];
+  const f = span > 0 ? (timeMs - e[i]) / span : 0;
+  const a = tecAtMap(grid, i, latDeg, lonDeg);
+  const b = tecAtMap(grid, i + 1, latDeg, lonDeg);
+  if (!Number.isFinite(a) || !Number.isFinite(b)) return null;
+  return a * (1 - f) + b * f;
+}
+function gimSlantIonoDelayL1(grid, latRad, lonRad, azRad, elRad, timeMs) {
+  if (elRad <= 0) return null;
+  const sinZp = R_E / (R_E + H_ION) * Math.cos(elRad);
+  const mapping = 1 / Math.sqrt(1 - sinZp * sinZp);
+  const psi = Math.PI / 2 - elRad - Math.asin(sinZp);
+  const latI = Math.asin(
+    Math.sin(latRad) * Math.cos(psi) + Math.cos(latRad) * Math.sin(psi) * Math.cos(azRad)
+  );
+  const lonI = lonRad + Math.asin(Math.sin(psi) * Math.sin(azRad) / Math.cos(latI));
+  const latDeg = latI * 180 / Math.PI;
+  const lonDeg = ((lonI * 180 / Math.PI + 540) % 360 + 360) % 360 - 180;
+  const vtec = gimVerticalTec(grid, timeMs, latDeg, lonDeg);
+  if (vtec === null) return null;
+  return IONO_L1_M_PER_TECU * vtec * mapping;
+}
+
+// src/positioning/lambda.ts
+var LOOPMAX = 1e4;
+var ROUND = (x) => Math.floor(x + 0.5);
+var SGN = (x) => x <= 0 ? -1 : 1;
+function ldFactor(n, Q, L, D) {
+  const A = Float64Array.from(Q);
+  for (let i = n - 1; i >= 0; i--) {
+    D[i] = A[i + i * n];
+    if (D[i] <= 0) return false;
+    const a = Math.sqrt(D[i]);
+    for (let j = 0; j <= i; j++) L[i + j * n] = A[i + j * n] / a;
+    for (let j = 0; j <= i - 1; j++)
+      for (let k = 0; k <= j; k++) A[j + k * n] -= L[i + k * n] * L[i + j * n];
+    for (let j = 0; j <= i; j++) L[i + j * n] /= L[i + i * n];
+  }
+  return true;
+}
+function gauss(n, L, Z, i, j) {
+  const mu = ROUND(L[i + j * n]);
+  if (mu !== 0) {
+    for (let k = i; k < n; k++) L[k + n * j] -= mu * L[k + i * n];
+    for (let k = 0; k < n; k++) Z[k + n * j] -= mu * Z[k + i * n];
+  }
+}
+function perm(n, L, D, j, del, Z) {
+  const eta = D[j] / del;
+  const lam = D[j + 1] * L[j + 1 + j * n] / del;
+  D[j] = eta * D[j + 1];
+  D[j + 1] = del;
+  for (let k = 0; k <= j - 1; k++) {
+    const a0 = L[j + k * n];
+    const a1 = L[j + 1 + k * n];
+    L[j + k * n] = -L[j + 1 + j * n] * a0 + a1;
+    L[j + 1 + k * n] = eta * a0 + lam * a1;
+  }
+  L[j + 1 + j * n] = lam;
+  for (let k = j + 2; k < n; k++) {
+    const t = L[k + j * n];
+    L[k + j * n] = L[k + (j + 1) * n];
+    L[k + (j + 1) * n] = t;
+  }
+  for (let k = 0; k < n; k++) {
+    const t = Z[k + j * n];
+    Z[k + j * n] = Z[k + (j + 1) * n];
+    Z[k + (j + 1) * n] = t;
+  }
+}
+function reduction(n, L, D, Z) {
+  let j = n - 2;
+  let k = n - 2;
+  while (j >= 0) {
+    if (j <= k) for (let i = j + 1; i < n; i++) gauss(n, L, Z, i, j);
+    const del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1];
+    if (del + 1e-6 < D[j + 1]) {
+      perm(n, L, D, j, del, Z);
+      k = j;
+      j = n - 2;
+    } else j--;
+  }
+}
+function search(n, m, L, D, zs, zn, s) {
+  let nn = 0;
+  let imax = 0;
+  let maxdist = 1e99;
+  const S = new Float64Array(n * n);
+  const dist = new Float64Array(n);
+  const zb = new Float64Array(n);
+  const z = new Float64Array(n);
+  const step = new Float64Array(n);
+  let k = n - 1;
+  dist[k] = 0;
+  zb[k] = zs[k];
+  z[k] = ROUND(zb[k]);
+  let y = zb[k] - z[k];
+  step[k] = SGN(y);
+  let c = 0;
+  for (; c < LOOPMAX; c++) {
+    const newdist = dist[k] + y * y / D[k];
+    if (newdist < maxdist) {
+      if (k !== 0) {
+        dist[--k] = newdist;
+        for (let i = 0; i <= k; i++)
+          S[k + i * n] = S[k + 1 + i * n] + (z[k + 1] - zb[k + 1]) * L[k + 1 + i * n];
+        zb[k] = zs[k] + S[k + k * n];
+        z[k] = ROUND(zb[k]);
+        y = zb[k] - z[k];
+        step[k] = SGN(y);
+      } else {
+        if (nn < m) {
+          if (nn === 0 || newdist > s[imax]) imax = nn;
+          for (let i = 0; i < n; i++) zn[i + nn * n] = z[i];
+          s[nn++] = newdist;
+        } else {
+          if (newdist < s[imax]) {
+            for (let i = 0; i < n; i++) zn[i + imax * n] = z[i];
+            s[imax] = newdist;
+            imax = 0;
+            for (let i = 0; i < m; i++) if (s[imax] < s[i]) imax = i;
+          }
+          maxdist = s[imax];
+        }
+        z[0] += step[0];
+        y = zb[0] - z[0];
+        step[0] = -step[0] - SGN(step[0]);
+      }
+    } else {
+      if (k === n - 1) break;
+      k++;
+      z[k] += step[k];
+      y = zb[k] - z[k];
+      step[k] = -step[k] - SGN(step[k]);
+    }
+  }
+  for (let i = 0; i < m - 1; i++) {
+    for (let j = i + 1; j < m; j++) {
+      if (s[i] < s[j]) continue;
+      const t = s[i];
+      s[i] = s[j];
+      s[j] = t;
+      for (let l = 0; l < n; l++) {
+        const u = zn[l + i * n];
+        zn[l + i * n] = zn[l + j * n];
+        zn[l + j * n] = u;
+      }
+    }
+  }
+  return c < LOOPMAX;
+}
+function solveZt(n, m, Z, E) {
+  const w = n + m;
+  const M = new Float64Array(n * w);
+  for (let r = 0; r < n; r++) {
+    for (let cIdx = 0; cIdx < n; cIdx++) M[r * w + cIdx] = Z[cIdx + r * n];
+    for (let cIdx = 0; cIdx < m; cIdx++) M[r * w + n + cIdx] = E[r + cIdx * n];
+  }
+  for (let col = 0; col < n; col++) {
+    let piv = col;
+    for (let r = col + 1; r < n; r++)
+      if (Math.abs(M[r * w + col]) > Math.abs(M[piv * w + col])) piv = r;
+    if (Math.abs(M[piv * w + col]) < 1e-12) return null;
+    if (piv !== col) {
+      for (let cIdx = col; cIdx < w; cIdx++) {
+        const t = M[col * w + cIdx];
+        M[col * w + cIdx] = M[piv * w + cIdx];
+        M[piv * w + cIdx] = t;
+      }
+    }
+    const d = M[col * w + col];
+    for (let cIdx = col; cIdx < w; cIdx++) M[col * w + cIdx] /= d;
+    for (let r = 0; r < n; r++) {
+      if (r === col) continue;
+      const f = M[r * w + col];
+      if (f === 0) continue;
+      for (let cIdx = col; cIdx < w; cIdx++)
+        M[r * w + cIdx] -= f * M[col * w + cIdx];
+    }
+  }
+  const X = new Float64Array(n * m);
+  for (let r = 0; r < n; r++)
+    for (let cIdx = 0; cIdx < m; cIdx++) X[r + cIdx * n] = M[r * w + n + cIdx];
+  return X;
+}
+function lambdaSearch(a, Q, n, m = 2) {
+  if (n <= 0 || m <= 0 || a.length < n || Q.length < n * n) return null;
+  const L = new Float64Array(n * n);
+  const D = new Float64Array(n);
+  if (!ldFactor(n, Q, L, D)) return null;
+  const Z = new Float64Array(n * n);
+  for (let i = 0; i < n; i++) Z[i + i * n] = 1;
+  reduction(n, L, D, Z);
+  const z = new Float64Array(n);
+  for (let j = 0; j < n; j++) {
+    let sum = 0;
+    for (let k = 0; k < n; k++) sum += Z[k + j * n] * a[k];
+    z[j] = sum;
+  }
+  const E = new Float64Array(n * m);
+  const s = new Float64Array(m);
+  if (!search(n, m, L, D, z, E, s)) return null;
+  const F = solveZt(n, m, Z, E);
+  if (!F) return null;
+  const candidates = [];
+  for (let c = 0; c < m; c++) {
+    const v = new Float64Array(n);
+    for (let i = 0; i < n; i++) v[i] = ROUND(F[i + c * n]);
+    candidates.push(v);
+  }
+  const ratio = m >= 2 && s[0] > 0 ? s[1] / s[0] : Infinity;
+  return { candidates, residuals: s, ratio };
+}
+function lambdaReduction(Q, n) {
+  if (n <= 0 || Q.length < n * n) return null;
+  const L = new Float64Array(n * n);
+  const D = new Float64Array(n);
+  if (!ldFactor(n, Q, L, D)) return null;
+  const Z = new Float64Array(n * n);
+  for (let i = 0; i < n; i++) Z[i + i * n] = 1;
+  reduction(n, L, D, Z);
+  return Z;
+}
+
 // src/positioning/rtk.ts
 var L1_CODES = {
   G: ["1C"],
@@ -822,7 +1073,13 @@ var RtkFloatEngine = class {
       maxGapMs: opts.maxGapMs ?? 1e4,
       codeGateM: opts.codeGateM ?? 30,
       slipGateM: opts.slipGateM ?? 5,
-      updateIterations: opts.updateIterations ?? 2
+      updateIterations: opts.updateIterations ?? 2,
+      ambiguityResolution: opts.ambiguityResolution ?? "off",
+      ratioThreshold: opts.ratioThreshold ?? 3,
+      arElevationMaskDeg: opts.arElevationMaskDeg ?? 0,
+      glonassAr: opts.glonassAr ?? false,
+      partialFixing: opts.partialFixing ?? true,
+      minFixAmbiguities: opts.minFixAmbiguities ?? 4
     };
   }
   /** Clear all filter state (position, ambiguities, lock history). */
@@ -880,6 +1137,56 @@ var RtkFloatEngine = class {
     const entry = this.amb[newRefIdx];
     entry.prn = oldRef;
   }
+  /**
+   * Try to fix the ambiguity subset `idxs` (indices into `this.amb`):
+   * extract the float subvector b and covariance Q_b from the filter,
+   * run MLAMBDA and, without touching the filter state, condition the
+   * position on the best integer candidate,
+   *   xa = x − P_xb·Q_b⁻¹·(b − ǎ)
+   * (RTKLIB rtkpos.c `resamb_LAMBDA`). Returns null when Q_b is not
+   * positive definite / invertible or the search fails; ratio
+   * validation is the caller's job.
+   */
+  tryFix(idxs) {
+    const k = idxs.length;
+    const b = new Float64Array(k);
+    const Qb = new Float64Array(k * k);
+    const QbRows = [];
+    for (let i = 0; i < k; i++) {
+      const si = 3 + idxs[i];
+      b[i] = this.x[si];
+      const row = [];
+      for (let j = 0; j < k; j++) {
+        const sj = 3 + idxs[j];
+        const q = (this.P[si][sj] + this.P[sj][si]) / 2;
+        Qb[i + j * k] = q;
+        row.push(q);
+      }
+      QbRows.push(row);
+    }
+    const res = lambdaSearch(b, Qb, k, 2);
+    if (!res) return null;
+    const best = res.candidates[0];
+    const QbInv = matInv(QbRows);
+    if (!QbInv) return null;
+    const db = [];
+    for (let i = 0; i < k; i++) db.push(b[i] - best[i]);
+    const position = [
+      this.x[0],
+      this.x[1],
+      this.x[2]
+    ];
+    for (let r = 0; r < 3; r++) {
+      let dx = 0;
+      for (let i = 0; i < k; i++) {
+        let yi = 0;
+        for (let j = 0; j < k; j++) yi += QbInv[i][j] * db[j];
+        dx += this.P[r][3 + idxs[i]] * yi;
+      }
+      position[r] -= dx;
+    }
+    return { ratio: res.ratio, position, intAmb: [...best] };
+  }
   /**
    * Process one synchronized epoch (same nominal `timeMs` for both
    * receivers). Returns null until a first position can be
@@ -1130,6 +1437,47 @@ var RtkFloatEngine = class {
         Pnew[j][i] = s;
       }
     this.P = Pnew;
+    let status = rows.some(
+      (r) => r.kind === "phase"
+    ) ? "float" : "dgnss";
+    let ratio;
+    let nFixed;
+    let fixedPos = null;
+    let fixedAmbiguities;
+    if (o.ambiguityResolution === "instant") {
+      const cands = [];
+      for (const row of rows) {
+        if (row.kind !== "phase") continue;
+        if (row.g.prn[0] === "R" && !o.glonassAr) continue;
+        if (row.g.elB < o.arElevationMaskDeg * Math.PI / 180) continue;
+        cands.push({ idx: row.ambIdx, el: row.g.elB });
+      }
+      cands.sort((a, b) => b.el - a.el);
+      const floor = Math.max(
+        o.minFixAmbiguities,
+        Math.ceil(cands.length / 2),
+        1
+      );
+      while (cands.length >= floor) {
+        const fix = this.tryFix(cands.map((c) => c.idx));
+        if (!fix) break;
+        const capped = Math.min(fix.ratio, 999.9);
+        ratio ??= capped;
+        if (fix.ratio >= o.ratioThreshold) {
+          ratio = capped;
+          status = "fixed";
+          nFixed = cands.length;
+          fixedPos = fix.position;
+          fixedAmbiguities = {};
+          cands.forEach((c, i) => {
+            fixedAmbiguities[this.amb[c.idx].prn] = fix.intAmb[i];
+          });
+          break;
+        }
+        if (!o.partialFixing) break;
+        cands.pop();
+      }
+    }
     for (const g of geom) {
       if (g.cpR === null || g.cpB === null) continue;
       this.track.set(g.prn, { lockR: g.lockR, lockB: g.lockB, lastMs: timeMs });
@@ -1143,14 +1491,22 @@ var RtkFloatEngine = class {
     const nSats = new Set(rows.flatMap((r) => [r.g.prn, r.ref.prn])).size;
     return {
       timeMs,
-      position: [this.x[0], this.x[1], this.x[2]],
+      position: fixedPos ?? [this.x[0], this.x[1], this.x[2]],
       floatBaseline: [
         this.x[0] - this.basePos[0],
         this.x[1] - this.basePos[1],
         this.x[2] - this.basePos[2]
       ],
+      status,
       nSats,
-      ratio: void 0,
+      ratio,
+      nFixed,
+      fixedBaseline: fixedPos ? [
+        fixedPos[0] - this.basePos[0],
+        fixedPos[1] - this.basePos[1],
+        fixedPos[2] - this.basePos[2]
+      ] : void 0,
+      fixedAmbiguities,
       sigmas: [
         Math.sqrt(Math.max(this.P[0][0], 0)),
         Math.sqrt(Math.max(this.P[1][1], 0)),
@@ -1197,10 +1553,17 @@ var PRIMARY_FREQ_HZ = {
   C: 1561098e3,
   R: 1602e6
 };
-var F_L1 = 157542e4;
-function tropoDelay2(elevationRad) {
-  const sinEl = Math.sin(elevationRad);
-  return 2.47 / (sinEl + 0.0121);
+var F_L12 = 157542e4;
+function tropoDelay2(elevationRad, latRad, heightM) {
+  if (elevationRad <= 0) return 0;
+  const h = heightM < 0 ? 0 : heightM > 1e4 ? 1e4 : heightM;
+  const humi = 0.7;
+  const pres = 1013.25 * Math.pow(1 - 22557e-9 * h, 5.2568);
+  const temp = 15 - 65e-4 * h + 273.16;
+  const e = 6.108 * humi * Math.exp((17.15 * temp - 4684) / (temp - 38.45));
+  const zhd = 22768e-7 * pres / (1 - 266e-5 * Math.cos(2 * latRad) - 28e-5 * (h / 1e3));
+  const zwd = 2277e-6 * (1255 / temp + 0.05) * e;
+  return (zhd + zwd) / Math.sin(elevationRad);
 }
 function sagnac2(pos, travelTimeS) {
   const theta = OMEGA_E * travelTimeS;
@@ -1233,7 +1596,8 @@ function solveSpp(pseudoranges, ephemerides, timeMs, opts = {}) {
     maxIterations = 15,
     convergenceM = 1e-4,
     tgd = true,
-    iono
+    iono,
+    gim
   } = opts;
   const gpsTow = (timeMs - GPS_EPOCH_MS_SPP) / 1e3 % 604800;
   const all = [...pseudoranges.keys()].filter((p) => ephemerides.has(p));
@@ -1260,7 +1624,7 @@ function solveSpp(pseudoranges, ephemerides, timeMs, opts = {}) {
       let rows = 0;
       for (const k of Object.keys(residuals2)) delete residuals2[k];
       const positionSaneIter = x2 * x2 + y2 * y2 + z2 * z2 > 1e12;
-      const [rxLat, rxLon] = positionSaneIter ? ecefToGeodetic(x2, y2, z2) : [0, 0];
+      const [rxLat, rxLon, rxHgt] = positionSaneIter ? ecefToGeodetic(x2, y2, z2) : [0, 0, 0];
       for (const prn of prns) {
         const psr = pseudoranges.get(prn);
         const eph = ephemerides.get(prn);
@@ -1290,11 +1654,18 @@ function solveSpp(pseudoranges, ephemerides, timeMs, opts = {}) {
           const sinEl = Math.max(Math.sin(elev), 0.1);
           weight = sinEl * sinEl;
         }
-        const tropo = troposphere && positionSane ? tropoDelay2(elev) : 0;
+        const tropo = troposphere && positionSane ? tropoDelay2(elev, rxLat, rxHgt) : 0;
         let ionoM = 0;
-        if (iono && positionSane) {
-          const f = PRIMARY_FREQ_HZ[sys] ?? F_L1;
-          ionoM = C_LIGHT * klobucharDelay(iono, rxLat, rxLon, azim, elev, gpsTow) * (F_L1 / f * (F_L1 / f));
+        if ((gim || iono) && positionSane) {
+          let l1M = null;
+          if (gim) l1M = gimSlantIonoDelayL1(gim, rxLat, rxLon, azim, elev, timeMs);
+          if (l1M === null && iono) {
+            l1M = C_LIGHT * klobucharDelay(iono, rxLat, rxLon, azim, elev, gpsTow);
+          }
+          if (l1M !== null) {
+            const f = PRIMARY_FREQ_HZ[sys] ?? F_L12;
+            ionoM = l1M * (F_L12 / f * (F_L12 / f));
+          }
         }
         const predicted = rho + clk - C_LIGHT * dts + tropo + ionoM;
         const v = psr - predicted;
@@ -1375,9 +1746,14 @@ function solveSpp(pseudoranges, ephemerides, timeMs, opts = {}) {
 }
 // Annotate the CommonJS export names for ESM import in node:
 0 && (module.exports = {
+  IONO_L1_M_PER_TECU,
   RtkFloatEngine,
+  gimSlantIonoDelayL1,
+  gimVerticalTec,
   ionoFree,
   klobucharDelay,
+  lambdaReduction,
+  lambdaSearch,
   satClockCorrection,
   solveDgnss,
   solveSpp,