speedandcurrent 1.4.2 → 1.5.1

package/index.js

@@ -1,55 +1,87 @@
-const { MessageHandler, Polar, PolarDamped, SI, Reporter } = require('signalkutilities');
-const { CorrectionTable } = require('./correctionTable.js');
-const { KalmanFilter } = require('kalman-filter');
-
-
+const {
+  SmoothedHeading,
+  SmoothedAttitude,
+  SmoothedSpeedThroughWater,
+  SmoothedGroundSpeed,
+  SI,
+  MessageHandler,
+  Polar,
+  Reporter,
+  BaseSmoother,
+  MovingAverageSmoother,
+  KalmanSmoother,
+  PolarSmoother,
+  createSmoothedPolar,
+  createSmoothedHandler
+} = require('signalkutilities');
 
+const { CorrectionTable } = require('./correctionTable.js');
 
 module.exports = function (app) {
 
-  let options = {};
   let isRunning = false;
-  let corrTable = null;
-  let lastSave = null;
-  let heading = null;
-  let attitude = null;
-  let current = null;
-  let currentStat = null;
-  let boatSpeed = null;
-  let boatSpeedStat = null;
+  let smoothedHeading = null;
+  let stability = null;
+  let smoothedAttitude = null;
+  let rawCurrent = null;
+  let smoothedCurrent = null;
+  let smoothedBoatSpeed = null;
   let correctedBoatSpeed = null;
   let boatSpeedRefGround = null;
-  let groundSpeed = null;
-  let groundSpeedStat = null;
+  let smoothedGroundSpeed = null;
   let speedCorrection = null;
   let residual = null;
   let reportFull = null;
   let reportVector = null;
   let table = null;
 
+  // add missing declarations
+  let rawHeading = null;
+  let rawAttitude = null;
+  let noCurrent = null;
+  let rawBoatSpeed = null;
+  let rawGroundSpeed = null;
+  let started = null;
+
   const plugin = {};
   plugin.id = "SpeedAndCurrent";
   plugin.name = "Speed and current";
-  plugin.description = "An experimental plugin that uses sensor fusion to get boat speed, current and leeway.";
-
-
+  plugin.description = "A plugin that uses sensor fusion to get boat speed, current and leeway.";
 
   plugin.schema = {
     type: "object",
     properties: {
-      mode: {
-        type: "string",
-        title: "Plugin mode",
-        description: "The mode of the plugin.",
-        enum: ["Start with new correction table, no current",
-          "Start with new correction table, with current",
-          "Fresh correction table, no current",
-          "Fresh correction table, with current",
-          "Mature correction table, no current",
-          "Mature correction table, with current",
-          "Locked correction table",
-          "Manual configuration"],
-        default: "Start with new correction table, no current",
+      startWithNewTable: {
+        type: "boolean",
+        title: "Start with new table",
+        description: "Start with a new correction table (overrides existing table).",
+        default: false
+      },
+      estimateBoatSpeed: {
+        type: "boolean",
+        title: "Estimate Boat Speed",
+        description: "Enable estimation of boat speed.",
+        default: false
+      },
+      updateCorrectionTable: {
+        type: "boolean",
+        title: "Update Correction Table",
+        description: "Enable updating of the correction table.",
+        default: true
+      },
+      stability: {
+        type: "number",
+        title: "Correction Table Stability",
+        description: "Stability of the corrections (higher means that corrections are changed at a slower rate).",
+        default: 7,
+        minimum: 1,
+        maximum: 20
+      },
+      assumeCurrent: {
+        type: "boolean",
+        title: "Assume Current",
+        description: "Assume there is a current present when updating the correction table.",
+        default: false
       },
       heelStep: {
         type: "number",
@@ -75,144 +107,98 @@ module.exports = function (app) {
         description: "Correction table maximum speed.",
         default: 9
       },
-
       headingSource: {
         type: "string",
         title: "Heading Source",
         description: "Source to use for navigation.heading (optional).",
         default: " "
       },
-
       boatSpeedSource: {
         type: "string",
         title: "Boat Speed Source",
         description: "Source to use for navigation.speedThroughWater (optional).",
         default: " "
       },
-
       COGSource: {
         type: "string",
         title: "Course over ground",
         description: "Source to use for navigation.courseOverGroundTrue (optional)",
         default: " "
       },
-
       SOGSource: {
         type: "string",
         title: "Ground Speed Source",
         description: "Source to use for navigation.speedOverGround (optional)",
         default: " "
       },
-
       attitudeSource: {
         type: "string",
         title: "Attitude Source",
         description: "Source to use for navigation.attitude (optional)",
         default: " "
       },
-
       preventDuplication: {
         type: "boolean",
         title: "Prevent duplication of boat speed",
         description: "Overwrite boat speed from sensor with corrected boat speed.",
-        default: true,
-      },
-
-      updateCorrectionTable: {
-        type: "boolean",
-        title: "Update correction table",
-        description: "Update estimations in the correction table.",
-        default: true,
-      },
-      estimateBoatSpeed: {
-        type: "boolean",
-        title: "Correct boat speed",
-        description: "Correct boat speed using correction table. This includes adding leeway.",
-        default: true,
-      },
-      correctionStability: {
-        type: "number",
-        title: "Stability of correction estimation",
-        description: `The stability of the correction estimation indicates how much the estimation is trusted 
-        opposed to the calculation of . Biggeboat speed. Bigger values give more trust to the estimation and more stable estimations`,
-        default: 7,
-        minimum: 5,
-        maximum: 12
-      },
-      doStartFresh: {
-        type: "boolean",
-        title: "Empty correction table",
-        description: "Restart with a fresh correction table. WARNING: all current corrections will be lost permanently!",
-        default: false
-      },
-      assumeCurrent: {
-        type: "boolean",
-        title: "assume current",
-        description: "The estimator assumes that there is current.",
-        default: false,
-      },
-      estimateCurrent: {
-        type: "boolean",
-        title: "Estimate current",
-        description: "Estimate water current.",
-        default: true,
-      },
-      currentStability: {
-        type: "number",
-        title: "Stability of current estimation ",
-        description: `The stability of the current estimation indicates how much the estimation is trusted 
-        opposed to the calculation of current. Bigger values give more trust to the estimation and more sable estimations`,
-        default: 5,
-        minimum: 1,
-        maximum: 8
+        default: true
       }
     }
   };
 
   plugin.uiSchema = {
-    'ui:order': ["mode", "maxSpeed", "speedStep", "maxHeel", "heelStep", "headingSource", "boatSpeedSource", "COGSource", "SOGSource", "attitudeSource", "preventDuplication", "updateCorrectionTable", "estimateBoatSpeed", "correctionStability", "doStartFresh", "assumeCurrent", "estimateCurrent", "currentStability"],
-    doStartFresh: {
-      "ui:widget": "hidden",
+    'ui:order': [
+      "estimateBoatSpeed",
+      "updateCorrectionTable",
+      "assumeCurrent",
+      "startWithNewTable",
+      "stability",
+      "maxSpeed",
+      "speedStep",
+      "maxHeel",
+      "heelStep",
+      "headingSource",
+      "boatSpeedSource",
+      "COGSource",
+      "SOGSource",
+      "attitudeSource",
+      "preventDuplication"
+    ],
+    startWithNewTable: {
+      "ui:widget": "checkbox"
+    },
+    estimateBoatSpeed: {
+      "ui:widget": "checkbox"
+    },
+    updateCorrectionTable: {
+      "ui:widget": "checkbox"
+    },
+    stability: {
+      "ui:widget": "updown"
+    },
+    assumeCurrent: {
+      "ui:widget": "checkbox"
     },
     heelStep: {
-      "ui:widget": "updown",
+      "ui:widget": "updown"
     },
     maxHeel: {
-      "ui:widget": "updown",
+      "ui:widget": "updown"
     },
     speedStep: {
-      "ui:widget": "updown",
+      "ui:widget": "updown"
     },
     maxSpeed: {
-      "ui:widget": "updown",
+      "ui:widget": "updown"
     },
     mode: {
-      "ui:widget": "select",
-    },
-    updateCorrectionTable: {
-      "ui:widget": "hidden",
-    },
-    estimateBoatSpeed: {
-      "ui:widget": "hidden",
-    },
-    correctionStability: {
-      "ui:widget": "hidden",
+      "ui:widget": "select"
     },
     preventDuplication: {
-      "ui:widget": "checkbox",
-    },
-    assumeCurrent: {
-      "ui:widget": "hidden",
-    },
-    estimateCurrent: {
-      "ui:widget": "hidden",
-    },
-    currentStability: {
-      "ui:widget": "hidden",
+      "ui:widget": "checkbox"
     }
   };
 
-
   plugin.registerWithRouter = function (router) {
     app.debug('registerWithRouter');
 
@@ -236,91 +222,66 @@ module.exports = function (app) {
 
   }
 
-  // ...existing code...
-
-  function saveTable(options, correctionTable) {
-    options.correctionTable = correctionTable.toJSON();
-    app.savePluginOptions(options, () => { app.debug('Correction table saved') });
-    return new Date();
-  }
-
-
-  plugin.start = (opts) => {
-    app.setPluginStatus("starting");
-    app.debug("plugin starting");
-    let outputs = [];
-
-    // make options survive to the stop function
-    options = opts;
-    loadPresets();
-
-    // correction table
-    table = loadTable(options);
+  plugin.start = (settings) => {
+    app.setPluginStatus("Starting");
+    app.debug("Starting");
+    table = loadTable(settings);
 
+    //#region Handler and Polar Initialization
+    let smootherOptions = { timeConstant: 1, processVariance: 1, measurementVariance: 20, timeSpan: 5 };
 
     // heading
-    heading = new MessageHandler("heading", "navigation.headingTrue", options.headingSource);
-    heading.subscribe(app, plugin.id, true);
-    heading.setDisplayAttributes({ label: "heading" });
-    headingStat = new PolarDamped("headingDamped", heading, 1);
-    heading.onChange = () => {
-      headingStat.sample();
-    };
+    smoothedHeading = new SmoothedHeading(app, plugin.id, settings.headingSource, true, MovingAverageSmoother, smootherOptions);
+    rawHeading = smoothedHeading.polar.angleHandler;
+
+    //attitude
+    smoothedAttitude = new SmoothedAttitude(app, plugin.id, settings.attitudeSource, true, MovingAverageSmoother, smootherOptions);
+    rawAttitude = smoothedAttitude.handler;
 
-    // attitude
-    attitude = new MessageHandler("attitude", "navigation.attitude", options.attitudeSource);
-    attitude.value = { pitch: 0, roll: 0, yaw: 0 }; //prevents errors when there is no attitude sensor
-    attitude.setDisplayAttributes({ label: "attitude" });
-    attitude.subscribe(app, plugin.id, true);
-    attitudeStat = new PolarDamped("attitudeDamped", attitude, 1);
-    attitude.onChange = () => {
-      attitudeStat.sample();
-    };
 
     // current
-    current = new Polar("current", "self.environment.current.drift", "self.environment.current.setTrue");
-    current.setDisplayAttributes({ label: "current", plane: "Ground" });
-    current.setAngleRange('0to2pi');
-    currentStat = new PolarDamped("currentDamped", current, 1);
-    currentStat.setAngleRange('0to2pi');
-    outputs.push(current);
+    // send metadata for current
+    MessageHandler.setMeta(app, plugin.id, "environment.current.drift", {units: "m/s", type: "number", description: "Speed of the current"});
+    MessageHandler.setMeta(app, plugin.id, "environment.current.setTrue", { units: "rad", type: "number", description: "Direction of the current" });
+    rawCurrent = new Polar("current", "self.environment.current.drift", "self.environment.current.setTrue");
+    rawCurrent.setDisplayAttributes({ label: "current", plane: "Ground" });
+    rawCurrent.setAngleRange('0to2pi');
+    smoothedCurrent = new PolarSmoother("currentDamped", rawCurrent, KalmanSmoother, { processVariance: 0.000001, measurementVariance: 0.01 });
+    smoothedCurrent.setAngleRange('0to2pi');
+    smoothedCurrent.setDisplayAttributes({ label: "Current", plane: "Ground" });
     // Current should be initialised as no current
-    current.setVectorValue({ x: 0, y: 0 });
-    // There should be at least two samples, otherwise we can't calculate a valid speed
-    currentStat.sample();
-    currentStat.sample();
+    rawCurrent.setVectorValue({ x: 0, y: 0 });
+    // Strongly assume no current at start
+    smoothedCurrent.xSmoother.reset(0, 0.00000001);
+    smoothedCurrent.ySmoother.reset(0, 0.00000001);
+    // no current
+    noCurrent = createSmoothedPolar({
+      id: "boatSpeed",
+      pathMagnitude: "self.environment.current.drift",
+      pathAngle: "self.environment.current.setTrue",
+      subscribe: false,
+      app,
+      pluginId: plugin.id,
+      SmootherClass: BaseSmoother,
+      smootherOptions: smootherOptions,
+      displayAttributes: { label: "NoCurrent", plane: "Ground" },
+    });
+    noCurrent.xSmoother.reset(0,0);
+    noCurrent.ySmoother.reset(0,0);
+    PolarSmoother.send(app, plugin.id, [noCurrent]);
 
-    // boat speed
-    boatSpeed = new Polar("boatSpeed", "navigation.speedThroughWater", options.boatSpeedSource);
-    boatSpeed.subscribe(app, plugin.id, true, false, !options.preventDuplication || !options.estimateBoatSpeed);
-    boatSpeed.setDisplayAttributes({ label: "boat speed", plane: "Boat" });
-    boatSpeedStat = new PolarDamped("boatSpeedDamped", boatSpeed, 1);
-    boatSpeedStat.setDisplayAttributes({ label: "boat speed (damped)", plane: "Boat" });
-    boatSpeed.onChange = () => {
-      boatSpeedStat.sample();
-    };
 
+    // boat speed
+    smoothedBoatSpeed = new SmoothedSpeedThroughWater(app, plugin.id, settings.boatSpeedSource, true, MovingAverageSmoother, smootherOptions, !settings.preventDuplication);
+    rawBoatSpeed = smoothedBoatSpeed.polar;
     correctedBoatSpeed = new Polar("correctedBoatSpeed", "navigation.speedThroughWater", "navigation.leewayAngle");
     correctedBoatSpeed.setDisplayAttributes({ label: "corrected boat speed", plane: "Boat" });
-    outputs.push(correctedBoatSpeed);
-    //const correctedBoatSpeedStat = new PolarDamped("correctedBoatSpeedDamped", correctedBoatSpeed);
-
-    // TODO: Add navigation.speedThroughWaterTransverse and navigation.speedThroughWaterLongitudinal
-
     boatSpeedRefGround = new Polar("boatSpeedRefGround");
-    boatSpeedRefGround.setDisplayAttributes("boat speed over ground", "Ground");
-
+    boatSpeedRefGround.setDisplayAttributes("Boat speed over ground", "Ground");
+ 
     // ground speed
-    groundSpeed = new Polar("groundSpeed", "navigation.speedOverGround", "navigation.courseOverGroundTrue", options.SOGSource, options.COGSource);
-    groundSpeed.subscribe(app, plugin.id, true, true, true);
-    groundSpeed.setDisplayAttributes({ label: "ground speed", plane: "Ground" });
-    groundSpeed.setAngleRange('0to2pi');
-    groundSpeedStat = new PolarDamped("groundSpeedDamped", groundSpeed, 1);
-    groundSpeedStat.setDisplayAttributes({ label: "ground speed (damped)", plane: "Ground" });
-    groundSpeedStat.setAngleRange('0to2pi');
-    groundSpeed.onChange = () => {
-      groundSpeedStat.sample();
-    };
+    smoothedGroundSpeed = new SmoothedGroundSpeed(app, plugin.id, settings.SOGSource, true, MovingAverageSmoother, smootherOptions, !settings.preventDuplication);
+    rawGroundSpeed = smoothedGroundSpeed.polar;
 
     // correction vector
     speedCorrection = new Polar("speedCorrection");
@@ -331,273 +292,93 @@ module.exports = function (app) {
     residual = new Polar("residual");
     residual.setDisplayAttributes({ label: "residual", plane: "Ground" });
 
-    // Make reporting object for webApp
+    //#endregion
+
+    //#region Reporting
     reportFull = new Reporter();
-    reportFull.addDelta(heading);
-    reportFull.addAttitude(attitude);
-    reportFull.addPolar(groundSpeed);
-    reportFull.addPolar(boatSpeed);
-    //reportFull.addPolar(boatSpeedStat);
-    //reportFull.addPolar(groundSpeedStat);
-    reportFull.addPolar(speedCorrection);
-    reportFull.addPolar(correctedBoatSpeed);
-    reportFull.addPolar(current);
-    reportFull.addPolar(residual);
+
+    if (settings.estimateBoatSpeed) {
+      reportFull.addDelta(rawHeading);
+      reportFull.addAttitude(rawAttitude);
+      reportFull.addPolar(rawBoatSpeed);
+      reportFull.addPolar(speedCorrection);
+      reportFull.addPolar(correctedBoatSpeed);
+      reportFull.addPolar(rawGroundSpeed);
+      reportFull.addPolar(smoothedCurrent);
+    }
+    if (settings.updateCorrectionTable) {
+      reportFull.addDelta(smoothedHeading);
+      reportFull.addAttitude(smoothedAttitude);
+      reportFull.addPolar(smoothedBoatSpeed);
+      reportFull.addPolar(smoothedGroundSpeed);
+
+        if (settings.assumeCurrent) {
+          reportFull.addPolar(boatSpeedRefGround);
+          reportFull.addPolar(smoothedCurrent);
+        }
+      reportFull.addPolar(speedCorrection);
+      reportFull.addPolar(residual);
+    }
     reportFull.addTable(table);
 
+    // Make reporting object for webApp
+
+
     reportVector = new Reporter();
-    reportVector.addDelta(heading);
+    reportVector.addDelta(rawHeading);
     reportVector.addPolar(residual);
     reportVector.addPolar(speedCorrection);
-    reportVector.addPolar(current);
+    if (settings.assumeCurrent) reportVector.addPolar(smoothedCurrent);
     reportVector.addPolar(correctedBoatSpeed);
-    reportVector.addPolar(boatSpeed);
-    reportVector.addPolar(groundSpeed);
-
-    boatSpeed.onChange = () => {
-      boatSpeedStat.sample();
-      calculate();
-    };
-
-
+    reportVector.addPolar(rawBoatSpeed);
+    reportVector.addPolar(rawGroundSpeed);
+    //#endregion
 
     isRunning = true;
+    started = new Date();
     app.setPluginStatus("Running");
     app.debug("Running");
 
-
-
-
-
-
-    // Main function, estimates boatSpeed, leeway and current
-    function calculate() {
-      // prepare iteration
-      heel = attitude.value.roll;
-      speed = boatSpeed.magnitude;
-      theta = heading.value;
-      if (!Number.isFinite(heel)) {
-        app.debug("Heel is not a valid number, skipping calculation");
-        return;
-      }
-      if (!Number.isFinite(speed)) {
-        app.debug("Speed is not a valid number, skipping calculation");
-        return;
-      }
-      if (!Number.isFinite(theta)) {
-        app.debug("Heading is not a valid number, skipping calculation");
-        return;
-      }
-
-      // update correction table
-      if (options.updateCorrectionTable) {
-        table.update(speed, heel, groundSpeedStat, currentStat, boatSpeedStat, theta);
-      }
-
-      // correct and estimate boat speed
-      estimateBoatSpeed();
-
-      if (options.assumeCurrent) {
-        // estimate current ;
-        estimateCurrent();
-      }
-      calcResidual();
-
-
-      // Send calculated delta's
-      updates = [];
-      if (options.estimateCurrent) updates.push(current);
-      if (options.estimateBoatSpeed) updates.push(correctedBoatSpeed);
-      if (updates.length) Polar.send(app, plugin.id, updates);
-
-      // Save correction table 
-      // periodically 
-      if (options.updateCorrectionTable && new Date() - lastSave > 5 * 60 * 1000) {
-        lastSave = saveTable(options, table);
-      }
-    }
-
-    // estimates boat speed from observed boatSpeed and correction
-    function estimateBoatSpeed() {
-      cor = table.getKalmanCorrection(speed, heel);
-      correctedBoatSpeed.copyFrom(boatSpeed);
-      speedCorrection.setVectorValue(cor);
-      correctedBoatSpeed.add(speedCorrection);
-    }
-
-    // estimates current from observed groundspeed and estimated boatspeed 
-    function estimateCurrent() {
-      boatSpeedRefGround.copyFrom(correctedBoatSpeed);
-      boatSpeedRefGround.rotate(theta);
-      current.copyFrom(groundSpeed);
-      current.substract(boatSpeedRefGround);
-      currentStat.sample();
-      current.copyFrom(currentStat);
-    }
-
-    // Calculate what ground speed is observed that is not attributed to estimated boat speed or estimated current
-    function calcResidual() {
-      residual.copyFrom(groundSpeed);
-      boatSpeedRefGround.copyFrom(correctedBoatSpeed);
-      boatSpeedRefGround.rotate(theta);
-      residual.substract(boatSpeedRefGround);
-      residual.substract(current);
-    }
-
-    // Make sure the table matches the settings
-    function enforceConsistancy(row, col) {
-      // function is bugged due to IS units vs knots and degrees
-      if (!options?.correctionTable) return false;
-      table = options.correctionTable;
-      if (!table) return false;
-      if (!table.row) return false;
-      if (table.row.min != row.min) return false;
-      if (table.row.max != row.max) return false;
-      if (table.row.step != row.step) return false;
-      if (table.col.min != col.min) return false;
-      if (table.col.max != col.max) return false;
-      if (table.col.step != col.step) return false;
-      return true;
-    }
-
-    function loadTable(options) {
-      const row = { min: 0, max: SI.fromKnots(options.maxSpeed), step: SI.fromKnots(options.speedStep) };
-      const col = { min: -SI.fromDegrees(options.maxHeel), max: SI.fromDegrees(options.maxHeel), step: SI.fromDegrees(options.heelStep) };
-      let table;
-      if (enforceConsistancy(row, col) && !options.doStartFresh) {
-        table = CorrectionTable.fromJSON(options.correctionTable, options.correctionStability);
-        app.debug("Correction table loaded");
-        lastSave = new Date();
-      }
-      else {
-        table = new CorrectionTable("correctionTable", row, col, options.correctionStability);
-        app.debug("Correction table created");
-        options.doStartFresh = false;
-        lastSave = saveTable(options, table);
-      }
-      table.setDisplayAttributes({ label: "correction table" });
-      return table;
-    }
-
-
-    function loadPresets() {
-      const modeEnum = plugin.schema.properties.mode.enum;
-      app.debug("Loading presests for mode: " + options.mode);
-
-      switch (modeEnum.indexOf(options.mode)) {
-        case -1:
-          app.debug("Invalid mode");
-          app.debug(options);
-          //options.mode = modeEnum[2]; 
-          break;
-        case 0:
-          // new table, no current
-          options.doStartFresh = true;
-          options.updateCorrectionTable = true;
-          options.estimateBoatSpeed = false;
-          options.assumeCurrent = false;
-          options.estimateCurrent = false;
-          options.correctionStability = 5;
-          options.mode = modeEnum[2];
-          break;
-        case 1:
-          // new table, with current
-          options.doStartFresh = true;
-          options.updateCorrectionTable = true;
-          options.estimateBoatSpeed = false;
-          options.assumeCurrent = true;
-          options.estimateCurrent = false;
-          options.correctionStability = 5;
-          options.currentStability = 7;
-          options.mode = modeEnum[3];
-          break;
-        case 2:
-          // fresh table, no current
-          options.doStartFresh = false;
-          options.updateCorrectionTable = true;
-          options.estimateBoatSpeed = false;
-          options.assumeCurrent = false;
-          options.estimateCurrent = false;
-          options.correctionStability = 6;
-          break;
-        case 3:
-          // fresh table, with current
-          options.doStartFresh = false;
-          options.updateCorrectionTable = true;
-          options.estimateBoatSpeed = false;
-          options.assumeCurrent = true;
-          options.estimateCurrent = false;
-          options.correctionStability = 6;
-          options.currentStability = 6;
-          break;
-        case 4:
-          // mature table, no current
-          options.doStartFresh = false;
-          options.updateCorrectionTable = true;
-          options.estimateBoatSpeed = true;
-          options.assumeCurrent = false;
-          options.estimateCurrent = false;
-          options.correctionStability = 10;
-          break;
-        case 5:
-          // mature table, with current
-          options.doStartFresh = false;
-          options.updateCorrectionTable = true;
-          options.estimateBoatSpeed = true;
-          options.assumeCurrent = true;
-          options.estimateCurrent = true;
-          options.correctionStability = 10;
-          options.currentStability = 3;
-          break;
-        case 6:
-          // locked table
-          options.doStartFresh = false;
-          options.updateCorrectionTable = false;
-          options.estimateBoatSpeed = true;
-          options.assumeCurrent = true;
-          options.estimateCurrent = true;
-          options.currentStability = 2;
-          break;
-        case 7:
-          // all manual configuration;
-          break;
-      }
-    }
-
-    // compare floats
-    function isEqual(a, b) {
-      const TOLERANCE = 1e-5;
-      if (Math.abs(a - b) < TOLERANCE) {
-        return true;
+    let lastSave = 0;
+    smoothedBoatSpeed.onChange = () => {
+      const wellUnderway = started < new Date() - 60 * 1000;
+      const minSpeed = SI.fromKnots(settings.speedStep / 2);
+      if (settings.estimateBoatSpeed) correct(wellUnderway);
+      if (settings.updateCorrectionTable && wellUnderway) {
+        updateTable(settings.assumeCurrent, minSpeed);
+        // Save correction table periodically 
+        const now = new Date();
+        if (now - lastSave > 5 * 1000) {
+          saveTable(settings, table);
+          lastSave = now;
+        }
       }
-      return false;
-    }
+    };
   }
 
-
   plugin.stop = () => {
     return new Promise((resolve, reject) => {
       try {
-        //corrTable.save(app);
-        if (corrTable != null) {
-          lastSave = saveTable(options, corrTable);
-        }
-
-        heading = heading?.terminate(app);
-        attitude = attitude?.terminate(app);
-        current = current?.terminate(app);
-        currentStat = null;
-        boatSpeed = boatSpeed?.terminate(app);
-        boatSpeedStat = null;
+        smoothedHeading = smoothedHeading?.terminate(app);
+        stability = stability?.terminate?.(app);
+        smoothedAttitude = smoothedAttitude?.terminate(app);
+        rawCurrent = rawCurrent?.terminate(app);
+        smoothedCurrent = smoothedCurrent?.terminate?.(app);
+        smoothedBoatSpeed = smoothedBoatSpeed?.terminate(app);
         correctedBoatSpeed = correctedBoatSpeed?.terminate(app);
         boatSpeedRefGround = boatSpeedRefGround?.terminate(app);
-        groundSpeed = groundSpeed?.terminate(app);
-        groundSpeedStat = null;
+        smoothedGroundSpeed = smoothedGroundSpeed?.terminate(app);
         speedCorrection = speedCorrection?.terminate(app);
         residual = residual?.terminate(app);
         reportFull = null;
         reportVector = null;
         table = null;
+        rawHeading = null;
+        rawAttitude = null;
+        noCurrent = null;
+        rawBoatSpeed = null;
+        rawGroundSpeed = null;
+        started = null;
         app.setPluginStatus("Stopped");
         app.debug("Stopped");
 
@@ -608,5 +389,157 @@ module.exports = function (app) {
       }
     });
   };
+
+   /**
+   * Estimates and applies corrections to boat speed, leeway, and current.
+   *
+   * This function uses the current attitude, boat speed, and heading to compute a correction
+   * vector from the correction table, applies it to the observed boat speed, and sends the
+   * corrected value. If the vessel is well underway, it also estimates the current vector
+   * by comparing the corrected boat speed (rotated to heading) with the ground speed.
+   *
+   * @param {boolean} wellUnderway - True if the vessel is considered underway and current estimation should be performed.
+   */
+  function correct(wellUnderway) {
+    // prepare iteration
+    const heel = rawAttitude.value?.roll;
+    const speed = rawBoatSpeed.magnitude;
+    const theta = rawHeading.value;
+    //app.debug(`Heel: ${SI.toDegrees(heel).toFixed(1)}°, Speed: ${SI.toKnots(speed).toFixed(2)} kn, Heading: ${SI.toDegrees(theta).toFixed(1)}°`);
+    if (!Number.isFinite(heel) || !Number.isFinite(speed) || !Number.isFinite(theta)) {
+      //app.debug("Invalid input data");
+      return;
+    }
+
+    correctedBoatSpeed.copyFrom(rawBoatSpeed);
+    speedCorrection.setVectorValue({ x: 0, y: 0 }) ;
+    if (speed > 0 && !rawAttitude.stale ) {
+      const { correction, variance } = table.getCorrection(speed, heel);
+      speedCorrection.setVectorValue(correction, variance);
+      correctedBoatSpeed.add(speedCorrection);
+    }
+    Polar.send(app, plugin.id, [correctedBoatSpeed]);
+    // estimate current
+    if (wellUnderway && !rawGroundSpeed.stale) {
+      boatSpeedRefGround.copyFrom(correctedBoatSpeed);
+      boatSpeedRefGround.rotate(theta);
+      rawCurrent.copyFrom(rawGroundSpeed);
+      rawCurrent.substract(boatSpeedRefGround);
+      smoothedCurrent.sample();
+    }
+    PolarSmoother.send(app, plugin.id, [smoothedCurrent]);
+  }
+
+  /**
+   * Updates the correction table and calculates the residual error.
+   *
+   * This function uses the current smoothed attitude, boat speed, and heading to update the correction table
+   * if the boat speed exceeds the specified minimum. It also calculates the residual between the measured ground speed
+   * and the expected ground speed (corrected boat speed plus current). The correction table is periodically saved.
+   *
+   * @param {boolean} [assumeCurrent=false] - If true, uses the smoothed current for correction; otherwise, assumes no current.
+   * @param {number} [minSpeed=0] - The minimum speed threshold (in SI units) required to update the correction table.
+   */
+  function updateTable(assumeCurrent = false, minSpeed = 0) {
+    // prepare iteration
+    const heel = smoothedAttitude.value.roll;
+    const speed = smoothedBoatSpeed.magnitude;
+    const theta = smoothedHeading.value;
+
+    if (!Number.isFinite(heel) || !Number.isFinite(speed) || !Number.isFinite(theta)) return;
+
+    // update correction table
+    if (speed > minSpeed && !smoothedAttitude.stale && !smoothedHeading.stale  && !smoothedGroundSpeed.stale) {
+      table.update(speed, heel, smoothedGroundSpeed, assumeCurrent ? smoothedCurrent : noCurrent, smoothedBoatSpeed, theta);
+    }
+
+    // calculate residual
+    residual.copyFrom(smoothedGroundSpeed);
+    boatSpeedRefGround.copyFrom(correctedBoatSpeed);
+    boatSpeedRefGround.rotate(theta);
+    residual.substract(boatSpeedRefGround);
+    residual.substract(smoothedCurrent);
+
+  }
+
+  /**
+   * Checks if the correction table in settings matches the expected row and column configuration.
+   *
+   * This function compares the min, max, and step values of the row and column definitions
+   * in the current correction table against the provided row and col objects. Returns true
+   * if all values match, otherwise false. If the table is missing or not properly structured,
+   * returns false.
+   *
+   * @param {Object} row - The expected row configuration ({ min, max, step }).
+   * @param {Object} col - The expected column configuration ({ min, max, step }).
+   * @returns {boolean} True if the table matches the configuration, false otherwise.
+   */
+  function enforceConsistancy(options, row, col) {
+    if (!options?.correctionTable) return false;
+    table = options.correctionTable;
+    if (!table) return false;
+    if (!table.row) return false;
+    if (table.row.min != row.min) return false;
+    if (table.row.max != row.max) return false;
+    if (table.row.step != row.step) return false;
+    if (table.col.min != col.min) return false;
+    if (table.col.max != col.max) return false;
+    if (table.col.step != col.step) return false;
+    return true;
+  }
+
+  /**
+   * Loads or creates a correction table based on the provided options.
+   *
+   * This function checks if the correction table in the options matches the expected row and column configuration.
+   * If so, it loads the table from JSON; otherwise, it creates a new correction table with the specified parameters.
+   * The table is labeled and, if newly created, saved to the plugin options.
+   *
+   * @param {Object} options - The plugin options containing correction table settings and parameters.
+   * @returns {CorrectionTable} The loaded or newly created CorrectionTable instance.
+   */
+  function loadTable(options) {
+    const row = { min: 0, max: SI.fromKnots(options.maxSpeed), step: SI.fromKnots(options.speedStep) };
+    const col = { min: -SI.fromDegrees(options.maxHeel), max: SI.fromDegrees(options.maxHeel), step: SI.fromDegrees(options.heelStep) };
+    let table;
+    const stability = (options.stability !== undefined) ? options.stability : 6;
+    const VAR_FLOOR = 1e-4;
+    if (enforceConsistancy(options, row, col) && !options.startWithNewTable) {
+      table = CorrectionTable.fromJSON(options.correctionTable, stability);
+      app.debug("Correction table loaded");
+    }
+    else if (options.correctionTable && !options.startWithNewTable) {
+      // Resample existing table to new dimensions conservatively (N=0, diag covariance with floor)
+      table = CorrectionTable.resampleFromJSON(options.correctionTable, row, col, stability, VAR_FLOOR);
+      app.debug("Correction table migrated to new dimensions");
+      options.startWithNewTable = false;
+      saveTable(options, table);
+    } else {
+      table = new CorrectionTable("correctionTable", row, col, stability);
+      app.debug("Correction table created");
+      // doStartFresh is not user-settable, so no need to reset it here
+      options.startWithNewTable = false;
+      saveTable(options, table);
+    }
+    table.setDisplayAttributes({ label: "correction table" });
+    return table;
+  }
+
+  /**
+   * Saves the correction table to the plugin options and logs the save event.
+   *
+   * This function serializes the provided correction table, updates the options object,
+   * and triggers the plugin's save mechanism. It returns the current date/time to indicate
+   * when the save occurred.
+   *
+   * @param {Object} options - The plugin options object to update.
+   * @param {CorrectionTable} correctionTable - The correction table instance to save.
+   * @returns {Date} The date and time when the table was saved.
+   */
+  function saveTable(options, correctionTable) {
+  options.correctionTable = correctionTable.toJSON();
+  app.savePluginOptions(options, () => { /*app.debug("Correction table saved")*/; });
+  }
+
   return plugin;
 };