speedandcurrent 1.0.0

package/CHANGELOG.md

@@ -0,0 +1,10 @@
+# Changelog
+
+All notable changes to this project will be documented in this file.
+
+The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/),
+
+## [0.1.0] - 2025-1-? First version
+### Added
+- basic functionality to estimate speed, leeway and current using Kalman filters
+- basic functionality for a webapp that shows what is going on

package/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 Aswin
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

package/README.md

@@ -0,0 +1,86 @@
+# **Speed Correction & Current Estimation Plugin**
+
+## **Overview**
+This SignalK plugin estimates **corrected speed through water**, **leeway**, and **water current** in real-time using advanced filter techniques. The plugin continuously refines its estimates based on vessel speed, heading, ground speed, and heel angle.
+
+### **Key Features**
+- ✅ Real-time estimation of vessel speed (STW).
+- ✅ Real-time estimation of leeway.
+- ✅ Real-time estimation of water current (both speed and direction).
+- ✅ Adaptive learning: The plugin refines corrections over time.
+- ✅ A web app to provide real-time insights into the plugin's operation.
+- ✅ Use of Kalman filters for optimal estimations of the correction table, boat speed, and current.
+- ✅ A user-configurable 2D correction table for speed errors based on heel and speed.
+
+## **Introduction**
+The plugin is based on the idea that a vessel's speed sensor is imperfect and can be corrected. Speed sensors, most often paddle wheels, are imperfect for two reasons:
+1. **Measurement Precision**: The water flow over the hull influences the paddle wheel's measurement.
+2. **Leeway**: A paddle wheel does not measure lateral speed or leeway, which is crucial for sailing vessels.
+
+A GPS sensor provides the vessel's speed over ground, which differs from the paddle wheel's speed through water when there is current. The GPS sensor also has its imperfections.
+
+In theory, one could use the GPS sensor to estimate the paddle wheel's error. However, estimating the paddle wheel's error requires knowing the water current, which cannot be measured directly from a floating vessel. Given enough measurements, one could estimate both current and paddle wheel error.
+
+The plugin estimates both current and sensor error simultaneously. Using these estimations, the plugin corrects boat speed, estimates leeway, and estimates the current in real-time.
+
+### **Assumptions**
+- The paddle wheel's error depends on boat speed and heel angle, and it may change slowly over time.
+- The speed and direction of the current change slowly and gradually.
+- The vessel changes direction regularly.
+
+## **Configuration of the Plugin**
+
+### **1. Plugin Mode**
+The plugin has different modes optimized for various circumstances. If there is no (or very little) current, use one of the no-current modes for the best paddle wheel corrections. If there is current, select one of the modes that account for current.
+
+You must start with a mode that creates a new correction table. These modes use the specified dimensions to create an empty correction table. Once the table is created, the plugin will automatically switch to one of the fresh correction table modes. Be careful when selecting to start with a new correction table, as the previous table will be permanently deleted.
+
+The modes that assume a fresh correction table are optimized to estimate valid correction values quickly. Corrections are not applied, and no leeway or current is estimated. Once the correction table has matured, select one of the mature correction table modes. In these modes, corrections are calculated and updated conservatively, and corrections are applied to the boat speed. Leeway and, optionally, current are estimated.
+
+If circumstances are very dynamic (e.g., unstable wind, currents, or big waves), you can lock the correction table. Corrections will no longer be updated but will be applied.
+
+| Mode | Updates Correction Table | Assume Current | Correct Boat Speed | Estimate Leeway | Estimate Current |
+|------|--------------------------|----------------|---------------------|-----------------|------------------|
+| Start with new correction table, no current | Yes, starting with a new one | No | No | No | No |
+| Start with new correction table, with current | Yes, starting with a new one | Yes | No | No | No |
+| Fresh correction table, no current | Fast | No | No | No | No |
+| Fresh correction table, with current | Fast | Yes | No | No | No |
+| Mature correction table, no current | Slow | No | Yes | Yes | No |
+| Mature correction table, with current | Slow | Yes | Yes | Yes | Yes |
+| Locked correction table | No | Yes | Yes | Yes | Yes |
+
+### **2. Correction Table**
+The other parameters for the plugin deal with the correction table. These include step size for heel and speed, and maximum heel and speed.
+- The correction table should cover the vessel's normal dynamic range. Set maximum speed and maximum heel to values the vessel would typically achieve. Avoid setting values for exceptional circumstances, as this may result in unreliable estimations.
+- Choose the step size carefully. Too small step sizes may lead to insufficient observations for a good estimation, while too large step sizes may result in imprecise corrections.
+- Changing any table dimensions will result in the loss of all existing estimations.
+
+## **WebApp**
+The plugin comes with a WebApp that allows you to view the correction table and see the estimations in real-time. The WebApp can be launched from the SignalK server. The WebApp:
+- Graphically shows the boat speeds (observed and estimated), ground speed, current, and correction.
+- Displays incoming deltas.
+- Displays the boat speeds (observed and estimated), ground speed, current, and correction.
+- Displays the content of the correction table in either Cartesian values or polar coordinates.
+- Displays the plugin configuration and internal settings.
+
+The graphical representation of the speed vectors, current vector, and correction vector can be embedded in KIP displays using the path `/speedandcurrent/vectors.html`.
+
+---
+
+## **How It Works**
+
+### **1. Speed Correction Model**
+- Vessel speed errors depend on **heel** and **speed** and are assumed constant for a given speed and heel angle.
+- A correction table stores speed errors for steps in speed and heel angle.
+- Each cell in the correction table contains a **Kalman filter** that updates speed corrections with new measurements.
+- The speed measurement is corrected by subtracting the correction from the correction table. The correction is **interpolated** based on nearby table cells, weighted by the uncertainty provided by the Kalman filter.
+- ![correction model](correctionModel.png)
+
+### **2. Current Estimation Model**
+- The water current is estimated as a **slowly changing** vector.
+- A **Kalman filter** updates the current based on the difference between GPS speed and corrected speed.
+- ![current estimation model](currentModel.png)
+
+---
+
+

