gnss-js 2.0.0 → 2.1.0

package/dist/{chunk-2CMAFEHI.js → chunk-DWUQRJ37.js}

@@ -1918,6 +1918,55 @@ var SbasProcessor = class {
   }
 };
 
+// src/positioning/ssr-apply.ts
+var cross = (a, b) => [
+  a[1] * b[2] - a[2] * b[1],
+  a[2] * b[0] - a[0] * b[2],
+  a[0] * b[1] - a[1] * b[0]
+];
+var norm = (a) => Math.hypot(a[0], a[1], a[2]);
+var unit = (a) => {
+  const n = norm(a);
+  return n > 0 ? [a[0] / n, a[1] / n, a[2] / n] : [0, 0, 0];
+};
+function applyOrbitClock(eph, corr, timeMs) {
+  if (eph.system === "R" || eph.system === "S") return null;
+  if (eph.iode !== corr.iod) return null;
+  const age = (timeMs - corr.refTimeMs) / 1e3;
+  if (corr.validitySeconds != null && (age < -1 || age > corr.validitySeconds))
+    return null;
+  const s = computeSatPosition(eph, timeMs);
+  if (!Number.isFinite(s.x)) return null;
+  const p = [s.x, s.y, s.z];
+  const v = [s.vx, s.vy, s.vz];
+  const er = unit(p);
+  const en = unit(cross(p, v));
+  const ea = cross(en, er);
+  const dr = corr.radial + (corr.dotRadial ?? 0) * age;
+  const da = corr.along + (corr.dotAlong ?? 0) * age;
+  const dc = corr.cross + (corr.dotCross ?? 0) * age;
+  const x = s.x + dr * er[0] + da * ea[0] + dc * en[0];
+  const y = s.y + dr * er[1] + da * ea[1] + dc * en[1];
+  const z = s.z + dr * er[2] + da * ea[2] + dc * en[2];
+  const clockM = corr.clockC0 + (corr.clockC1 ?? 0) * age + (corr.clockC2 ?? 0) * age * age + (corr.highRateClock ?? 0);
+  return { x, y, z, clockOffsetS: clockM / C_LIGHT };
+}
+function hasToOrbitClock(orbit, clock, refTimeMs, validitySeconds) {
+  if (orbit.deltaRadial == null || orbit.deltaInTrack == null || orbit.deltaCrossTrack == null)
+    return null;
+  const clockC0 = clock && clock.deltaClock != null && !clock.notUsable ? clock.deltaClock : 0;
+  return {
+    prn: orbit.prn,
+    iod: orbit.gnssIod,
+    refTimeMs,
+    radial: orbit.deltaRadial,
+    along: orbit.deltaInTrack,
+    cross: orbit.deltaCrossTrack,
+    clockC0,
+    validitySeconds
+  };
+}
+
 // src/positioning/ppp-tropo.ts
 var HYD_AVG_A = [
   0.0012769934,
@@ -2175,14 +2224,14 @@ function buildPppAntenna(antex) {
     }
   };
 }
