speedandcurrent 2.3.4 → 2.4.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 +74 -45
- package/correctionTable.js +296 -66
- package/docs/correction-table-learning.md +33 -0
- package/docs/correction-table-v1.schema.json +219 -0
- package/docs/correction-table-v2.schema.json +134 -0
- package/docs/correction-table.schema.json +219 -0
- package/index.js +285 -127
- package/package.json +2 -2
- package/public/TableRenderer.js +89 -14
- package/public/app.js +126 -78
- package/public/index.html +14 -6
- package/public/main.css +19 -5
package/correctionTable.js
CHANGED
|
@@ -1,6 +1,20 @@
|
|
|
1
1
|
const { Table2D} = require('signalkutilities');
|
|
2
2
|
const { KalmanFilter, State } = require('kalman-filter');
|
|
3
3
|
|
|
4
|
+
const MAX_INTERPOLATION_DISTANCE = 2.5;
|
|
5
|
+
const MIN_CELL_INDEX = 0;
|
|
6
|
+
const Q_RECALCULATION_INTERVAL = 600;
|
|
7
|
+
const MIN_Q_PAIR_COUNT = 3;
|
|
8
|
+
const DEFAULT_Q = 0.002;
|
|
9
|
+
const COVARIANCE_FLOOR = 1e-9;
|
|
10
|
+
const MAX_AGING_SECONDS = 90 * 24 * 60 * 60;
|
|
11
|
+
const SECONDS_PER_MONTH = 30 * 24 * 60 * 60;
|
|
12
|
+
const KNOT_TO_MPS = 0.5144444444444444;
|
|
13
|
+
const DEFAULT_CORRECTION_DRIFT_RATE = 0.3;
|
|
14
|
+
const DEFAULT_PROCESS_NOISE_RATE = (DEFAULT_CORRECTION_DRIFT_RATE * KNOT_TO_MPS) ** 2 / SECONDS_PER_MONTH;
|
|
15
|
+
const OBSERVATION_STATE_PROJECTION = [[1, 0], [0, 1]]; // observation matrix H
|
|
16
|
+
const DYNAMIC_TRANSITION = [[1, 0], [0, 1]]; // state transition matrix F
|
|
17
|
+
|
|
4
18
|
// Module-level helpers — avoids creating new Function objects on every Kalman update call
|
|
5
19
|
function _rotateValue(cos, sin, vector) {
|
|
6
20
|
return [
|
|
@@ -11,8 +25,49 @@ function _rotateValue(cos, sin, vector) {
|
|
|
11
25
|
|
|
12
26
|
function _rotateVariance(cos, sin, vector) {
|
|
13
27
|
return [
|
|
14
|
-
[vector[0] * cos ** 2 + vector[1] * sin ** 2, (vector[
|
|
15
|
-
[(vector[
|
|
28
|
+
[vector[0] * cos ** 2 + vector[1] * sin ** 2, (vector[1] - vector[0]) * cos * sin],
|
|
29
|
+
[(vector[1] - vector[0]) * cos * sin, vector[0] * sin ** 2 + vector[1] * cos ** 2]
|
|
30
|
+
];
|
|
31
|
+
}
|
|
32
|
+
|
|
33
|
+
function _addHeadingVariance(covariance, groundVector, currentVector, headingVariance) {
|
|
34
|
+
const variance = Number.isFinite(headingVariance) && headingVariance >= 0 ? headingVariance : 0;
|
|
35
|
+
if (variance === 0) return covariance;
|
|
36
|
+
|
|
37
|
+
const ux = groundVector[0] - currentVector[0];
|
|
38
|
+
const uy = groundVector[1] - currentVector[1];
|
|
39
|
+
const jx = uy;
|
|
40
|
+
const jy = -ux;
|
|
41
|
+
return [
|
|
42
|
+
[covariance[0][0] + jx * variance * jx, covariance[0][1] + jx * variance * jy],
|
|
43
|
+
[covariance[1][0] + jy * variance * jx, covariance[1][1] + jy * variance * jy]
|
|
44
|
+
];
|
|
45
|
+
}
|
|
46
|
+
|
|
47
|
+
function _median(values) {
|
|
48
|
+
const sorted = values.filter(Number.isFinite).sort((a, b) => a - b);
|
|
49
|
+
if (sorted.length === 0) return null;
|
|
50
|
+
const middle = Math.floor(sorted.length / 2);
|
|
51
|
+
return sorted.length % 2 === 0
|
|
52
|
+
? (sorted[middle - 1] + sorted[middle]) / 2
|
|
53
|
+
: sorted[middle];
|
|
54
|
+
}
|
|
55
|
+
|
|
56
|
+
function _symmetricCovariance(covariance, diagonalInflation = 0) {
|
|
57
|
+
if (!Array.isArray(covariance) || !Array.isArray(covariance[0]) || !Array.isArray(covariance[1])) return null;
|
|
58
|
+
const xx = covariance[0][0] + diagonalInflation;
|
|
59
|
+
const yy = covariance[1][1] + diagonalInflation;
|
|
60
|
+
const xy = (covariance[0][1] + covariance[1][0]) / 2;
|
|
61
|
+
if (![xx, xy, yy].every(Number.isFinite)) return null;
|
|
62
|
+
return [[Math.max(xx, COVARIANCE_FLOOR), xy], [xy, Math.max(yy, COVARIANCE_FLOOR)]];
|
|
63
|
+
}
|
|
64
|
+
|
|
65
|
+
function _inverse2(matrix) {
|
|
66
|
+
const det = matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0];
|
|
67
|
+
if (!Number.isFinite(det) || det <= COVARIANCE_FLOOR ** 2) return null;
|
|
68
|
+
return [
|
|
69
|
+
[matrix[1][1] / det, -matrix[0][1] / det],
|
|
70
|
+
[-matrix[1][0] / det, matrix[0][0] / det]
|
|
16
71
|
];
|
|
17
72
|
}
|
|
18
73
|
|
|
@@ -23,9 +78,10 @@ class CorrectionTable extends Table2D{
|
|
|
23
78
|
|
|
24
79
|
|
|
25
80
|
|
|
26
|
-
static fromJSON(data,
|
|
27
|
-
const table = new CorrectionTable(data.id, data.row, data.col,
|
|
28
|
-
table.table = data.table.map(row => row.map(cellData => CorrectionEstimator.fromJSON(cellData,
|
|
81
|
+
static fromJSON(data, processNoiseRate = DEFAULT_PROCESS_NOISE_RATE) {
|
|
82
|
+
const table = new CorrectionTable(data.id, data.row, data.col, processNoiseRate);
|
|
83
|
+
table.table = data.table.map(row => row.map(cellData => CorrectionEstimator.fromJSON(cellData, processNoiseRate)));
|
|
84
|
+
table.calculateQ();
|
|
29
85
|
return table;
|
|
30
86
|
}
|
|
31
87
|
|
|
@@ -38,12 +94,12 @@ class CorrectionTable extends Table2D{
|
|
|
38
94
|
* @param {CorrectionTable} oldTable - Source table to sample from
|
|
39
95
|
* @param {{min:number,max:number,step:number}} newRow - New speed axis definition (SI units)
|
|
40
96
|
* @param {{min:number,max:number,step:number}} newCol - New heel axis definition (SI units)
|
|
41
|
-
|
|
97
|
+
* @param {number} [processNoiseRate=DEFAULT_PROCESS_NOISE_RATE] - SI process-noise rate
|
|
42
98
|
* @param {number} [varianceFloor=1e-4] - Floor applied to cov[0][0] and cov[1][1]
|
|
43
99
|
* @returns {CorrectionTable}
|
|
44
100
|
*/
|
|
45
|
-
static resample(oldTable, newRow, newCol,
|
|
46
|
-
const newTable = new CorrectionTable(oldTable.id, newRow, newCol,
|
|
101
|
+
static resample(oldTable, newRow, newCol, processNoiseRate = DEFAULT_PROCESS_NOISE_RATE, varianceFloor = 1e-4) {
|
|
102
|
+
const newTable = new CorrectionTable(oldTable.id, newRow, newCol, processNoiseRate);
|
|
47
103
|
|
|
48
104
|
const nRows = Math.round((newRow.max - newRow.min) / newRow.step) + 1;
|
|
49
105
|
const nCols = Math.round((newCol.max - newCol.min) / newCol.step) + 1;
|
|
@@ -58,7 +114,8 @@ class CorrectionTable extends Table2D{
|
|
|
58
114
|
const mean = [[(correction?.x ?? 0)], [(correction?.y ?? 0)]];
|
|
59
115
|
const covXX = Math.max(Number.isFinite(variance?.x) ? variance.x : 0, varianceFloor);
|
|
60
116
|
const covYY = Math.max(Number.isFinite(variance?.y) ? variance.y : 0, varianceFloor);
|
|
61
|
-
const
|
|
117
|
+
const covXY = Number.isFinite(variance?.covariance?.[0]?.[1]) ? variance.covariance[0][1] : 0;
|
|
118
|
+
const covariance = [[covXX, covXY], [covXY, covYY]];
|
|
62
119
|
|
|
63
120
|
// Decide initialization index based on coverage/support from old table
|
|
64
121
|
const inBounds = (
|
|
@@ -89,64 +146,142 @@ class CorrectionTable extends Table2D{
|
|
|
89
146
|
/**
|
|
90
147
|
* Convenience to resample from serialized JSON table data
|
|
91
148
|
*/
|
|
92
|
-
static resampleFromJSON(data, newRow, newCol,
|
|
93
|
-
const oldTable = CorrectionTable.fromJSON(data,
|
|
94
|
-
return CorrectionTable.resample(oldTable, newRow, newCol,
|
|
149
|
+
static resampleFromJSON(data, newRow, newCol, processNoiseRate = DEFAULT_PROCESS_NOISE_RATE, varianceFloor = 1e-4) {
|
|
150
|
+
const oldTable = CorrectionTable.fromJSON(data, processNoiseRate);
|
|
151
|
+
return CorrectionTable.resample(oldTable, newRow, newCol, processNoiseRate, varianceFloor);
|
|
95
152
|
}
|
|
96
153
|
|
|
97
|
-
constructor(id, row, col,
|
|
98
|
-
super(id, row, col, CorrectionEstimator, CorrectionEstimator.getFilterModel(
|
|
154
|
+
constructor(id, row, col, processNoiseRate = DEFAULT_PROCESS_NOISE_RATE) {
|
|
155
|
+
super(id, row, col, CorrectionEstimator, CorrectionEstimator.getFilterModel(processNoiseRate));
|
|
156
|
+
// Table2D persists the constructor param verbatim as `this.parameters`. The live filter model
|
|
157
|
+
// carries a runtime callback for observation.covariance, which JSON.stringify would silently
|
|
158
|
+
// drop. Replace it with a serializable descriptor so persisted files stay honest.
|
|
159
|
+
this.parameters = CorrectionEstimator.getModelDescriptor(processNoiseRate);
|
|
99
160
|
this.lastUpdatedCell = null;
|
|
100
161
|
this.lastUpdateResult = null;
|
|
101
162
|
this.neighbours = [];
|
|
163
|
+
this.q = DEFAULT_Q;
|
|
164
|
+
this.qPairCount = 0;
|
|
165
|
+
this.qEstimate = null;
|
|
166
|
+
this.qSource = 'fallback';
|
|
167
|
+
this.acceptedObservationsSinceQ = 0;
|
|
168
|
+
for (const rowCells of this.table) {
|
|
169
|
+
for (const cell of rowCells) cell.setProcessNoiseRate(processNoiseRate);
|
|
170
|
+
}
|
|
171
|
+
}
|
|
172
|
+
|
|
173
|
+
setProcessNoiseRate(processNoiseRate) {
|
|
174
|
+
this.parameters = CorrectionEstimator.getModelDescriptor(processNoiseRate);
|
|
175
|
+
for (const rowCells of this.table) {
|
|
176
|
+
for (const cell of rowCells) cell.setProcessNoiseRate(processNoiseRate);
|
|
177
|
+
}
|
|
102
178
|
}
|
|
103
179
|
|
|
104
|
-
update(speed, heel, groundSpeed, current, boatSpeed, heading) {
|
|
180
|
+
update(speed, heel, groundSpeed, current, boatSpeed, heading, headingVariance) {
|
|
105
181
|
const cell = this.getCell(speed, heel);
|
|
106
|
-
const accepted = cell?.update(groundSpeed, current, boatSpeed, heading);
|
|
182
|
+
const accepted = cell?.update(groundSpeed, current, boatSpeed, heading, headingVariance);
|
|
107
183
|
this.lastUpdatedCell = cell;
|
|
108
184
|
this.lastUpdateResult = accepted === true ? 'accepted' : 'rejected';
|
|
185
|
+
if (accepted === true) {
|
|
186
|
+
this.acceptedObservationsSinceQ++;
|
|
187
|
+
if (this.acceptedObservationsSinceQ >= Q_RECALCULATION_INTERVAL) {
|
|
188
|
+
this.calculateQ();
|
|
189
|
+
}
|
|
190
|
+
}
|
|
191
|
+
return accepted === true;
|
|
109
192
|
}
|
|
110
193
|
|
|
111
|
-
|
|
112
|
-
|
|
113
|
-
|
|
114
|
-
|
|
115
|
-
|
|
116
|
-
|
|
117
|
-
|
|
118
|
-
|
|
119
|
-
|
|
120
|
-
|
|
121
|
-
|
|
122
|
-
|
|
123
|
-
|
|
124
|
-
|
|
125
|
-
|
|
126
|
-
|
|
127
|
-
|
|
128
|
-
|
|
129
|
-
totalWeight += weight;
|
|
194
|
+
calculateQ() {
|
|
195
|
+
const pairEstimates = [];
|
|
196
|
+
for (let row = 0; row < this.n[0]; row++) {
|
|
197
|
+
for (let col = 0; col < this.n[1]; col++) {
|
|
198
|
+
const first = this.table[row][col];
|
|
199
|
+
if (first.N <= MIN_CELL_INDEX) continue;
|
|
200
|
+
for (const [rowOffset, colOffset] of [[1, 0], [0, 1]]) {
|
|
201
|
+
const second = this.table[row + rowOffset]?.[col + colOffset];
|
|
202
|
+
if (!second || second.N <= MIN_CELL_INDEX) continue;
|
|
203
|
+
const firstCovariance = _symmetricCovariance(first.covariance);
|
|
204
|
+
const secondCovariance = _symmetricCovariance(second.covariance);
|
|
205
|
+
if (!firstCovariance || !secondCovariance) continue;
|
|
206
|
+
const dx = first.x - second.x;
|
|
207
|
+
const dy = first.y - second.y;
|
|
208
|
+
const uncertainty = firstCovariance[0][0] + firstCovariance[1][1]
|
|
209
|
+
+ secondCovariance[0][0] + secondCovariance[1][1];
|
|
210
|
+
pairEstimates.push((dx * dx + dy * dy - uncertainty) / 2);
|
|
211
|
+
}
|
|
130
212
|
}
|
|
131
213
|
}
|
|
132
|
-
|
|
133
|
-
|
|
134
|
-
|
|
214
|
+
|
|
215
|
+
const estimate = _median(pairEstimates);
|
|
216
|
+
this.qPairCount = pairEstimates.length;
|
|
217
|
+
this.qEstimate = Number.isFinite(estimate) ? estimate : null;
|
|
218
|
+
if (pairEstimates.length >= MIN_Q_PAIR_COUNT && estimate > 0) {
|
|
219
|
+
this.q = estimate;
|
|
220
|
+
this.qSource = 'derived';
|
|
221
|
+
} else {
|
|
222
|
+
this.q = DEFAULT_Q;
|
|
223
|
+
this.qSource = 'fallback';
|
|
224
|
+
}
|
|
225
|
+
this.acceptedObservationsSinceQ = 0;
|
|
226
|
+
return this.q;
|
|
227
|
+
}
|
|
228
|
+
|
|
229
|
+
_findEligibleNeighbours(speed, heel) {
|
|
230
|
+
const neighbours = [];
|
|
231
|
+
for (let row = 0; row < this.n[0]; row++) {
|
|
232
|
+
for (let col = 0; col < this.n[1]; col++) {
|
|
233
|
+
const cell = this.table[row][col];
|
|
234
|
+
cell.effectiveVariance = null;
|
|
235
|
+
if (cell.N <= MIN_CELL_INDEX) continue;
|
|
236
|
+
const rowDistance = (this.indexToValue(row, 0) - speed) / this.step[0];
|
|
237
|
+
const colDistance = (this.indexToValue(col, 1) - heel) / this.step[1];
|
|
238
|
+
const dist = Math.sqrt(rowDistance * rowDistance + colDistance * colDistance);
|
|
239
|
+
if (Number.isFinite(dist) && dist <= MAX_INTERPOLATION_DISTANCE) {
|
|
240
|
+
neighbours.push({ cell, row, col, dist, normWeight: 0 });
|
|
241
|
+
}
|
|
135
242
|
}
|
|
136
243
|
}
|
|
244
|
+
return neighbours.sort((a, b) => a.dist - b.dist);
|
|
245
|
+
}
|
|
137
246
|
|
|
138
|
-
|
|
247
|
+
getCorrection(speed, heel) {
|
|
248
|
+
this.neighbours = this._findEligibleNeighbours(speed, heel);
|
|
249
|
+
const emptyResult = { correction: { x: 0, y: 0 }, variance: { x: 0, y: 0, covariance: [[0, 0], [0, 0]] } };
|
|
250
|
+
if (this.neighbours.length === 0) return emptyResult;
|
|
139
251
|
|
|
140
|
-
|
|
141
|
-
|
|
142
|
-
|
|
143
|
-
|
|
144
|
-
|
|
145
|
-
|
|
146
|
-
|
|
147
|
-
|
|
252
|
+
const precision = [[0, 0], [0, 0]];
|
|
253
|
+
const information = [0, 0];
|
|
254
|
+
let totalScalarWeight = 0;
|
|
255
|
+
for (const neighbour of this.neighbours) {
|
|
256
|
+
const effectiveCovariance = _symmetricCovariance(neighbour.cell.covariance, this.q * neighbour.dist ** 2);
|
|
257
|
+
const cellPrecision = effectiveCovariance && _inverse2(effectiveCovariance);
|
|
258
|
+
if (!cellPrecision) continue;
|
|
259
|
+
|
|
260
|
+
neighbour.effectiveCovariance = effectiveCovariance;
|
|
261
|
+
neighbour.cell.effectiveVariance = effectiveCovariance[0][0] + effectiveCovariance[1][1];
|
|
262
|
+
neighbour.scalarWeight = cellPrecision[0][0] + cellPrecision[1][1];
|
|
263
|
+
totalScalarWeight += neighbour.scalarWeight;
|
|
264
|
+
precision[0][0] += cellPrecision[0][0];
|
|
265
|
+
precision[0][1] += cellPrecision[0][1];
|
|
266
|
+
precision[1][0] += cellPrecision[1][0];
|
|
267
|
+
precision[1][1] += cellPrecision[1][1];
|
|
268
|
+
information[0] += cellPrecision[0][0] * neighbour.cell.x + cellPrecision[0][1] * neighbour.cell.y;
|
|
269
|
+
information[1] += cellPrecision[1][0] * neighbour.cell.x + cellPrecision[1][1] * neighbour.cell.y;
|
|
270
|
+
}
|
|
148
271
|
|
|
149
|
-
|
|
272
|
+
const covariance = _inverse2(precision);
|
|
273
|
+
if (!covariance) return emptyResult;
|
|
274
|
+
for (const neighbour of this.neighbours) {
|
|
275
|
+
neighbour.normWeight = totalScalarWeight > 0 && Number.isFinite(neighbour.scalarWeight)
|
|
276
|
+
? neighbour.scalarWeight / totalScalarWeight
|
|
277
|
+
: 0;
|
|
278
|
+
}
|
|
279
|
+
const x = covariance[0][0] * information[0] + covariance[0][1] * information[1];
|
|
280
|
+
const y = covariance[1][0] * information[0] + covariance[1][1] * information[1];
|
|
281
|
+
return {
|
|
282
|
+
correction: { x, y },
|
|
283
|
+
variance: { x: covariance[0][0], y: covariance[1][1], covariance }
|
|
284
|
+
};
|
|
150
285
|
}
|
|
151
286
|
|
|
152
287
|
report() {
|
|
@@ -178,6 +313,9 @@ class CorrectionTable extends Table2D{
|
|
|
178
313
|
cellReport.factor = factor;
|
|
179
314
|
cellReport.leeway = leeway;
|
|
180
315
|
cellReport.trace = trace;
|
|
316
|
+
cellReport.effectiveVariance = Number.isFinite(correction.effectiveVariance)
|
|
317
|
+
? correction.effectiveVariance
|
|
318
|
+
: null;
|
|
181
319
|
cellReport.speedBin = speedBin;
|
|
182
320
|
cellReport.heelBin = heelBin;
|
|
183
321
|
// Mark selected if this is the last updated cell
|
|
@@ -194,10 +332,26 @@ class CorrectionTable extends Table2D{
|
|
|
194
332
|
})
|
|
195
333
|
),
|
|
196
334
|
displayAttributes: this.displayAttributes,
|
|
335
|
+
q: this.q,
|
|
336
|
+
qPairCount: this.qPairCount,
|
|
337
|
+
qEstimate: this.qEstimate,
|
|
338
|
+
qSource: this.qSource,
|
|
197
339
|
lastUpdateResult: this.lastUpdateResult ?? null
|
|
198
340
|
};
|
|
199
341
|
}
|
|
200
342
|
|
|
343
|
+
toJSON() {
|
|
344
|
+
return {
|
|
345
|
+
schemaVersion: 2,
|
|
346
|
+
id: this.id,
|
|
347
|
+
row: { min: this.min[0], max: this.max[0], step: this.step[0] },
|
|
348
|
+
col: { min: this.min[1], max: this.max[1], step: this.step[1] },
|
|
349
|
+
parameters: this.parameters,
|
|
350
|
+
table: this.table.map(row => row.map(cell => cell.toJSON())),
|
|
351
|
+
displayAttributes: this.displayAttributes
|
|
352
|
+
};
|
|
353
|
+
}
|
|
354
|
+
|
|
201
355
|
}
|
|
202
356
|
|
|
203
357
|
class CorrectionEstimator {
|
|
@@ -205,22 +359,45 @@ class CorrectionEstimator {
|
|
|
205
359
|
* Represents a Kalman correction at a cell in a correction table
|
|
206
360
|
*/
|
|
207
361
|
|
|
208
|
-
static fromJSON(data,
|
|
209
|
-
const filterModel = CorrectionEstimator.getFilterModel(
|
|
362
|
+
static fromJSON(data, processNoiseRate = DEFAULT_PROCESS_NOISE_RATE) {
|
|
363
|
+
const filterModel = CorrectionEstimator.getFilterModel(processNoiseRate);
|
|
210
364
|
const estimator = new CorrectionEstimator(filterModel, data.state);
|
|
365
|
+
estimator.setProcessNoiseRate(processNoiseRate);
|
|
366
|
+
estimator.lastAcceptedAt = Number.isFinite(data.lastAcceptedAt) ? data.lastAcceptedAt : null;
|
|
211
367
|
return estimator;
|
|
212
368
|
}
|
|
213
369
|
|
|
214
|
-
static getFilterModel(
|
|
370
|
+
static getFilterModel(processNoiseRate = DEFAULT_PROCESS_NOISE_RATE) {
|
|
371
|
+
return {
|
|
372
|
+
observation: {
|
|
373
|
+
stateProjection: OBSERVATION_STATE_PROJECTION,
|
|
374
|
+
// Measurement noise R is computed per observation in update() from live sensor
|
|
375
|
+
// variances (see observationCovariance below) and supplied to the filter through
|
|
376
|
+
// the `observationCovariance` option, rather than being a fixed matrix.
|
|
377
|
+
covariance: (options) => options.observationCovariance,
|
|
378
|
+
dimension: 2
|
|
379
|
+
},
|
|
380
|
+
dynamic: {
|
|
381
|
+
transition: DYNAMIC_TRANSITION, // state transition matrix F
|
|
382
|
+
covariance: [0, 0], // aging applies process noise explicitly from elapsed time
|
|
383
|
+
}
|
|
384
|
+
};
|
|
385
|
+
}
|
|
386
|
+
|
|
387
|
+
/**
|
|
388
|
+
* Serializable descriptor of the filter model, stored in table.parameters for persistence.
|
|
389
|
+
* Unlike getFilterModel(), this never contains functions.
|
|
390
|
+
*/
|
|
391
|
+
static getModelDescriptor(processNoiseRate = DEFAULT_PROCESS_NOISE_RATE) {
|
|
215
392
|
return {
|
|
216
393
|
observation: {
|
|
217
|
-
stateProjection:
|
|
218
|
-
covariance:
|
|
394
|
+
stateProjection: OBSERVATION_STATE_PROJECTION,
|
|
395
|
+
covariance: 'per-observation', // actual R supplied at runtime by CorrectionEstimator.update()
|
|
219
396
|
dimension: 2
|
|
220
397
|
},
|
|
221
398
|
dynamic: {
|
|
222
|
-
transition:
|
|
223
|
-
covariance: [
|
|
399
|
+
transition: DYNAMIC_TRANSITION,
|
|
400
|
+
covariance: [processNoiseRate, processNoiseRate]
|
|
224
401
|
}
|
|
225
402
|
};
|
|
226
403
|
}
|
|
@@ -228,12 +405,45 @@ class CorrectionEstimator {
|
|
|
228
405
|
constructor(filterModel, initialState) {
|
|
229
406
|
this.filter = new KalmanFilter(filterModel);
|
|
230
407
|
this.filterState = null;
|
|
408
|
+
this.effectiveVariance = null;
|
|
409
|
+
this.processNoiseRate = Number.isFinite(filterModel?.dynamic?.covariance?.[0])
|
|
410
|
+
? filterModel.dynamic.covariance[0]
|
|
411
|
+
: DEFAULT_PROCESS_NOISE_RATE;
|
|
412
|
+
this.lastAcceptedAt = null;
|
|
231
413
|
if (initialState != null) {
|
|
232
414
|
this.filterState = new State(initialState);
|
|
233
415
|
}
|
|
234
416
|
}
|
|
417
|
+
|
|
418
|
+
setProcessNoiseRate(processNoiseRate = DEFAULT_PROCESS_NOISE_RATE) {
|
|
419
|
+
this.processNoiseRate = Number.isFinite(processNoiseRate) && processNoiseRate >= 0
|
|
420
|
+
? processNoiseRate
|
|
421
|
+
: DEFAULT_PROCESS_NOISE_RATE;
|
|
422
|
+
return this;
|
|
423
|
+
}
|
|
424
|
+
|
|
425
|
+
getAgedCovariance(now = Date.now()) {
|
|
426
|
+
if (this.filterState == null) return null;
|
|
427
|
+
const covariance = this.filterState.covariance.map(row => row.slice());
|
|
428
|
+
if (!Number.isFinite(this.lastAcceptedAt) || !Number.isFinite(now)) return covariance;
|
|
429
|
+
const elapsedSeconds = Math.max(0, (now - this.lastAcceptedAt) / 1000);
|
|
430
|
+
const effectiveDt = Math.min(elapsedSeconds, MAX_AGING_SECONDS);
|
|
431
|
+
const agedVariance = this.processNoiseRate * effectiveDt;
|
|
432
|
+
covariance[0][0] += agedVariance;
|
|
433
|
+
covariance[1][1] += agedVariance;
|
|
434
|
+
return covariance;
|
|
435
|
+
}
|
|
436
|
+
|
|
437
|
+
_agedState(now) {
|
|
438
|
+
if (this.filterState == null) return null;
|
|
439
|
+
return new State({
|
|
440
|
+
mean: this.filterState.mean.map(row => row.slice()),
|
|
441
|
+
covariance: this.getAgedCovariance(now),
|
|
442
|
+
index: this.filterState.index
|
|
443
|
+
});
|
|
444
|
+
}
|
|
235
445
|
|
|
236
|
-
update(groundSpeed, current, boatSpeed, heading) {
|
|
446
|
+
update(groundSpeed, current, boatSpeed, heading, headingVariance) {
|
|
237
447
|
if(groundSpeed.xVariance == null || groundSpeed.yVariance == null ||
|
|
238
448
|
current.xVariance == null || current.yVariance == null ||
|
|
239
449
|
boatSpeed.xVariance == null || boatSpeed.yVariance == null ) {
|
|
@@ -256,13 +466,19 @@ class CorrectionEstimator {
|
|
|
256
466
|
var currentCov = _rotateVariance(cosTheta, sinTheta, current.variance);
|
|
257
467
|
var boatCov = [[boatSpeed.xVariance, 0], [0, boatSpeed.yVariance]];
|
|
258
468
|
|
|
259
|
-
|
|
469
|
+
let observationCovariance = [[
|
|
260
470
|
groundCov[0][0] + currentCov[0][0] + boatCov[0][0],
|
|
261
471
|
groundCov[0][1] + currentCov[0][1] + boatCov[0][1]],
|
|
262
472
|
[
|
|
263
473
|
groundCov[1][0] + currentCov[1][0] + boatCov[1][0],
|
|
264
474
|
groundCov[1][1] + currentCov[1][1] + boatCov[1][1]],
|
|
265
475
|
];
|
|
476
|
+
observationCovariance = _addHeadingVariance(
|
|
477
|
+
observationCovariance,
|
|
478
|
+
groundVector,
|
|
479
|
+
currentVector,
|
|
480
|
+
headingVariance
|
|
481
|
+
);
|
|
266
482
|
// Mahalanobis distance check.
|
|
267
483
|
// For empty cells (filterState === null) we use a diffuse prior — mean (0,0),
|
|
268
484
|
// variance DIFFUSE_PRIOR_VAR — expressing "assume no correction needed, but
|
|
@@ -270,11 +486,13 @@ class CorrectionEstimator {
|
|
|
270
486
|
// accepting it unconditionally, preventing a single outlier from locking a
|
|
271
487
|
// cell at a bad value.
|
|
272
488
|
const DIFFUSE_PRIOR_VAR = 1.0; // (m/s)² — rejects corrections > ~3 m/s from zero
|
|
273
|
-
const
|
|
274
|
-
|
|
489
|
+
const now = Date.now();
|
|
490
|
+
const agedState = this._agedState(now);
|
|
491
|
+
const priorMean = agedState !== null
|
|
492
|
+
? [agedState.mean[0][0], agedState.mean[1][0]]
|
|
275
493
|
: [0, 0];
|
|
276
|
-
const priorCov =
|
|
277
|
-
?
|
|
494
|
+
const priorCov = agedState !== null
|
|
495
|
+
? agedState.covariance
|
|
278
496
|
: [[DIFFUSE_PRIOR_VAR, 0], [0, DIFFUSE_PRIOR_VAR]];
|
|
279
497
|
const inno = [observation[0] - priorMean[0], observation[1] - priorMean[1]];
|
|
280
498
|
const S = [
|
|
@@ -293,13 +511,14 @@ class CorrectionEstimator {
|
|
|
293
511
|
+ inno[1] * (Sinv[1][0] * inno[0] + Sinv[1][1] * inno[1]);
|
|
294
512
|
if (d2 > 9.21) return false;
|
|
295
513
|
}
|
|
296
|
-
this.filterState = this.filter.filter({ previousCorrected:
|
|
514
|
+
this.filterState = this.filter.filter({ previousCorrected: agedState, observation, observationCovariance });
|
|
515
|
+
this.lastAcceptedAt = now;
|
|
297
516
|
return true;
|
|
298
517
|
}
|
|
299
518
|
|
|
300
519
|
|
|
301
520
|
report() {
|
|
302
|
-
return { x: this.x, y: this.y, N: this.N };
|
|
521
|
+
return { x: this.x, y: this.y, N: this.N, lastAcceptedAt: this.lastAcceptedAt };
|
|
303
522
|
}
|
|
304
523
|
|
|
305
524
|
get N() {
|
|
@@ -318,15 +537,26 @@ class CorrectionEstimator {
|
|
|
318
537
|
}
|
|
319
538
|
|
|
320
539
|
get covariance() {
|
|
321
|
-
return this.
|
|
540
|
+
return this.getAgedCovariance();
|
|
322
541
|
}
|
|
323
542
|
|
|
324
543
|
|
|
325
544
|
toJSON() {
|
|
326
|
-
return this.N != 0
|
|
545
|
+
return this.N != 0
|
|
546
|
+
? { state: this.filterState, lastAcceptedAt: this.lastAcceptedAt }
|
|
547
|
+
: { state: null, lastAcceptedAt: null };
|
|
327
548
|
}
|
|
328
549
|
|
|
329
550
|
}
|
|
330
551
|
|
|
331
552
|
|
|
332
|
-
module.exports = {
|
|
553
|
+
module.exports = {
|
|
554
|
+
CorrectionTable,
|
|
555
|
+
CorrectionEstimator,
|
|
556
|
+
MAX_AGING_SECONDS,
|
|
557
|
+
MIN_CELL_INDEX,
|
|
558
|
+
SECONDS_PER_MONTH,
|
|
559
|
+
KNOT_TO_MPS,
|
|
560
|
+
DEFAULT_CORRECTION_DRIFT_RATE,
|
|
561
|
+
DEFAULT_PROCESS_NOISE_RATE
|
|
562
|
+
};
|
|
@@ -0,0 +1,33 @@
|
|
|
1
|
+
# Correction-table learning model
|
|
2
|
+
|
|
3
|
+
## Smoothing cadence
|
|
4
|
+
|
|
5
|
+
Learning inputs always use `MovingAverageSmoother`. The configurable `smootherTimeSpan` is the observation-window duration and is clamped to a minimum of 5 seconds. The learning interval is:
|
|
6
|
+
|
|
7
|
+
```text
|
|
8
|
+
learningInterval = smootherTimeSpan + 1 second
|
|
9
|
+
```
|
|
10
|
+
|
|
11
|
+
The extra second separates observation windows. Legacy exponential/Kalman smoother selections and their obsolete parameters are removed silently; a valid legacy window is retained and shorter windows become 5 seconds.
|
|
12
|
+
|
|
13
|
+
## Cell aging
|
|
14
|
+
|
|
15
|
+
Each learned cell stores the UTC epoch-millisecond timestamp of its last accepted observation. Its persisted posterior covariance is never changed by a read. At time `t`, the covariance used by the learner is aged by:
|
|
16
|
+
|
|
17
|
+
```text
|
|
18
|
+
effectiveDt = min((t - lastAcceptedAt) / 1000, 90 days)
|
|
19
|
+
Q(dt) = Q_rate * effectiveDt * I
|
|
20
|
+
```
|
|
21
|
+
|
|
22
|
+
The 90-day value is an internal constant. Offline time counts toward `effectiveDt`; once the cap is reached, additional offline time has no effect. The aged covariance is used consistently for Mahalanobis gating, Kalman filtering, interpolation, reporting, and spatial-q estimation. A rejected observation leaves the timestamp unchanged. An accepted observation stores the posterior covariance and current timestamp.
|
|
23
|
+
|
|
24
|
+
## Eligibility and spatial q
|
|
25
|
+
|
|
26
|
+
`MIN_CELL_INDEX` remains an explicit maturity gate and is currently zero. A cell is eligible only when `cell.N > MIN_CELL_INDEX`, so one accepted observation is sufficient. The same condition is used by interpolation and spatial-q pair selection. The index remains persisted and exposed in diagnostics.
|
|
27
|
+
The user-facing process-noise setting is **Correction drift**, expressed in knots per month. A month is defined as 30 days. Internally the value is converted to the SI `processNoiseRate` in `(m/s)^2` per second. The default is 0.3 knots/month; the UI range is 0.0 to 3.0 in 0.1 increments.
|
|
28
|
+
|
|
29
|
+
## File compatibility
|
|
30
|
+
|
|
31
|
+
Schema version 1 is the historical format and is preserved in `correction-table-v1.schema.json`; the existing `correction-table.schema.json` filename remains a compatibility alias. Schema version 2 adds `schemaVersion: 2`, per-cell `lastAcceptedAt` timestamps, and the serializable current model descriptor, including the SI `processNoiseRate`. The former `stability` descriptor is accepted only for compatibility with older files and is not written by new tables.
|
|
32
|
+
|
|
33
|
+
Legacy files without a discriminator and files marked version 1 are migrated on load. Every learned legacy cell receives the file's `mtimeMs`, because individual cell ages are unavailable. This is a conservative estimate. Empty cells receive `null`. If filesystem metadata is unavailable, load time is used and the fallback is recorded through debug logging. The original file is replaced only after successful deserialization and validation, using an atomic temporary-file rename. Loading an existing version-2 file never rewrites its timestamps.
|