gnss-js 1.23.0 → 1.24.0

package/dist/{chunk-CUWF56ST.js → chunk-T2ZMNRCY.js}

@@ -43,6 +43,207 @@ function klobucharDelay(coeffs, latRad, lonRad, azRad, elRad, gpsTowSec) {
   return F * (5e-9 + amp * (1 - x2 / 2 + x2 * x2 / 24));
 }
 
+// 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"],
@@ -403,7 +604,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). */
@@ -461,6 +668,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
@@ -711,6 +968,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 });
@@ -724,14 +1022,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)),
@@ -957,6 +1263,8 @@ function solveSpp(pseudoranges, ephemerides, timeMs, opts = {}) {
 
 export {
   klobucharDelay,
+  lambdaSearch,
+  lambdaReduction,
   toRtkEpoch,
   solveDgnss,
   RtkFloatEngine,