gnss-js 2.11.0 → 2.13.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/README.md +19 -1
- package/dist/analysis.cjs +24 -25
- package/dist/analysis.js +2 -2
- package/dist/android.cjs +338 -0
- package/dist/android.d.cts +68 -0
- package/dist/android.d.ts +68 -0
- package/dist/android.js +311 -0
- package/dist/{clk-DGVBGRzA.d.cts → bias-sinex-OGMw9xEZ.d.cts} +66 -1
- package/dist/{clk-DGVBGRzA.d.ts → bias-sinex-OGMw9xEZ.d.ts} +66 -1
- package/dist/{chunk-MDO77CCC.js → chunk-2DJ4XENO.js} +45 -0
- package/dist/{chunk-IQHQHUOB.js → chunk-2KQMCJ5Z.js} +5 -3
- package/dist/chunk-E3Y6YKQM.js +65 -0
- package/dist/{chunk-3U5AX7PY.js → chunk-FEABDV5P.js} +24 -25
- package/dist/{chunk-RFPR4LKM.js → chunk-HWHD5E6V.js} +19 -75
- package/dist/{chunk-KUSUN7DP.js → chunk-MFCPYUEI.js} +1 -1
- package/dist/{chunk-SSWFX3P3.js → chunk-NIJ6WFQJ.js} +6 -321
- package/dist/{chunk-UMINQTMK.js → chunk-RWFJEVLJ.js} +589 -295
- package/dist/chunk-TWGZTRBL.js +323 -0
- package/dist/index.cjs +655 -316
- package/dist/index.d.cts +2 -2
- package/dist/index.d.ts +2 -2
- package/dist/index.js +14 -9
- package/dist/osnma.cjs +1336 -0
- package/dist/osnma.d.cts +273 -0
- package/dist/osnma.d.ts +273 -0
- package/dist/osnma.js +1216 -0
- package/dist/positioning.cjs +585 -291
- package/dist/positioning.d.cts +114 -4
- package/dist/positioning.d.ts +114 -4
- package/dist/positioning.js +1 -1
- package/dist/rinex.cjs +70 -25
- package/dist/rinex.d.cts +2 -50
- package/dist/rinex.d.ts +2 -50
- package/dist/rinex.js +4 -2
- package/dist/sbf.js +7 -4
- package/dist/ubx.js +3 -2
- package/package.json +11 -1
package/dist/positioning.cjs
CHANGED
|
@@ -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
|
-
|
|
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(
|
|
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
|
-
|
|
874
|
-
|
|
875
|
-
|
|
876
|
-
|
|
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
|
-
|
|
888
|
-
|
|
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
|
|
987
|
+
const west = enuToEcef([-1, 0, 0], latRad, lonRad);
|
|
965
988
|
const kdotn = dot(k, north);
|
|
966
|
-
const
|
|
989
|
+
const kcrossW = cross(k, west);
|
|
967
990
|
const dR = [
|
|
968
|
-
north[0] - k[0] * kdotn +
|
|
969
|
-
north[1] - k[1] * kdotn +
|
|
970
|
-
north[2] - k[2] * kdotn +
|
|
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
|
-
|
|
1098
|
-
|
|
1099
|
-
|
|
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 [
|
|
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
|
-
|
|
1200
|
-
|
|
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 [
|
|
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 [
|
|
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
|
|
3662
|
-
if (
|
|
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
|
|
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
|
-
|
|
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
|
|
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
|
-
|
|
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
|
-
|
|
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
|
-
|
|
3772
|
-
|
|
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
|
-
|
|
3798
|
-
|
|
3799
|
-
|
|
3800
|
-
|
|
3801
|
-
|
|
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
|
|
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
|
|
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;
|