gnss-js 2.11.0 → 2.12.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.
@@ -779,6 +779,12 @@ function solveZt(n, m, Z, E) {
779
779
  for (let cIdx = 0; cIdx < m; cIdx++) X[r + cIdx * n] = M[r * w + n + cIdx];
780
780
  return X;
781
781
  }
782
+ function normCdf(x) {
783
+ const z = Math.abs(x) / Math.SQRT2;
784
+ const t = 1 / (1 + 0.3275911 * z);
785
+ const erf = 1 - ((((1.061405429 * t - 1.453152027) * t + 1.421413741) * t - 0.284496736) * t + 0.254829592) * t * Math.exp(-z * z);
786
+ return x >= 0 ? 0.5 * (1 + erf) : 0.5 * (1 - erf);
787
+ }
782
788
  function lambdaSearch(a, Q, n, m = 2) {
783
789
  if (n <= 0 || m <= 0 || a.length < n || Q.length < n * n) return null;
784
790
  const L = new Float64Array(n * n);
@@ -805,7 +811,10 @@ function lambdaSearch(a, Q, n, m = 2) {
805
811
  candidates.push(v);
806
812
  }
807
813
  const ratio = m >= 2 && s[0] > 0 ? s[1] / s[0] : Infinity;
808
- return { candidates, residuals: s, ratio };
814
+ let successRate = 1;
815
+ for (let i = 0; i < n; i++)
816
+ successRate *= 2 * normCdf(1 / (2 * Math.sqrt(D[i]))) - 1;
817
+ return { candidates, residuals: s, ratio, successRate };
809
818
  }