-function cross(a, b) {
+function cross2(a, b) {
   return [
     a[1] * b[2] - a[2] * b[1],
     a[2] * b[0] - a[0] * b[2],
     a[0] * b[1] - a[1] * b[0]
   ];
 }
-function norm(a) {
+function norm2(a) {
   const n = Math.hypot(a[0], a[1], a[2]) || 1;
   return [a[0] / n, a[1] / n, a[2] / n];
 }
@@ -2190,14 +2239,14 @@ function dot(a, b) {
   return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
 }
 function satBodyFrame(satEcef, sunEcef2) {
-  const z = norm([-satEcef[0], -satEcef[1], -satEcef[2]]);
-  const eSun = norm([
+  const z = norm2([-satEcef[0], -satEcef[1], -satEcef[2]]);
+  const eSun = norm2([
     sunEcef2[0] - satEcef[0],
     sunEcef2[1] - satEcef[1],
     sunEcef2[2] - satEcef[2]
   ]);
-  const y = norm(cross(z, eSun));
-  const x = norm(cross(y, z));
+  const y = norm2(cross2(z, eSun));
+  const x = norm2(cross2(y, z));
   return { x, y, z };
 }
 function satPcoToEcef(pcoBody, frame) {
@@ -2238,7 +2287,7 @@ function phaseWindup(prn, losRcvToSat, satFrame, latRad, lonRad, state) {
   const kx = satFrame.x;
   const ky = satFrame.y;
   const kdotx = dot(k, kx);
-  const kcrossY = cross(k, ky);
+  const kcrossY = cross2(k, ky);
   const dS = [
     kx[0] - k[0] * kdotx - kcrossY[0],
     kx[1] - k[1] * kdotx - kcrossY[1],
@@ -2247,7 +2296,7 @@ function phaseWindup(prn, losRcvToSat, satFrame, latRad, lonRad, state) {
   const north = enuToEcef([0, 1, 0], latRad, lonRad);
   const east = enuToEcef([1, 0, 0], latRad, lonRad);
   const kdotn = dot(k, north);
-  const kcrossE = cross(k, east);
+  const kcrossE = cross2(k, east);
   const dR = [
     north[0] - k[0] * kdotn + kcrossE[0],
     north[1] - k[1] * kdotn + kcrossE[1],
@@ -2257,7 +2306,7 @@ function phaseWindup(prn, losRcvToSat, satFrame, latRad, lonRad, state) {
   const nR = Math.hypot(dR[0], dR[1], dR[2]) || 1;
   let cosPhi = dot(dS, dR) / (nS * nR);
   cosPhi = Math.max(-1, Math.min(1, cosPhi));
-  const zeta = cross(dS, dR);
+  const zeta = cross2(dS, dR);
   const sign = dot(k, zeta) >= 0 ? 1 : -1;
   const dphi = sign * Math.acos(cosPhi) / (2 * Math.PI);
   const prev = state.get(prn);
@@ -2395,13 +2444,35 @@ function ifCoeffs(f1, f2) {
   const lambdaIf = C_LIGHT / (f1 + f2);
   return { g, lambdaIf };
 }
+var Sp3EphemerisSource = class {
+  constructor(sp3, clk) {
+    this.sp3 = sp3;
+    this.clk = clk;
+  }
+  satState(prn, tEmitMs) {
+    const p = sp3Position(this.sp3, prn, tEmitMs);
+    if (!p || p.clk == null) return null;
+    const pPlus = sp3Position(this.sp3, prn, tEmitMs + 500);
+    const pMinus = sp3Position(this.sp3, prn, tEmitMs - 500);
+    let vx = 0;
+    let vy = 0;
+    let vz = 0;
+    if (pPlus && pMinus) {
+      vx = pPlus.x - pMinus.x;
+      vy = pPlus.y - pMinus.y;
+      vz = pPlus.z - pMinus.z;
+    }
+    const clkS = (this.clk ? clkBias(this.clk, prn, tEmitMs) : null) ?? p.clk;
+    return { x: p.x, y: p.y, z: p.z, vx, vy, vz, clkS };
+  }
+};
 function sagnacRotate(x, y, z, travelS) {
   const theta = OMEGA_E * travelS;
   const c = Math.cos(theta);
   const s = Math.sin(theta);
   return [x * c + y * s, -x * s + y * c, z];
 }
-function satStateAtEmission(sp3, prn, recvTimeMs, rcv, clk) {
+function satStateAtEmission(source, prn, recvTimeMs, rcv) {
   let travelS = 0.075;
   let sx = 0;
   let sy = 0;
@@ -2410,18 +2481,11 @@ function satStateAtEmission(sp3, prn, recvTimeMs, rcv, clk) {
   let velDotPos = 0;
   for (let iter = 0; iter < 3; iter++) {
     const tEmit = recvTimeMs - travelS * 1e3;
-    const p = sp3Position(sp3, prn, tEmit);
-    if (!p || p.clk == null) return null;
-    const pPlus = sp3Position(sp3, prn, tEmit + 500);
-    const pMinus = sp3Position(sp3, prn, tEmit - 500);
-    if (pPlus && pMinus) {
-      const vx = (pPlus.x - pMinus.x) / 1;
-      const vy = (pPlus.y - pMinus.y) / 1;
-      const vz = (pPlus.z - pMinus.z) / 1;
-      velDotPos = p.x * vx + p.y * vy + p.z * vz;
-    }
-    clkS = (clk && clkBias(clk, prn, tEmit)) ?? p.clk;
-    const [rx, ry, rz] = sagnacRotate(p.x, p.y, p.z, travelS);
+    const s = source.satState(prn, tEmit);
+    if (!s) return null;
+    velDotPos = s.x * s.vx + s.y * s.vy + s.z * s.vz;
+    clkS = s.clkS;
+    const [rx, ry, rz] = sagnacRotate(s.x, s.y, s.z, travelS);
     sx = rx;
     sy = ry;
     sz = rz;
@@ -2443,39 +2507,72 @@ var IDX_ZWD = 3;
 var NBASE = 4;
 var CLK_VAR = 1e10;
 var POS_VAR = 60 * 60;
-function solvePpp(epochs, sp3, opts) {
-  const elevMask = (opts.elevationMaskDeg ?? 10) * Math.PI / 180;
-  const codeSigma = opts.codeSigma ?? 3;
-  const phaseSigma = opts.phaseSigma ?? 0.01;
-  const ztdQ = opts.ztdProcessNoise ?? 1e-8;
-  const kinematic = opts.mode === "kinematic";
-  const corrections = opts.corrections;
-  const x = [
-    opts.aprioriPos[0],
-    opts.aprioriPos[1],
-    opts.aprioriPos[2],
-    0.1
-    // zenith wet delay (m) a priori
-  ];
-  const P = [];
-  for (let i = 0; i < NBASE; i++) {
-    P.push(new Array(NBASE).fill(0));
-  }
-  P[IDX_X][IDX_X] = 100 * 100;
-  P[IDX_Y][IDX_Y] = 100 * 100;
-  P[IDX_Z][IDX_Z] = 100 * 100;
-  P[IDX_ZWD][IDX_ZWD] = 0.5 * 0.5;
-  const clkIdx = /* @__PURE__ */ new Map();
-  const clockSeeded = /* @__PURE__ */ new Set();
-  const ambIdx = /* @__PURE__ */ new Map();
-  const lastSeen = /* @__PURE__ */ new Map();
-  const arcs = [];
-  const arcSnap = /* @__PURE__ */ new Map();
-  const flushArc = (prn, minEpochs = 10) => {
-    const s = arcSnap.get(prn);
-    arcSnap.delete(prn);
+var PppEngine = class {
+  source;
+  opts;
+  elevMask;
+  codeSigma;
+  phaseSigma;
+  ztdQ;
+  kinematic;
+  corrections;
+  truth;
+  // Filter state (reset() initialises; mutated in place across epochs).
+  x;
+  P;
+  clkIdx;
+  clockSeeded;
+  ambIdx;
+  lastSeen;
+  arcs;
+  arcSnap;
+  mwState;
+  series;
+  ei = 0;
+  t0 = 0;
+  convergenceSec = null;
+  convergedStreak = 0;
+  constructor(opts, source) {
+    this.opts = opts;
+    this.source = source;
+    this.elevMask = (opts.elevationMaskDeg ?? 10) * Math.PI / 180;
+    this.codeSigma = opts.codeSigma ?? 3;
+    this.phaseSigma = opts.phaseSigma ?? 0.01;
+    this.ztdQ = opts.ztdProcessNoise ?? 1e-8;
+    this.kinematic = opts.mode === "kinematic";
+    this.corrections = opts.corrections;
+    this.truth = opts.groundTruth;
+    this.reset();
+  }
+  /** Reinitialise the filter (fresh session). */
+  reset() {
+    const { opts } = this;
+    this.x = [opts.aprioriPos[0], opts.aprioriPos[1], opts.aprioriPos[2], 0.1];
+    const P = [];
+    for (let i = 0; i < NBASE; i++) P.push(new Array(NBASE).fill(0));
+    P[IDX_X][IDX_X] = 100 * 100;
+    P[IDX_Y][IDX_Y] = 100 * 100;
+    P[IDX_Z][IDX_Z] = 100 * 100;
+    P[IDX_ZWD][IDX_ZWD] = 0.5 * 0.5;
+    this.P = P;
+    this.clkIdx = /* @__PURE__ */ new Map();
+    this.clockSeeded = /* @__PURE__ */ new Set();
+    this.ambIdx = /* @__PURE__ */ new Map();
+    this.lastSeen = /* @__PURE__ */ new Map();
+    this.arcs = [];
+    this.arcSnap = /* @__PURE__ */ new Map();
+    this.mwState = /* @__PURE__ */ new Map();
+    this.series = [];
+    this.ei = 0;
+    this.t0 = 0;
+    this.convergenceSec = null;
+    this.convergedStreak = 0;
+  }
+  flushArc(prn, minEpochs = 10) {
+    const s = this.arcSnap.get(prn);
+    this.arcSnap.delete(prn);
     if (!s || s.nEpochs < minEpochs) return;
-    arcs.push({
+    this.arcs.push({
       prn,
       aIF: s.aIF,
       mwCyc: s.mwCyc,
@@ -2486,64 +2583,88 @@ function solvePpp(epochs, sp3, opts) {
       startMs: s.startMs,
       endMs: s.endMs
     });
-  };
-  const mwState = /* @__PURE__ */ new Map();
-  const detectSlip = (o, ei) => {
+  }
+  /** Detect a cycle slip via the Melbourne–Wübbena wide-lane ambiguity. */
+  detectSlip(o, ei) {
     const lamW = C_LIGHT / (o.f1 - o.f2);
     const lw = o.l1 - o.l2;
     const pn = (o.f1 * o.c1 + o.f2 * o.c2) / (o.f1 + o.f2);
     const mw = lw - pn / lamW;
-    const st = mwState.get(o.prn);
+    const st = this.mwState.get(o.prn);
     let slip = o.slip;
     if (!st || ei - st.lastEi > 2)
       slip = true;
     else if (Math.abs(mw - st.mean) > 4) slip = true;
-    if (slip) mwState.set(o.prn, { mean: mw, n: 1, lastEi: ei });
+    if (slip) this.mwState.set(o.prn, { mean: mw, n: 1, lastEi: ei });
     else {
       const n = st.n + 1;
-      mwState.set(o.prn, {
+      this.mwState.set(o.prn, {
         mean: st.mean + (mw - st.mean) / Math.min(n, 100),
         n,
         lastEi: ei
       });
     }
     return slip;
-  };
-  const growState = (initVal, initVar) => {
-    const idx = x.length;
-    x.push(initVal);
-    for (const row of P) row.push(0);
-    P.push(new Array(x.length).fill(0));
-    P[idx][idx] = initVar;
+  }
+  growState(initVal, initVar) {
+    const idx = this.x.length;
+    this.x.push(initVal);
+    for (const row of this.P) row.push(0);
+    this.P.push(new Array(this.x.length).fill(0));
+    this.P[idx][idx] = initVar;
     return idx;
-  };
-  const dropState = (idx) => {
-    x.splice(idx, 1);
-    P.splice(idx, 1);
-    for (const row of P) row.splice(idx, 1);
-    for (const [prn, i] of ambIdx) {
-      if (i === idx) ambIdx.delete(prn);
-      else if (i > idx) ambIdx.set(prn, i - 1);
+  }
+  dropState(idx) {
+    this.x.splice(idx, 1);
+    this.P.splice(idx, 1);
+    for (const row of this.P) row.splice(idx, 1);
+    for (const [prn, i] of this.ambIdx) {
+      if (i === idx) this.ambIdx.delete(prn);
+      else if (i > idx) this.ambIdx.set(prn, i - 1);
     }
-    for (const [sys, i] of clkIdx) {
-      if (i > idx) clkIdx.set(sys, i - 1);
+    for (const [sys, i] of this.clkIdx) {
+      if (i > idx) this.clkIdx.set(sys, i - 1);
     }
-  };
-  const clockOf = (sys) => {
-    let i = clkIdx.get(sys);
+  }
+  /** State index of a constellation's receiver clock, creating it lazily. */
+  clockOf(sys) {
+    let i = this.clkIdx.get(sys);
     if (i === void 0) {
-      i = growState(0, CLK_VAR);
-      clkIdx.set(sys, i);
+      i = this.growState(0, CLK_VAR);
+      this.clkIdx.set(sys, i);
     }
     return i;
-  };
-  const series = [];
-  const truth = opts.groundTruth;
-  const t0 = epochs[0]?.timeMs ?? 0;
-  let convergenceSec = null;
-  let convergedStreak = 0;
-  for (let ei = 0; ei < epochs.length; ei++) {
-    const epoch = epochs[ei];
+  }
+  /** Process one epoch, carrying the filter forward; returns its result. */
+  process(epoch) {
+    const {
+      source,
+      opts,
+      elevMask,
+      codeSigma,
+      phaseSigma,
+      ztdQ,
+      kinematic,
+      corrections,
+      truth,
+      x,
+      P,
+      clkIdx,
+      clockSeeded,
+      ambIdx,
+      lastSeen,
+      arcSnap,
+      mwState,
+      series
+    } = this;
+    const flushArc = this.flushArc.bind(this);
+    const detectSlip = this.detectSlip.bind(this);
+    const growState = this.growState.bind(this);
+    const dropState = this.dropState.bind(this);
+    const clockOf = this.clockOf.bind(this);
+    const ei = this.ei;
+    if (ei === 0) this.t0 = epoch.timeMs;
+    const t0 = this.t0;
     if (kinematic && ei > 0) {
       for (const p of [IDX_X, IDX_Y, IDX_Z]) {
         for (let j = 0; j < x.length; j++) {
@@ -2564,7 +2685,7 @@ function solvePpp(epochs, sp3, opts) {
     for (const o of epoch.obs) {
       if (o.c1 === 0 || o.c2 === 0 || o.l1 === 0 || o.l2 === 0) continue;
       const slip = detectSlip(o, ei);
-      const sat = satStateAtEmission(sp3, o.prn, epoch.timeMs, rcv, opts.clk);
+      const sat = satStateAtEmission(source, o.prn, epoch.timeMs, rcv);
       if (!sat) continue;
       const [elRad, azRad] = getAer(
         sat.state.x,
@@ -2584,7 +2705,9 @@ function solvePpp(epochs, sp3, opts) {
       const lam1 = C_LIGHT / o.f1;
       const lam2 = C_LIGHT / o.f2;
       if (o.c1 === 0 || o.c2 === 0 || o.l1 === 0 || o.l2 === 0) continue;
-      const pIf = g * o.c1 - (g - 1) * o.c2;
+      const b1 = source.codeBias?.(o.prn, o.code1 ?? "", epoch.timeMs) ?? 0;
+      const b2 = source.codeBias?.(o.prn, o.code2 ?? "", epoch.timeMs) ?? 0;
+      const pIf = g * (o.c1 - b1) - (g - 1) * (o.c2 - b2);
       const lIf = g * (o.l1 * lam1) - (g - 1) * (o.l2 * lam2);
       if (!Number.isFinite(pIf) || !Number.isFinite(lIf)) continue;
       const trop = opts.troposphere ?? true ? niellMapping(elRad, latRad, heightM, epoch.timeMs) : { zhd: 0, mh: 0, mw: 0 };
@@ -2771,16 +2894,16 @@ function solvePpp(epochs, sp3, opts) {
         pos[2] - truth[2]
       );
       if (error3d < 0.1) {
-        convergedStreak++;
-        if (convergedStreak >= 10 && convergenceSec == null) {
-          convergenceSec = (epoch.timeMs - t0) / 1e3;
+        this.convergedStreak++;
+        if (this.convergedStreak >= 10 && this.convergenceSec == null) {
+          this.convergenceSec = (epoch.timeMs - t0) / 1e3;
         }
       } else {
-        convergedStreak = 0;
-        convergenceSec = null;
+        this.convergedStreak = 0;
+        this.convergenceSec = null;
       }
     }
-    series.push({
+    const result = {
       timeMs: epoch.timeMs,
       enu,
       error3d,
@@ -2789,50 +2912,68 @@ function solvePpp(epochs, sp3, opts) {
       ztdWet: x[IDX_ZWD],
       ztdHydrostatic: epochZhd,
       phaseResRms: nPhaseRes > 0 ? Math.sqrt(sumPhaseResSq / nPhaseRes) : 0
-    });
-  }
-  let finalState;
-  if (opts.exposeState) {
-    const ambs = [];
-    for (const [prn, index] of ambIdx) {
-      const s = arcSnap.get(prn);
-      if (!s || s.nEpochs < 10) continue;
-      ambs.push({
-        prn,
-        index,
-        aIF: x[index],
-        mwCyc: mwState.get(prn)?.mean ?? s.mwCyc,
-        f1: s.f1,
-        f2: s.f2,
-        elevDeg: s.elevSum / s.nEpochs
-      });
+    };
+    series.push(result);
+    this.ei = ei + 1;
+    return result;
+  }
+  /** Finalise the session: capture the AR state, flush open arcs, assemble. */
+  solution() {
+    const { opts, x, P, ambIdx, arcSnap, mwState, arcs, series, truth } = this;
+    const flushArc = this.flushArc.bind(this);
+    let finalState;
+    if (opts.exposeState) {
+      const ambs = [];
+      for (const [prn, index] of ambIdx) {
+        const s = arcSnap.get(prn);
+        if (!s || s.nEpochs < 10) continue;
+        ambs.push({
+          prn,
+          index,
+          aIF: x[index],
+          mwCyc: mwState.get(prn)?.mean ?? s.mwCyc,
+          f1: s.f1,
+          f2: s.f2,
+          elevDeg: s.elevSum / s.nEpochs
+        });
+      }
+      finalState = {
+        position: [x[IDX_X], x[IDX_Y], x[IDX_Z]],
+        covariance: P.map((row) => row.slice()),
+        ambiguities: ambs
+      };
     }
-    finalState = {
-      position: [x[IDX_X], x[IDX_Y], x[IDX_Z]],
-      covariance: P.map((row) => row.slice()),
-      ambiguities: ambs
+    for (const prn of [...arcSnap.keys()]) flushArc(prn);
+    const finalPos = [
+      x[IDX_X],
+      x[IDX_Y],
+      x[IDX_Z]
+    ];
+    const llh = ecefToGeodetic(finalPos[0], finalPos[1], finalPos[2]);
+    const finalError3d = truth ? Math.hypot(
+      finalPos[0] - truth[0],
+      finalPos[1] - truth[1],
+      finalPos[2] - truth[2]
+    ) : null;
+    return {
+      position: finalPos,
+      llh,
+      series,
+      ztdWet: x[IDX_ZWD],
+      ztdHydrostatic: series.length ? series[series.length - 1].ztdHydrostatic : 0,
+      epochsUsed: series.length,
+      convergenceSec: this.convergenceSec,
+      finalError3d,
+      arcs,
+      finalState
     };
   }
-  for (const prn of [...arcSnap.keys()]) flushArc(prn);
-  const finalPos = [x[IDX_X], x[IDX_Y], x[IDX_Z]];
-  const llh = ecefToGeodetic(finalPos[0], finalPos[1], finalPos[2]);
-  const finalError3d = truth ? Math.hypot(
-    finalPos[0] - truth[0],
-    finalPos[1] - truth[1],
-    finalPos[2] - truth[2]
-  ) : null;
-  return {
-    position: finalPos,
-    llh,
-    series,
-    ztdWet: x[IDX_ZWD],
-    ztdHydrostatic: series.length ? series[series.length - 1].ztdHydrostatic : 0,
-    epochsUsed: epochs.length,
-    convergenceSec,
-    finalError3d,
-    arcs,
-    finalState
-  };
+};
+function solvePpp(epochs, sp3OrSource, opts) {
+  const source = "satState" in sp3OrSource ? sp3OrSource : new Sp3EphemerisSource(sp3OrSource, opts.clk);
+  const eng = new PppEngine(opts, source);
+  for (const epoch of epochs) eng.process(epoch);
+  return eng.solution();
 }
 
 // src/positioning/ppp-ar.ts
@@ -3580,6 +3721,8 @@ export {
   sbasLongTermDeg,
   sbasIonoDeg,
   SbasProcessor,
+  applyOrbitClock,
+  hasToOrbitClock,
   niellMapping,
   gmst,
   sunEcef,
@@ -3587,6 +3730,8 @@ export {
   solidEarthTide,
   buildPppAntenna,
   createPppCorrections,
+  Sp3EphemerisSource,
+  PppEngine,
   solvePpp,
   wlWavelength,
   nlWavelength,