package/correctionTable.js

@@ -0,0 +1,179 @@
+const { Table2D} = require('signalkutilities');
+const { KalmanFilter, State } = require('kalman-filter');
+
+class CorrectionTable extends Table2D{
+  /**
+   * Represents a 2D correction table for heel and speed.
+   */
+
+
+
+  static fromJSON(data, stability) {
+    const table = new CorrectionTable(data.row, data.col, stability);
+    table.table = data.table.map(row => row.map(cellData => CorrectionEstimator.fromJSON(cellData)));
+    return table;
+  }
+
+  constructor(row, col, stability=5) {
+    super(row, col, CorrectionEstimator, CorrectionEstimator.getFilterModel(stability));
+    this.id = "correctionTable";
+    this.label = "Correction Table";
+  }
+  
+  update(speed, heel, groundSpeed, current, boatSpeed, heading) {
+    this.getCell(speed, heel)?.update(groundSpeed, current, boatSpeed, heading);
+  }
+
+  getKalmanCorrection(speed, heel) {
+    /**
+   * Return a estimated correction based on interpolation and weighting of correction variance;
+   */
+    if (speed == 0) return { x: 0, y: 0 };
+    const neighbours = this.findNeighbours(speed, heel);
+    let totalWeightX   = 0;
+    let totalWeightY   = 0;
+    let x = 0;
+    let y = 0;
+    for (const neighbour of neighbours) {
+      const { cell:correction, xdist, ydist } = neighbour;
+      if (correction.N > 0) {
+        const weightX = 1 / (xdist + 1e-6) ;
+        const weightY = 1 / (ydist + 1e-6) ;
+        const varianceX = correction.covariance[0][0];
+        const varianceY = correction.covariance[1][1];
+        const N = correction.N;
+        const varWeightX = weightX * N / (varianceX + 1e-6);
+        const varWeightY = weightY * N / (varianceY + 1e-6);
+        x += correction.x * varWeightX;
+        y += correction.y * varWeightY; 
+        totalWeightX += varWeightX;
+        totalWeightY += varWeightY;
+      }
+    }
+    if (totalWeightX == 0 || totalWeightY == 0) return { x: 0, y: 0 };  
+    x = x / totalWeightX;
+    y = y / totalWeightY;
+    return { x, y };
+
+  }
+  
+  getInfo() {
+    return {
+      row: { min: this.min[0], max: this.max[0], step: this.step[0] },
+      col: { min: this.min[1], max: this.max[1], step: this.step[1] },
+      table: this.table.map(row => row.map(correction => correction.getInfo()))
+    };
+  }
+
+}
+
+class CorrectionEstimator {
+  /**
+   * Represents a Kalman correction at a cell in a correction table
+   */
+
+  static fromJSON(data, stability) {
+    const filterModel = CorrectionEstimator.getFilterModel(stability);
+    const estimator = new CorrectionEstimator(filterModel, data.state);
+    return estimator;
+  }
+
+  static getFilterModel(stability = 5) {
+    return {
+      observation: {
+        stateProjection: [[1, 0], [0, 1]], // observation matrix H
+        covariance: [[10 ** stability, 0], [0, 10 ** stability]], //measurement noise R
+        dimension: 2
+      },
+      dynamic: {
+        transition: [[1, 0], [0, 1]], // state transition matrix F
+        covariance: [1, 1],// process noise covariance matrix Q
+      }
+    };
+  }
+
+  constructor(filterModel, initialState) {
+    this.filter = new KalmanFilter(filterModel);
+    this.filterState = null;
+    if (initialState != null) {
+      this.filterState = new State(initialState);
+    }
+  }
+  
+  update(groundSpeed, current, boatSpeed, heading) {
+    if (groundSpeed.n < 2 || current.n < 2 || boatSpeed.n < 2) return;
+    // Rotation matrix for -theta
+    const cosTheta = Math.cos(heading);
+    const sinTheta = Math.sin(heading);
+
+    const rotateValue = (vector) => ([
+      cosTheta * vector[0] + sinTheta * vector[1],
+      -sinTheta * vector[0] + cosTheta * vector[1]]
+    );
+
+    const rotateVariance = (vector) => (
+      [
+        [vector[0] * cosTheta ** 2 + vector[1] * sinTheta ** 2,
+        (vector[0] - vector[1]) * cosTheta * sinTheta],
+        [(vector[0] - vector[1]) * cosTheta * sinTheta,
+        vector[0] * sinTheta ** 2 + vector[1] * cosTheta ** 2]
+      ]
+    );
+
+    var groundVector = rotateValue(groundSpeed.obs);
+    var currentVector = rotateValue(current.obs);
+    var boatVector = boatSpeed.obs;
+
+    const observation = [
+      -boatVector[0] + groundVector[0] - currentVector[0],
+      -boatVector[1] + groundVector[1] - currentVector[1]
+    ];
+
+    var groundCov = rotateVariance(groundSpeed.variance);
+    var currentCov = rotateVariance(current.variance);
+    var boatCov = [[boatSpeed.variance[0], 0], [0, boatSpeed.variance[1]]];
+
+    const observationCovariance = [[
+      groundCov[0][0] + currentCov[0][0] + boatCov[0][0],
+      groundCov[0][1] + currentCov[0][1] + boatCov[0][1]],
+    [
+      groundCov[1][0] + currentCov[1][0] + boatCov[1][0],
+      groundCov[1][1] + currentCov[1][1] + boatCov[1][1]],
+    ];
+
+    this.filterState = this.filter.filter({ previousCorrected: this.filterState, observation, observationCovariance });
+  }
+
+
+  getInfo() {
+    return { x: this.x, y: this.y, N: this.N };
+  }
+
+  get N() {
+    if (this.filterState == null) return 0;
+    return this.filterState.index;
+  }
+
+  get x() {
+    if (this.filterState == null) return 0;
+    return this.filterState.mean[0][0];
+  }
+
+  get y() {
+    if (this.filterState == null) return 0;
+    return this.filterState.mean[1][0];
+  }
+
+  get covariance() {
+    return this.filterState.covariance;
+  }
+
+
+  toJSON() {
+    return this.N != 0 ? { state: this.filterState } : { state: null };
+  }
+
+}
+
+
+module.exports = { CorrectionTable };