810
819
  function lambdaReduction(Q, n) {
811
820
  if (n <= 0 || Q.length < n * n) return null;
@@ -820,7 +829,7 @@ function lambdaReduction(Q, n) {
820
829
 
821
830
  // src/positioning/ppp-antenna.ts
822
831
  function parseAntexTime(s) {
823
- const p = s.trim().split(/\s+/).map(Number);
832
+ const p = s.trim().split(/[\s:-]+/).map(Number);
824
833
  if (p.length < 3 || p.some((v) => Number.isNaN(v))) return null;
825
834
  return Date.UTC(
826
835
  p[0],
@@ -870,11 +879,21 @@ function buildPppAntenna(antex) {
870
879
  satOffset(prn, dateMs, band1, band2, f1, f2) {
871
880
  const list = sats.get(prn);
872
881
  if (!list) return null;
873
- const entry = list.find((e) => {
874
- const from = parseAntexTime(e.validFrom);
875
- const until = parseAntexTime(e.validUntil);
876
- return (from == null || dateMs >= from) && (until == null || dateMs <= until);
877
- });
882
+ let entry;
883
+ let bestDist = Infinity;
884
+ for (const e of list) {
885
+ const from = parseAntexTime(e.validFrom) ?? -Infinity;
886
+ const until = parseAntexTime(e.validUntil) ?? Infinity;
887
+ if (dateMs >= from && dateMs <= until) {
888
+ entry = e;
889
+ break;
890
+ }
891
+ const dist = dateMs < from ? from - dateMs : dateMs - until;
892
+ if (dist < bestDist) {
893
+ bestDist = dist;
894
+ entry = e;
895
+ }
896
+ }
878
897
  if (!entry) return null;
879
898
  const a = freqOf(entry, band1);
880
899
  const b = freqOf(entry, band2);
@@ -882,10 +901,14 @@ function buildPppAntenna(antex) {
882
901
  return withZen(entry, combineIf(a, b, f1, f2));
883
902
  },
884
903
  rcvOffset(antType, band1, band2, f1, f2) {
885
- const entry = rcvs.get(antType);
904
+ const entry = rcvs.get(antType) ?? rcvs.get(antType.substring(0, 16).padEnd(16, " ") + "NONE");
886
905
  if (!entry) return null;
887
- const a = freqOf(entry, band1);
888
- const b = freqOf(entry, band2);
906
+ let a = freqOf(entry, band1);
907
+ let b = freqOf(entry, band2);
908
+ if (!a || !b) {
909
+ a = freqOf(entry, "G01");
910
+ b = freqOf(entry, "G02");
911
+ }
889
912
  if (!a || !b) return null;
890
913
  return withZen(entry, combineIf(a, b, f1, f2));
891
914
  }
@@ -961,13 +984,13 @@ function phaseWindup(prn, losRcvToSat, satFrame, latRad, lonRad, state) {
961
984
  kx[2] - k[2] * kdotx - kcrossY[2]
962
985
  ];
963
986
  const north = enuToEcef([0, 1, 0], latRad, lonRad);
964
- const east = enuToEcef([1, 0, 0], latRad, lonRad);
987
+ const west = enuToEcef([-1, 0, 0], latRad, lonRad);
965
988
  const kdotn = dot(k, north);
966
- const kcrossE = cross(k, east);
989
+ const kcrossW = cross(k, west);
967
990
  const dR = [
968
- north[0] - k[0] * kdotn + kcrossE[0],
969
- north[1] - k[1] * kdotn + kcrossE[1],
970
- north[2] - k[2] * kdotn + kcrossE[2]
991
+ north[0] - k[0] * kdotn + kcrossW[0],
992
+ north[1] - k[1] * kdotn + kcrossW[1],
993
+ north[2] - k[2] * kdotn + kcrossW[2]
971
994
  ];
972
995
  const nS = Math.hypot(dS[0], dS[1], dS[2]) || 1;
973
996
  const nR = Math.hypot(dR[0], dR[1], dR[2]) || 1;
@@ -1094,9 +1117,11 @@ function buildGeometry(rover, base, basePos, ephemerides, timeMs, maskRad, tropo
1094
1117
  let baseLatRad = 0;
1095
1118
  let baseLonRad = 0;
1096
1119
  if (antenna?.base) {
1097
- const [latDeg, lonDeg] = ecefToGeodetic(basePos[0], basePos[1], basePos[2]);
1098
- baseLatRad = latDeg * Math.PI / 180;
1099
- baseLonRad = lonDeg * Math.PI / 180;
1120
+ [baseLatRad, baseLonRad] = ecefToGeodetic(
1121
+ basePos[0],
1122
+ basePos[1],
1123
+ basePos[2]
1124
+ );
1100
1125
  }
1101
1126
  for (const [prn, mR] of rover) {
1102
1127
  const sys = prn[0];
@@ -1192,12 +1217,12 @@ function roverTerms(g, x, y, z, troposphere) {
1192
1217
  dTropo = tropoDelay(Math.max(elR, 0.05)) - g.tropoB;
1193
1218
  }
1194
1219
  if (g.offR) {
1195
- const [latDeg, lonDeg] = ecefToGeodetic(x, y, z);
1220
+ const [latRad, lonRad] = ecefToGeodetic(x, y, z);
1196
1221
  rho += rcvAntennaRangeM(
1197
1222
  g.offR,
1198
1223
  [-u[0], -u[1], -u[2]],
1199
- latDeg * Math.PI / 180,
1200
- lonDeg * Math.PI / 180,
1224
+ latRad,
1225
+ lonRad,
1201
1226
  elR,
1202
1227
  g.roverDelta
1203
1228
  );
@@ -3107,6 +3132,234 @@ function clkBias(clk, prn, tMs) {
3107
3132
  return c[lo] + f * (c[lo + 1] - c[lo]);
3108
3133
  }
3109
3134
 
3135
+ // src/positioning/ppp-ar.ts
3136
+ function wlWavelength(f1, f2) {
3137
+ return C_LIGHT / (f1 - f2);
3138
+ }
3139
+ function nlWavelength(f1, f2) {
3140
+ return C_LIGHT / (f1 + f2);
3141
+ }
3142
+ function resolvePppAmbiguities(sats, Qaif, opts = {}) {
3143
+ const wlThreshold = opts.wlThreshold ?? 0.25;
3144
+ const ratioThreshold = opts.ratioThreshold ?? 3;
3145
+ const minPartial = opts.minPartial ?? 4;
3146
+ const minSuccess = opts.minSuccessRate ?? 0;
3147
+ const n = sats.length;
3148
+ const none = (rejectedWl2, ratio = 0) => ({
3149
+ fixed: false,
3150
+ ratio,
3151
+ refPrn: null,
3152
+ refPrns: [],
3153
+ sats: [],
3154
+ rejectedWl: rejectedWl2
3155
+ });
3156
+ const elev = (i) => sats[i].elevDeg ?? -1;
3157
+ const wlOf = (i) => sats[i].mwCyc - (sats[i].wlBiasCyc ?? 0);
3158
+ const refOfSys = /* @__PURE__ */ new Map();
3159
+ for (let i = 0; i < n; i++) {
3160
+ const sys = sats[i].prn[0];
3161
+ const r = refOfSys.get(sys);
3162
+ if (r === void 0 || elev(i) > elev(r)) refOfSys.set(sys, i);
3163
+ }
3164
+ const nWlOf = /* @__PURE__ */ new Map();
3165
+ const refIdx = /* @__PURE__ */ new Map();
3166
+ const rejectedWl = [];
3167
+ for (const r of refOfSys.values()) {
3168
+ nWlOf.set(r, Math.round(wlOf(r)));
3169
+ refIdx.set(r, r);
3170
+ }
3171
+ for (let i = 0; i < n; i++) {
3172
+ if (refIdx.has(i)) continue;
3173
+ const r = refOfSys.get(sats[i].prn[0]);
3174
+ const sdWl = wlOf(i) - wlOf(r);
3175
+ const k = Math.round(sdWl);
3176
+ if (Math.abs(sdWl - k) <= wlThreshold) {
3177
+ nWlOf.set(i, nWlOf.get(r) + k);
3178
+ refIdx.set(i, r);
3179
+ } else rejectedWl.push(sats[i].prn);
3180
+ }
3181
+ const lamNl = (i) => nlWavelength(sats[i].f1, sats[i].f2);
3182
+ const n1Float = (i) => {
3183
+ const s = sats[i];
3184
+ return s.aIF / lamNl(i) - s.f2 / (s.f1 - s.f2) * nWlOf.get(i) - (s.nlBiasCyc ?? 0);
3185
+ };
3186
+ const qN1 = (a, b) => Qaif[a * n + b] / (lamNl(a) * lamNl(b));
3187
+ let cand = [...refIdx.keys()].filter((i) => refIdx.get(i) !== i).sort((a, b) => elev(b) - elev(a));
3188
+ if (cand.length < 1) return none(rejectedWl);
3189
+ let best = null;
3190
+ let firstRatio = 0;
3191
+ for (; ; ) {
3192
+ const m = cand.length;
3193
+ const sd = new Float64Array(m);
3194
+ const Qsd = new Float64Array(m * m);
3195
+ for (let a = 0; a < m; a++) {
3196
+ const ia = cand[a];
3197
+ const ra = refIdx.get(ia);
3198
+ sd[a] = n1Float(ia) - n1Float(ra);
3199
+ for (let b = 0; b < m; b++) {
3200
+ const ib = cand[b];
3201
+ const rb = refIdx.get(ib);
3202
+ Qsd[a * m + b] = qN1(ia, ib) - qN1(ia, rb) - qN1(ra, ib) + qN1(ra, rb);
3203
+ }
3204
+ }
3205
+ const res = lambdaSearch(sd, Qsd, m, 2);
3206
+ const ratio = res ? Math.min(res.ratio, 999.9) : 0;
3207
+ if (best === null && firstRatio === 0) firstRatio = ratio;
3208
+ if (res && ratio >= ratioThreshold && res.successRate >= minSuccess) {
3209
+ best = { set: cand, ratio, fix: Float64Array.from(res.candidates[0]) };
3210
+ break;
3211
+ }
3212
+ if (m <= minPartial) break;
3213
+ cand = cand.slice(0, -1);
3214
+ }
3215
+ if (!best) return { ...none(rejectedWl, firstRatio), refPrn: null };
3216
+ const refN1 = /* @__PURE__ */ new Map();
3217
+ for (const r of refOfSys.values()) refN1.set(r, Math.round(n1Float(r)));
3218
+ const out = [];
3219
+ const emit = (i, n1) => {
3220
+ const s = sats[i];
3221
+ const l = lamNl(i);
3222
+ const nWl = nWlOf.get(i);
3223
+ out.push({
3224
+ prn: s.prn,
3225
+ nWl,
3226
+ n1,
3227
+ aIF: l * n1 + l * (s.f2 / (s.f1 - s.f2)) * nWl,
3228
+ ref: sats[refIdx.get(i)].prn
3229
+ });
3230
+ };
3231
+ const usedRefs = new Set(best.set.map((i) => refIdx.get(i)));
3232
+ for (const r of usedRefs) emit(r, refN1.get(r));
3233
+ best.set.forEach(
3234
+ (i, a) => emit(i, refN1.get(refIdx.get(i)) + Math.round(best.fix[a]))
3235
+ );
3236
+ const refPrns = [...usedRefs].map((r) => sats[r].prn);
3237
+ return {
3238
+ fixed: true,
3239
+ ratio: best.ratio,
3240
+ refPrn: refPrns[0] ?? null,
3241
+ refPrns,
3242
+ sats: out,
3243
+ rejectedWl
3244
+ };
3245
+ }
3246
+ function solveSpd(A, b) {
3247
+ const n = b.length;
3248
+ const L = Array.from({ length: n }, () => new Array(n).fill(0));
3249
+ for (let i = 0; i < n; i++) {
3250
+ for (let j = 0; j <= i; j++) {
3251
+ let s = A[i][j];
3252
+ for (let k = 0; k < j; k++) s -= L[i][k] * L[j][k];
3253
+ if (i === j) {
3254
+ if (s <= 0) return null;
3255
+ L[i][j] = Math.sqrt(s);
3256
+ } else {
3257
+ L[i][j] = s / L[j][j];
3258
+ }
3259
+ }
3260
+ }
3261
+ const y = new Array(n).fill(0);
3262
+ for (let i = 0; i < n; i++) {
3263
+ let s = b[i];
3264
+ for (let k = 0; k < i; k++) s -= L[i][k] * y[k];
3265
+ y[i] = s / L[i][i];
3266
+ }
3267
+ const x = new Array(n).fill(0);
3268
+ for (let i = n - 1; i >= 0; i--) {
3269
+ let s = y[i];
3270
+ for (let k = i + 1; k < n; k++) s -= L[k][i] * x[k];
3271
+ x[i] = s / L[i][i];
3272
+ }
3273
+ return x;
3274
+ }
3275
+ function fixPppPosition(state, fcb, opts = {}) {
3276
+ const floatPos = state.position;
3277
+ const noFix = {
3278
+ fixed: false,
3279
+ position: floatPos,
3280
+ floatPosition: floatPos,
3281
+ shift: [0, 0, 0],
3282
+ nFixed: 0,
3283
+ ratio: 0,
3284
+ refPrn: null
3285
+ };
3286
+ const amb = state.ambiguities;
3287
+ if (amb.length < 3) return noFix;
3288
+ const sats = amb.map((a) => ({
3289
+ prn: a.prn,
3290
+ aIF: a.aIF,
3291
+ mwCyc: a.mwCyc,
3292
+ f1: a.f1,
3293
+ f2: a.f2,
3294
+ wlBiasCyc: fcb.satWlFcb.get(a.prn) ?? 0,
3295
+ nlBiasCyc: fcb.satNlFcb.get(a.prn) ?? 0,
3296
+ elevDeg: a.elevDeg
3297
+ }));
3298
+ const n = sats.length;
3299
+ const Qaif = new Float64Array(n * n);
3300
+ for (let i = 0; i < n; i++)
3301
+ for (let j = 0; j < n; j++)
3302
+ Qaif[i * n + j] = state.covariance[amb[i].index][amb[j].index];
3303
+ const res = resolvePppAmbiguities(sats, Qaif, {
3304
+ ...opts,
3305
+ minPartial: opts.minPartial ?? Math.max(4, opts.minFixed ?? 0)
3306
+ });
3307
+ if (!res.fixed || res.refPrn === null) {
3308
+ return { ...noFix, ratio: res.ratio, refPrn: res.refPrn };
3309
+ }
3310
+ const idxOfPrn = new Map(amb.map((a) => [a.prn, a.index]));
3311
+ const floatOfPrn = new Map(amb.map((a) => [a.prn, a.aIF]));
3312
+ const fixedOf = new Map(res.sats.map((s) => [s.prn, s.aIF]));
3313
+ const pairs = res.sats.filter(
3314
+ (s) => s.ref !== s.prn && idxOfPrn.has(s.prn) && idxOfPrn.has(s.ref)
3315
+ ).map((s) => ({ prn: s.prn, ref: s.ref }));
3316
+ const m = pairs.length;
3317
+ if (m < Math.max(1, opts.minFixed ?? 1))
3318
+ return { ...noFix, ratio: res.ratio, refPrn: res.refPrn };
3319
+ const P = state.covariance;
3320
+ const ix = (prn) => idxOfPrn.get(prn);
3321
+ const qAA = (i, j) => P[ix(i)][ix(j)];
3322
+ const qXA = (r, i) => P[r][ix(i)];
3323
+ const Qzz = Array.from({ length: m }, () => new Array(m).fill(0));
3324
+ const z = new Array(m).fill(0);
3325
+ for (let a = 0; a < m; a++) {
3326
+ const { prn: ia, ref: ra } = pairs[a];
3327
+ z[a] = floatOfPrn.get(ia) - floatOfPrn.get(ra) - (fixedOf.get(ia) - fixedOf.get(ra));
3328
+ for (let b = 0; b < m; b++) {
3329
+ const { prn: ib, ref: rb } = pairs[b];
3330
+ Qzz[a][b] = qAA(ia, ib) - qAA(ia, rb) - qAA(ra, ib) + qAA(ra, rb);
3331
+ }
3332
+ }
3333
+ const Qxz = [0, 1, 2].map(
3334
+ (r) => pairs.map(({ prn, ref }) => qXA(r, prn) - qXA(r, ref))
3335
+ );
3336
+ const y = solveSpd(Qzz, z);
3337
+ if (!y) return { ...noFix, ratio: res.ratio, refPrn: res.refPrn };
3338
+ let chi2 = 0;
3339
+ for (let a = 0; a < m; a++) chi2 += z[a] * y[a];
3340
+ if (chi2 / m > (opts.maxChi2 ?? Infinity))
3341
+ return { ...noFix, ratio: res.ratio, refPrn: res.refPrn };
3342
+ const shift = [0, 0, 0];
3343
+ for (let r = 0; r < 3; r++) {
3344
+ let s = 0;
3345
+ for (let a = 0; a < m; a++) s += Qxz[r][a] * y[a];
3346
+ shift[r] = -s;
3347
+ }
3348
+ return {
3349
+ fixed: true,
3350
+ position: [
3351
+ floatPos[0] + shift[0],
3352
+ floatPos[1] + shift[1],
3353
+ floatPos[2] + shift[2]
3354
+ ],
3355
+ floatPosition: floatPos,
3356
+ shift,
3357
+ nFixed: m,
3358
+ ratio: res.ratio,
3359
+ refPrn: res.refPrn
3360
+ };
3361
+ }
3362
+
3110
3363
  // src/positioning/ppp-tropo.ts
3111
3364
  var HYD_AVG_A = [
3112
3365
  0.0012769934,
@@ -3314,13 +3567,11 @@ function createPppCorrections(cfg) {
3314
3567
  let rangeM = 0;
3315
3568
  let phaseWindupM = 0;
3316
3569
  const e = geom.los;
3317
- const [latDeg, lonDeg] = ecefToGeodetic(
3570
+ const [latRad, lonRad] = ecefToGeodetic(
3318
3571
  geom.rcvEcef[0],
3319
3572
  geom.rcvEcef[1],
3320
3573
  geom.rcvEcef[2]
3321
3574
  );
3322
- const latRad = latDeg * Math.PI / 180;
3323
- const lonRad = lonDeg * Math.PI / 180;
3324
3575
  const frame = ctx ? satBodyFrame(geom.satEcef, ctx.sun) : null;
3325
3576
  if (useSat && frame && geom.band1 && geom.band2) {
3326
3577
  const off = cfg.antenna.satOffset(
@@ -3378,6 +3629,10 @@ function createPppCorrections(cfg) {
3378
3629
  }
3379
3630
 
3380
3631
  // src/positioning/ppp.ts
3632
+ var NO_FCB = {
3633
+ satWlFcb: /* @__PURE__ */ new Map(),
3634
+ satNlFcb: /* @__PURE__ */ new Map()
3635
+ };
3381
3636
  function matVec(P, h) {
3382
3637
  const n = P.length;
3383
3638
  const out = new Array(n).fill(0);
@@ -3389,6 +3644,34 @@ function matVec(P, h) {
3389
3644
  }
3390
3645
  return out;
3391
3646
  }
3647
+ function tdLsq(qs) {
3648
+ const M = [0, 1, 2, 3].map(() => [0, 0, 0, 0, 0]);
3649
+ for (const q of qs) {
3650
+ const a = [-q.v.e[0], -q.v.e[1], -q.v.e[2], 1];
3651
+ for (let i = 0; i < 4; i++) {
3652
+ M[i][4] += a[i] * q.d;
3653
+ for (let j = 0; j < 4; j++) M[i][j] += a[i] * a[j];
3654
+ }
3655
+ }
3656
+ for (let c = 0; c < 4; c++) {
3657
+ let p = c;
3658
+ for (let r = c + 1; r < 4; r++)
3659
+ if (Math.abs(M[r][c]) > Math.abs(M[p][c])) p = r;
3660
+ if (Math.abs(M[p][c]) < 1e-9) return null;
3661
+ [M[c], M[p]] = [M[p], M[c]];
3662
+ for (let r = 0; r < 4; r++) {
3663
+ if (r === c) continue;
3664
+ const f = M[r][c] / M[c][c];
3665
+ for (let k = c; k < 5; k++) M[r][k] -= f * M[c][k];
3666
+ }
3667
+ }
3668
+ return [0, 1, 2, 3].map((i) => M[i][4] / M[i][i]);
3669
+ }
3670
+ function tdResidual(q, s) {
3671
+ return q.d - (-q.v.e[0] * s[0] - q.v.e[1] * s[1] - q.v.e[2] * s[2] + s[3]);
3672
+ }
3673
+ var TD_SLIP_M = 0.08;
3674
+ var CODE_GATE_M = 15;
3392
3675
  function ekfUpdateScalar(x, P, h, innovation, r, rejectK = 0) {
3393
3676
  const n = x.length;
3394
3677
  const Ph = matVec(P, h);
@@ -3421,10 +3704,17 @@ function ifCoeffs(f1, f2) {
3421
3704
  return { g, lambdaIf };
3422
3705
  }
3423
3706
  var Sp3EphemerisSource = class {
3424
- constructor(sp3, clk) {
3707
+ constructor(sp3, clk, osb) {
3425
3708
  this.sp3 = sp3;
3426
3709
  this.clk = clk;
3710
+ this.osb = osb;
3711
+ if (osb) {
3712
+ this.codeBias = (prn, code, t) => osb.get(prn, "C" + code, t);
3713
+ this.phaseBias = (prn, code, t) => osb.get(prn, "L" + code, t);
3714
+ }
3427
3715
  }
3716
+ codeBias;
3717
+ phaseBias;
3428
3718
  satState(prn, tEmitMs) {
3429
3719
  const p = sp3Position(this.sp3, prn, tEmitMs);
3430
3720
  if (!p || p.clk == null) return null;
@@ -3442,6 +3732,21 @@ var Sp3EphemerisSource = class {
3442
3732
  return { x: p.x, y: p.y, z: p.z, vx, vy, vz, clkS };
3443
3733
  }
3444
3734
  };
3735
+ function applySatBiases(source, o, tMs) {
3736
+ if (!source.codeBias && !source.phaseBias) return o;
3737
+ const b1 = source.codeBias?.(o.prn, o.code1 ?? "", tMs) ?? 0;
3738
+ const b2 = source.codeBias?.(o.prn, o.code2 ?? "", tMs) ?? 0;
3739
+ let l1 = o.l1;
3740
+ let l2 = o.l2;
3741
+ if (source.phaseBias) {
3742
+ const p1 = source.phaseBias(o.prn, o.phase1 ?? o.code1 ?? "", tMs);
3743
+ const p2 = source.phaseBias(o.prn, o.phase2 ?? o.code2 ?? "", tMs);
3744
+ if (p1 == null || p2 == null) return null;
3745
+ l1 -= p1 / (C_LIGHT / o.f1);
3746
+ l2 -= p2 / (C_LIGHT / o.f2);
3747
+ }
3748
+ return { ...o, c1: o.c1 - b1, c2: o.c2 - b2, l1, l2 };
3749
+ }
3445
3750
  function sagnacRotate(x, y, z, travelS) {
3446
3751
  const theta = OMEGA_E * travelS;
3447
3752
  const c = Math.cos(theta);
@@ -3503,6 +3808,16 @@ var PppEngine = class {
3503
3808
  arcs;
3504
3809
  arcSnap;
3505
3810
  mwState;
3811
+ /** Previous epoch's phase terms per satellite, for the time-differenced
3812
+ * (clock-free) cycle-slip test. */
3813
+ tdPrev;
3814
+ /** Consecutive gated-out phase updates per satellite. */
3815
+ phaseRejects;
3816
+ /** PPP-AR: the latest accepted fix (null: float). */
3817
+ lastFix;
3818
+ lastArMs;
3819
+ firstFixMs;
3820
+ fixedEpochs;
3506
3821
  series;
3507
3822
  ei = 0;
3508
3823
  t0 = 0;
@@ -3538,6 +3853,12 @@ var PppEngine = class {
3538
3853
  this.arcs = [];
3539
3854
  this.arcSnap = /* @__PURE__ */ new Map();
3540
3855
  this.mwState = /* @__PURE__ */ new Map();
3856
+ this.tdPrev = /* @__PURE__ */ new Map();
3857
+ this.phaseRejects = /* @__PURE__ */ new Map();
3858
+ this.lastFix = null;
3859
+ this.lastArMs = -Infinity;
3860
+ this.firstFixMs = null;
3861
+ this.fixedEpochs = 0;
3541
3862
  this.series = [];
3542
3863
  this.ei = 0;
3543
3864
  this.t0 = 0;
@@ -3631,7 +3952,9 @@ var PppEngine = class {
3631
3952
  lastSeen,
3632
3953
  arcSnap,
3633
3954
  mwState,
3634
- series
3955
+ series,
3956
+ tdPrev,
3957
+ phaseRejects
3635
3958
  } = this;
3636
3959
  const flushArc = this.flushArc.bind(this);
3637
3960
  const detectSlip = this.detectSlip.bind(this);
@@ -3653,13 +3976,15 @@ var PppEngine = class {
3653
3976
  for (const ci of clkIdx.values()) P[ci][ci] = CLK_VAR;
3654
3977
  P[IDX_ZWD][IDX_ZWD] += ztdQ * (ei > 0 ? 1 : 0);
3655
3978
  const rcv = [x[IDX_X], x[IDX_Y], x[IDX_Z]];
3656
- const [latDeg, , heightM] = ecefToGeodetic(rcv[0], rcv[1], rcv[2]);
3657
- const latRad = latDeg * Math.PI / 180;
3979
+ const [latRad, , heightM] = ecefToGeodetic(rcv[0], rcv[1], rcv[2]);
3658
3980
  const epochCtx = corrections?.epochContext?.(epoch.timeMs, rcv) ?? null;
3659
3981
  const vis = [];
3660
3982
  let epochZhd = 0;
3661
- for (const o of epoch.obs) {
3662
- if (o.c1 === 0 || o.c2 === 0 || o.l1 === 0 || o.l2 === 0) continue;
3983
+ for (const raw of epoch.obs) {
3984
+ if (raw.c1 === 0 || raw.c2 === 0 || raw.l1 === 0 || raw.l2 === 0)
3985
+ continue;
3986
+ const o = applySatBiases(source, raw, epoch.timeMs);
3987
+ if (!o) continue;
3663
3988
  const slip = detectSlip(o, ei);
3664
3989
  const sat = satStateAtEmission(source, o.prn, epoch.timeMs, rcv);
3665
3990
  if (!sat) continue;
@@ -3681,9 +4006,7 @@ var PppEngine = class {
3681
4006
  const lam1 = C_LIGHT / o.f1;
3682
4007
  const lam2 = C_LIGHT / o.f2;
3683
4008
  if (o.c1 === 0 || o.c2 === 0 || o.l1 === 0 || o.l2 === 0) continue;
3684
- const b1 = source.codeBias?.(o.prn, o.code1 ?? "", epoch.timeMs) ?? 0;
3685
- const b2 = source.codeBias?.(o.prn, o.code2 ?? "", epoch.timeMs) ?? 0;
3686
- const pIf = g * (o.c1 - b1) - (g - 1) * (o.c2 - b2);
4009
+ const pIf = g * o.c1 - (g - 1) * o.c2;
3687
4010
  const lIf = g * (o.l1 * lam1) - (g - 1) * (o.l2 * lam2);
3688
4011
  if (!Number.isFinite(pIf) || !Number.isFinite(lIf)) continue;
3689
4012
  const trop = opts.troposphere ?? true ? niellMapping(elRad, latRad, heightM, epoch.timeMs) : { zhd: 0, mh: 0, mw: 0 };
@@ -3716,9 +4039,63 @@ var PppEngine = class {
3716
4039
  windupM: corr.phaseWindupM,
3717
4040
  slip,
3718
4041
  f1: o.f1,
3719
- f2: o.f2
4042
+ f2: o.f2,
4043
+ sat: [sat.state.x, sat.state.y, sat.state.z]
3720
4044
  });
3721
4045
  }
4046
+ {
4047
+ const dq = [];
4048
+ for (const v of vis) {
4049
+ const prev = tdPrev.get(v.prn);
4050
+ const model = -v.satClkM + v.tropoFixed + v.windupM;
4051
+ if (prev && prev.ei === ei - 1 && !v.slip) {
4052
+ const r0 = Math.hypot(
4053
+ prev.sat[0] - rcv[0],
4054
+ prev.sat[1] - rcv[1],
4055
+ prev.sat[2] - rcv[2]
4056
+ );
4057
+ dq.push({
4058
+ v,
4059
+ d: v.lIf - prev.lIf - (v.range - r0) - (model - prev.model)
4060
+ });
4061
+ }
4062
+ tdPrev.set(v.prn, { ei, lIf: v.lIf, sat: v.sat, model });
4063
+ }
4064
+ if (!kinematic) {
4065
+ if (dq.length >= 4) {
4066
+ const sorted = dq.map((q) => q.d).sort((a, b) => a - b);
4067
+ const med = sorted[Math.floor(sorted.length / 2)];
4068
+ for (const q of dq)
4069
+ if (Math.abs(q.d - med) > TD_SLIP_M) q.v.slip = true;
4070
+ }
4071
+ } else if (dq.length >= 6) {
4072
+ let use = dq;
4073
+ for (let iter = 0; iter < 6; iter++) {
4074
+ const sol = use.length >= 6 ? tdLsq(use) : null;
4075
+ if (!sol) {
4076
+ for (const q of dq) q.v.slip = true;
4077
+ break;
4078
+ }
4079
+ let worst = -1;
4080
+ let worstR = 0;
4081
+ use.forEach((q, k) => {
4082
+ const r = Math.abs(tdResidual(q, sol));
4083
+ if (r > worstR) {
4084
+ worstR = r;
4085
+ worst = k;
4086
+ }
4087
+ });
4088
+ if (worstR <= TD_SLIP_M || iter === 5) {
4089
+ for (const q of dq)
4090
+ if (Math.abs(tdResidual(q, sol)) > TD_SLIP_M) q.v.slip = true;
4091
+ break;
4092
+ }
4093
+ use = use.filter((_, k) => k !== worst);
4094
+ }
4095
+ } else {
4096
+ for (const q of dq) q.v.slip = true;
4097
+ }
4098
+ }
3722
4099
  const zwd0 = x[IDX_ZWD];
3723
4100
  const bySys = /* @__PURE__ */ new Map();
3724
4101
  for (const v of vis) {
@@ -3728,6 +4105,20 @@ var PppEngine = class {
3728
4105
  if (arr) arr.push(r);
3729
4106
  else bySys.set(sys, [r]);
3730
4107
  }
4108
+ const posSigma = Math.sqrt(
4109
+ P[IDX_X][IDX_X] + P[IDX_Y][IDX_Y] + P[IDX_Z][IDX_Z]
4110
+ );
4111
+ const codeGate = CODE_GATE_M + 3 * posSigma;
4112
+ const screened = /* @__PURE__ */ new Set();
4113
+ for (const [sys, res] of bySys) {
4114
+ if (res.length < 4) continue;
4115
+ const med = [...res].sort((a, b) => a - b)[Math.floor(res.length / 2)];
4116
+ for (const v of vis) {
4117
+ if (v.prn[0] !== sys) continue;
4118
+ const r = v.pIf - (v.range - v.satClkM + v.tropoFixed + zwd0 * v.mw);
4119
+ if (Math.abs(r - med) > codeGate) screened.add(v.prn);
4120
+ }
4121
+ }
3731
4122
  for (const [sys, res] of bySys) {
3732
4123
  const ci = clockOf(sys);
3733
4124
  if (!clockSeeded.has(sys)) {
@@ -3738,9 +4129,11 @@ var PppEngine = class {
3738
4129
  P[ci][ci] = CLK_VAR;
3739
4130
  }
3740
4131
  let nSats = 0;
3741
- for (const v of vis) {
4132
+ const live = vis.filter((v) => !screened.has(v.prn));
4133
+ const rangeNow = (v) => v.range - (v.e[0] * (x[IDX_X] - rcv[0]) + v.e[1] * (x[IDX_Y] - rcv[1]) + v.e[2] * (x[IDX_Z] - rcv[2]));
4134
+ const prepAmbiguity = (v) => {
3742
4135
  const ci = clockOf(v.prn[0]);
3743
- const common = v.range + x[ci] - v.satClkM + v.tropoFixed + zwd0 * v.mw;
4136
+ const common = rangeNow(v) + x[ci] - v.satClkM + v.tropoFixed + zwd0 * v.mw;
3744
4137
  if (v.slip) flushArc(v.prn);
3745
4138
  let ai = ambIdx.get(v.prn);
3746
4139
  if (ai === void 0 || v.slip) {
@@ -3757,67 +4150,91 @@ var PppEngine = class {
3757
4150
  }
3758
4151
  ambIdx.set(v.prn, ai);
3759
4152
  }
3760
- const n = x.length;
4153
+ return ai;
4154
+ };
4155
+ const codeUpdate = (v) => {
4156
+ const ci = clockOf(v.prn[0]);
4157
+ const common = rangeNow(v) + x[ci] - v.satClkM + v.tropoFixed + x[IDX_ZWD] * v.mw;
4158
+ const sinEl = Math.max(Math.sin(v.elRad), 0.1);
4159
+ const h = new Array(x.length).fill(0);
4160
+ h[IDX_X] = -v.e[0];
4161
+ h[IDX_Y] = -v.e[1];
4162
+ h[IDX_Z] = -v.e[2];
4163
+ h[ci] = 1;
4164
+ h[IDX_ZWD] = v.mw;
4165
+ const innov = v.pIf - common;
4166
+ if (Math.abs(innov) < 1e6)
4167
+ ekfUpdateScalar(x, P, h, innov, (codeSigma / sinEl) ** 2, 4);
4168
+ };
4169
+ const phaseUpdate = (v, ai) => {
4170
+ const ci = clockOf(v.prn[0]);
3761
4171
  const sinEl = Math.max(Math.sin(v.elRad), 0.1);
3762
- const rCode = (codeSigma / sinEl) ** 2;
3763
4172
  const rPhase = (phaseSigma / sinEl) ** 2;
3764
- {
3765
- const h = new Array(n).fill(0);
4173
+ const commonPh = rangeNow(v) + x[ci] - v.satClkM + v.tropoFixed + x[IDX_ZWD] * v.mw;
4174
+ const innov = v.lIf - (commonPh + v.windupM + x[ai]);
4175
+ const reinit = (phaseRejects.get(v.prn) ?? 0) >= 3;
4176
+ let used = false;
4177
+ if (!reinit) {
4178
+ const h = new Array(x.length).fill(0);
3766
4179
  h[IDX_X] = -v.e[0];
3767
4180
  h[IDX_Y] = -v.e[1];
3768
4181
  h[IDX_Z] = -v.e[2];
3769
4182
  h[ci] = 1;
3770
4183
  h[IDX_ZWD] = v.mw;
3771
- const innov = v.pIf - common;
3772
- if (Math.abs(innov) < 1e6) ekfUpdateScalar(x, P, h, innov, rCode, 4);
3773
- }
3774
- {
3775
- const commonPh = v.range + x[ci] - v.satClkM + v.tropoFixed + x[IDX_ZWD] * v.mw;
3776
- const innov = v.lIf - (commonPh + v.windupM + x[ai]);
3777
- if (Math.abs(innov) > 0.15) {
3778
- flushArc(v.prn);
3779
- x[ai] = v.lIf - (commonPh + v.windupM);
3780
- for (let j = 0; j < P.length; j++) {
3781
- P[ai][j] = 0;
3782
- P[j][ai] = 0;
3783
- }
3784
- P[ai][ai] = 100 * 100;
3785
- } else {
3786
- const h = new Array(n).fill(0);
3787
- h[IDX_X] = -v.e[0];
3788
- h[IDX_Y] = -v.e[1];
3789
- h[IDX_Z] = -v.e[2];
3790
- h[ci] = 1;
3791
- h[IDX_ZWD] = v.mw;
3792
- h[ai] = 1;
3793
- ekfUpdateScalar(x, P, h, innov, rPhase, 4);
3794
- }
4184
+ h[ai] = 1;
4185
+ used = ekfUpdateScalar(x, P, h, innov, rPhase, 4);
3795
4186
  }
3796
- {
3797
- const elevDeg = v.elRad * 180 / Math.PI;
3798
- const mwMean = mwState.get(v.prn)?.mean ?? 0;
3799
- const s = arcSnap.get(v.prn);
3800
- if (s) {
3801
- s.aIF = x[ai];
3802
- s.mwCyc = mwMean;
3803
- s.elevSum += elevDeg;
3804
- s.nEpochs++;
3805
- s.endMs = epoch.timeMs;
3806
- } else {
3807
- arcSnap.set(v.prn, {
3808
- aIF: x[ai],
3809
- mwCyc: mwMean,
3810
- f1: v.f1,
3811
- f2: v.f2,
3812
- elevSum: elevDeg,
3813
- nEpochs: 1,
3814
- startMs: epoch.timeMs,
3815
- endMs: epoch.timeMs
3816
- });
4187
+ if (reinit) {
4188
+ flushArc(v.prn);
4189
+ x[ai] = v.lIf - (commonPh + v.windupM);
4190
+ for (let j = 0; j < P.length; j++) {
4191
+ P[ai][j] = 0;
4192
+ P[j][ai] = 0;
3817
4193
  }
4194
+ P[ai][ai] = 100 * 100;
4195
+ phaseRejects.delete(v.prn);
4196
+ } else if (used) phaseRejects.delete(v.prn);
4197
+ else phaseRejects.set(v.prn, (phaseRejects.get(v.prn) ?? 0) + 1);
4198
+ };
4199
+ const snapshot = (v, ai) => {
4200
+ const elevDeg = v.elRad * 180 / Math.PI;
4201
+ const mwMean = mwState.get(v.prn)?.mean ?? 0;
4202
+ const s = arcSnap.get(v.prn);
4203
+ if (s) {
4204
+ s.aIF = x[ai];
4205
+ s.mwCyc = mwMean;
4206
+ s.elevSum += elevDeg;
4207
+ s.nEpochs++;
4208
+ s.endMs = epoch.timeMs;
4209
+ } else {
4210
+ arcSnap.set(v.prn, {
4211
+ aIF: x[ai],
4212
+ mwCyc: mwMean,
4213
+ f1: v.f1,
4214
+ f2: v.f2,
4215
+ elevSum: elevDeg,
4216
+ nEpochs: 1,
4217
+ startMs: epoch.timeMs,
4218
+ endMs: epoch.timeMs
4219
+ });
3818
4220
  }
3819
4221
  lastSeen.set(v.prn, ei);
3820
4222
  nSats++;
4223
+ };
4224
+ if (kinematic) {
4225
+ for (const v of live) codeUpdate(v);
4226
+ for (const v of live) {
4227
+ const ai = prepAmbiguity(v);
4228
+ phaseUpdate(v, ai);
4229
+ snapshot(v, ai);
4230
+ }
4231
+ } else {
4232
+ for (const v of live) {
4233
+ const ai = prepAmbiguity(v);
4234
+ codeUpdate(v);
4235
+ phaseUpdate(v, ai);
4236
+ snapshot(v, ai);
4237
+ }
3821
4238
  }
3822
4239
  let sumPhaseResSq = 0;
3823
4240
  let nPhaseRes = 0;
@@ -3828,7 +4245,7 @@ var PppEngine = class {
3828
4245
  if (ai === void 0) continue;
3829
4246
  const sys = v.prn[0];
3830
4247
  const ci = clockOf(sys);
3831
- const commonPh = v.range + x[ci] - v.satClkM + v.tropoFixed + x[IDX_ZWD] * v.mw;
4248
+ const commonPh = rangeNow(v) + x[ci] - v.satClkM + v.tropoFixed + x[IDX_ZWD] * v.mw;
3832
4249
  const innov = v.lIf - (commonPh + v.windupM + x[ai]);
3833
4250
  if (Math.abs(innov) >= 0.15) continue;
3834
4251
  const arr = bySysRes.get(sys);
@@ -3852,6 +4269,8 @@ var PppEngine = class {
3852
4269
  flushArc(prn);
3853
4270
  }
3854
4271
  }
4272
+ this.tryFix(epoch.timeMs, vis);
4273
+ if (this.lastFix) this.fixedEpochs++;
3855
4274
  const pos = [x[IDX_X], x[IDX_Y], x[IDX_Z]];
3856
4275
  let enu = null;
3857
4276
  let error3d = null;
@@ -3887,12 +4306,75 @@ var PppEngine = class {
3887
4306
  nSats,
3888
4307
  ztdWet: x[IDX_ZWD],
3889
4308
  ztdHydrostatic: epochZhd,
3890
- phaseResRms: nPhaseRes > 0 ? Math.sqrt(sumPhaseResSq / nPhaseRes) : 0
4309
+ phaseResRms: nPhaseRes > 0 ? Math.sqrt(sumPhaseResSq / nPhaseRes) : 0,
4310
+ nFixed: this.lastFix?.nFixed ?? 0,
4311
+ fixedPosition: this.lastFix?.position ?? null
3891
4312
  };
3892
4313
  series.push(result);
3893
4314
  this.ei = ei + 1;
3894
4315
  return result;
3895
4316
  }
4317
+ /**
4318
+ * PPP-AR. Every `everyS`, resolve the converged ambiguities of the
4319
+ * satellites in view (between satellites of one constellation) and, when
4320
+ * the fix passes the ratio, success-rate and χ² tests, condition the
4321
+ * float position on it ({@link fixPppPosition}). The float filter itself
4322
+ * is never constrained — a wrong fix can't contaminate later epochs, and
4323
+ * the next attempt starts clean. Between attempts the last fix carries
4324
+ * over (static) or is dropped (kinematic).
4325
+ */
4326
+ tryFix(tMs, vis) {
4327
+ const ar = this.opts.ar;
4328
+ if (!ar || !this.source.phaseBias) return;
4329
+ const cfg = ar === true ? {} : ar;
4330
+ if (tMs - this.lastArMs < (cfg.everyS ?? 120) * 1e3) {
4331
+ if (this.kinematic) this.lastFix = null;
4332
+ return;
4333
+ }
4334
+ this.lastArMs = tMs;
4335
+ this.lastFix = null;
4336
+ const { x, P, ambIdx, arcSnap, mwState } = this;
4337
+ const minArcMs = (cfg.minArcS ?? 600) * 1e3;
4338
+ const ambiguities = [];
4339
+ for (const v of vis) {
4340
+ const index = ambIdx.get(v.prn);
4341
+ const snap = arcSnap.get(v.prn);
4342
+ const mw = mwState.get(v.prn);
4343
+ if (index === void 0 || !snap || !mw) continue;
4344
+ if (tMs - snap.startMs < minArcMs || snap.nEpochs < 20) continue;
4345
+ ambiguities.push({
4346
+ prn: v.prn,
4347
+ index,
4348
+ aIF: x[index],
4349
+ mwCyc: mw.mean,
4350
+ f1: v.f1,
4351
+ f2: v.f2,
4352
+ elevDeg: v.elRad * 180 / Math.PI
4353
+ });
4354
+ }
4355
+ const fx = fixPppPosition(
4356
+ {
4357
+ position: [x[IDX_X], x[IDX_Y], x[IDX_Z]],
4358
+ covariance: P,
4359
+ ambiguities
4360
+ },
4361
+ NO_FCB,
4362
+ {
4363
+ ratioThreshold: cfg.ratio ?? 3,
4364
+ wlThreshold: cfg.wlThreshold ?? 0.15,
4365
+ minSuccessRate: cfg.successRate ?? 0.999,
4366
+ minFixed: cfg.minFixed ?? 5,
4367
+ maxChi2: cfg.maxChi2 ?? 20
4368
+ }
4369
+ );
4370
+ if (!fx.fixed) return;
4371
+ this.lastFix = {
4372
+ position: fx.position,
4373
+ nFixed: fx.nFixed,
4374
+ ratio: fx.ratio
4375
+ };
4376
+ this.firstFixMs ??= tMs;
4377
+ }
3896
4378
  /** Finalise the session: capture the AR state, flush open arcs, assemble. */
3897
4379
  solution() {
3898
4380
  const { opts, x, P, ambIdx, arcSnap, mwState, arcs, series, truth } = this;
@@ -3925,7 +4407,12 @@ var PppEngine = class {
3925
4407
  x[IDX_Y],
3926
4408
  x[IDX_Z]
3927
4409
  ];
3928
- const llh = ecefToGeodetic(finalPos[0], finalPos[1], finalPos[2]);
4410
+ const geo = ecefToGeodetic(finalPos[0], finalPos[1], finalPos[2]);
4411
+ const llh = [
4412
+ geo[0] * 180 / Math.PI,
4413
+ geo[1] * 180 / Math.PI,
4414
+ geo[2]
4415
+ ];
3929
4416
  const finalError3d = truth ? Math.hypot(
3930
4417
  finalPos[0] - truth[0],
3931
4418
  finalPos[1] - truth[1],
@@ -3941,218 +4428,25 @@ var PppEngine = class {
3941
4428
  convergenceSec: this.convergenceSec,
3942
4429
  finalError3d,
3943
4430
  arcs,
3944
- finalState
4431
+ finalState,
4432
+ ...opts.ar && this.source.phaseBias ? {
4433
+ ar: {
4434
+ firstFixSec: this.firstFixMs === null || !series.length ? null : (this.firstFixMs - series[0].timeMs) / 1e3,
4435
+ nFixedEnd: this.lastFix?.nFixed ?? 0,
4436
+ position: this.lastFix?.position ?? null,
4437
+ fixedEpochShare: series.length ? this.fixedEpochs / series.length : 0
4438
+ }
4439
+ } : {}
3945
4440
  };
3946
4441
  }
3947
4442
  };
3948
4443
  function solvePpp(epochs, sp3OrSource, opts) {
3949
- const source = "satState" in sp3OrSource ? sp3OrSource : new Sp3EphemerisSource(sp3OrSource, opts.clk);
4444
+ const source = "satState" in sp3OrSource ? sp3OrSource : new Sp3EphemerisSource(sp3OrSource, opts.clk, opts.osb);
3950
4445
  const eng = new PppEngine(opts, source);
3951
4446
  for (const epoch of epochs) eng.process(epoch);
3952
4447
  return eng.solution();
3953
4448
  }
3954
4449
 
3955
- // src/positioning/ppp-ar.ts
3956
- function wlWavelength(f1, f2) {
3957
- return C_LIGHT / (f1 - f2);
3958
- }
3959
- function nlWavelength(f1, f2) {
3960
- return C_LIGHT / (f1 + f2);
3961
- }
3962
- function resolvePppAmbiguities(sats, Qaif, opts = {}) {
3963
- const wlThreshold = opts.wlThreshold ?? 0.25;
3964
- const ratioThreshold = opts.ratioThreshold ?? 3;
3965
- const kept = [];
3966
- const nWl = [];
3967
- const rejectedWl = [];
3968
- for (let i = 0; i < sats.length; i++) {
3969
- const s = sats[i];
3970
- const wlFloat = s.mwCyc - (s.wlBiasCyc ?? 0);
3971
- const r = Math.round(wlFloat);
3972
- if (Math.abs(wlFloat - r) <= wlThreshold) {
3973
- kept.push(i);
3974
- nWl.push(r);
3975
- } else {
3976
- rejectedWl.push(s.prn);
3977
- }
3978
- }
3979
- if (kept.length < 2) {
3980
- return {
3981
- fixed: false,
3982
- ratio: 0,
3983
- refPrn: null,
3984
- sats: [],
3985
- rejectedWl
3986
- };
3987
- }
3988
- const n1Float = [];
3989
- const lamNl = [];
3990
- for (let k = 0; k < kept.length; k++) {
3991
- const s = sats[kept[k]];
3992
- const lNl = nlWavelength(s.f1, s.f2);
3993
- lamNl.push(lNl);
3994
- const n1 = s.aIF / lNl - s.f2 / (s.f1 - s.f2) * nWl[k] - (s.nlBiasCyc ?? 0);
3995
- n1Float.push(n1);
3996
- }
3997
- let ref = 0;
3998
- for (let k = 1; k < kept.length; k++) {
3999
- const e = sats[kept[k]].elevDeg ?? -1;
4000
- if (e > (sats[kept[ref]].elevDeg ?? -1)) ref = k;
4001
- }
4002
- const m = kept.length - 1;
4003
- const sd = new Float64Array(m);
4004
- const Qsd = new Float64Array(m * m);
4005
- const others = [];
4006
- for (let k = 0; k < kept.length; k++) if (k !== ref) others.push(k);
4007
- const n = sats.length;
4008
- const qN1 = (ka, kb) => Qaif[kept[ka] * n + kept[kb]] / (lamNl[ka] * lamNl[kb]);
4009
- for (let a = 0; a < m; a++) {
4010
- const ka = others[a];
4011
- sd[a] = n1Float[ka] - n1Float[ref];
4012
- for (let b = 0; b < m; b++) {
4013
- const kb = others[b];
4014
- Qsd[a * m + b] = qN1(ka, kb) - qN1(ka, ref) - qN1(ref, kb) + qN1(ref, ref);
4015
- }
4016
- }
4017
- const res = lambdaSearch(sd, Qsd, m, 2);
4018
- const ratio = res ? Math.min(res.ratio, 999.9) : 0;
4019
- const fixed = !!res && ratio >= ratioThreshold;
4020
- const refN1 = Math.round(n1Float[ref]);
4021
- const sdFix = res?.candidates[0];
4022
- const outSats = [];
4023
- for (let k = 0; k < kept.length; k++) {
4024
- const s = sats[kept[k]];
4025
- let n1;
4026
- if (k === ref) {
4027
- n1 = refN1;
4028
- } else {
4029
- const a = others.indexOf(k);
4030
- n1 = fixed && sdFix ? refN1 + Math.round(sdFix[a]) : Math.round(n1Float[k]);
4031
- }
4032
- const lNl = lamNl[k];
4033
- const aIF = lNl * n1 + lNl * (s.f2 / (s.f1 - s.f2)) * nWl[k];
4034
- outSats.push({ prn: s.prn, nWl: nWl[k], n1, aIF });
4035
- }
4036
- return {
4037
- fixed,
4038
- ratio,
4039
- refPrn: sats[kept[ref]].prn,
4040
- sats: outSats,
4041
- rejectedWl
4042
- };
4043
- }
4044
- function solveSpd(A, b) {
4045
- const n = b.length;
4046
- const L = Array.from({ length: n }, () => new Array(n).fill(0));
4047
- for (let i = 0; i < n; i++) {
4048
- for (let j = 0; j <= i; j++) {
4049
- let s = A[i][j];
4050
- for (let k = 0; k < j; k++) s -= L[i][k] * L[j][k];
4051
- if (i === j) {
4052
- if (s <= 0) return null;
4053
- L[i][j] = Math.sqrt(s);
4054
- } else {
4055
- L[i][j] = s / L[j][j];
4056
- }
4057
- }
4058
- }
4059
- const y = new Array(n).fill(0);
4060
- for (let i = 0; i < n; i++) {
4061
- let s = b[i];
4062
- for (let k = 0; k < i; k++) s -= L[i][k] * y[k];
4063
- y[i] = s / L[i][i];
4064
- }
4065
- const x = new Array(n).fill(0);
4066
- for (let i = n - 1; i >= 0; i--) {
4067
- let s = y[i];
4068
- for (let k = i + 1; k < n; k++) s -= L[k][i] * x[k];
4069
- x[i] = s / L[i][i];
4070
- }
4071
- return x;
4072
- }
4073
- function fixPppPosition(state, fcb, opts = {}) {
4074
- const floatPos = state.position;
4075
- const noFix = {
4076
- fixed: false,
4077
- position: floatPos,
4078
- floatPosition: floatPos,
4079
- shift: [0, 0, 0],
4080
- nFixed: 0,
4081
- ratio: 0,
4082
- refPrn: null
4083
- };
4084
- const amb = state.ambiguities;
4085
- if (amb.length < 3) return noFix;
4086
- const sats = amb.map((a) => ({
4087
- prn: a.prn,
4088
- aIF: a.aIF,
4089
- mwCyc: a.mwCyc,
4090
- f1: a.f1,
4091
- f2: a.f2,
4092
- wlBiasCyc: fcb.satWlFcb.get(a.prn) ?? 0,
4093
- nlBiasCyc: fcb.satNlFcb.get(a.prn) ?? 0,
4094
- elevDeg: a.elevDeg
4095
- }));
4096
- const n = sats.length;
4097
- const Qaif = new Float64Array(n * n);
4098
- for (let i = 0; i < n; i++)
4099
- for (let j = 0; j < n; j++)
4100
- Qaif[i * n + j] = state.covariance[amb[i].index][amb[j].index];
4101
- const res = resolvePppAmbiguities(sats, Qaif, opts);
4102
- if (!res.fixed || res.refPrn === null) {
4103
- return { ...noFix, ratio: res.ratio, refPrn: res.refPrn };
4104
- }
4105
- const idxOfPrn = new Map(amb.map((a) => [a.prn, a.index]));
4106
- const floatOfPrn = new Map(amb.map((a) => [a.prn, a.aIF]));
4107
- const kept = res.sats.filter((s) => idxOfPrn.has(s.prn));
4108
- const refPos = kept.findIndex((s) => s.prn === res.refPrn);
4109
- if (refPos < 0 || kept.length < 2) {
4110
- return { ...noFix, ratio: res.ratio, refPrn: res.refPrn };
4111
- }
4112
- const ambIdx = kept.map((s) => idxOfPrn.get(s.prn));
4113
- const aFloat = kept.map((s) => floatOfPrn.get(s.prn));
4114
- const aFixed = kept.map((s) => s.aIF);
4115
- const others = kept.map((_, i) => i).filter((i) => i !== refPos);
4116
- const m = others.length;
4117
- const P = state.covariance;
4118
- const qAA = (i, j) => P[ambIdx[i]][ambIdx[j]];
4119
- const qXA = (r, i) => P[r][ambIdx[i]];
4120
- const Qzz = Array.from({ length: m }, () => new Array(m).fill(0));
4121
- const z = new Array(m).fill(0);
4122
- for (let a = 0; a < m; a++) {
4123
- const ia = others[a];
4124
- z[a] = aFloat[ia] - aFloat[refPos] - (aFixed[ia] - aFixed[refPos]);
4125
- for (let b = 0; b < m; b++) {
4126
- const ib = others[b];
4127
- Qzz[a][b] = qAA(ia, ib) - qAA(ia, refPos) - qAA(refPos, ib) + qAA(refPos, refPos);
4128
- }
4129
- }
4130
- const Qxz = [0, 1, 2].map(
4131
- (r) => others.map((ia) => qXA(r, ia) - qXA(r, refPos))
4132
- );
4133
- const y = solveSpd(Qzz, z);
4134
- if (!y) return { ...noFix, ratio: res.ratio, refPrn: res.refPrn };
4135
- const shift = [0, 0, 0];
4136
- for (let r = 0; r < 3; r++) {
4137
- let s = 0;
4138
- for (let a = 0; a < m; a++) s += Qxz[r][a] * y[a];
4139
- shift[r] = -s;
4140
- }
4141
- return {
4142
- fixed: true,
4143
- position: [
4144
- floatPos[0] + shift[0],
4145
- floatPos[1] + shift[1],
4146
- floatPos[2] + shift[2]
4147
- ],
4148
- floatPosition: floatPos,
4149
- shift,
4150
- nFixed: m,
4151
- ratio: res.ratio,
4152
- refPrn: res.refPrn
4153
- };
4154
- }
4155
-
4156
4450
  // src/positioning/ppp-fcb.ts
4157
4451
  function extractWidelaneArcs(station, epochs, opts = {}) {
4158
4452
  const slipCyc = opts.slipCyc ?? 4;