Merge pull request #684 from nujw/master

Kalman filter to smooth alt and speed readings.

apps.json

@@ -2677,7 +2677,7 @@
   "name": "GPS Adventure Sports",
   "shortName":"GPS Adv Sport",
   "icon": "app.png",
-  "version":"1.00",
+  "version":"1.02",
   "description": "GPS speed, altitude and distance to waypoint display. Designed for easy viewing and use during outdoor activities such as para-gliding, hang-gliding, sailing, cycling etc.",
   "tags": "tool,outdoors",
   "type":"app",

apps/speedalt/ChangeLog

@@ -6,3 +6,5 @@
 0.06: Tweaks to vibration settings.
 0.07: Switch to BTN1 for Max toggle and reset function.
 1.00: New features. Added waypoints file and distance to selected waypoint display. Added integration with GPS Setup module to switch GPS to low power mode when screen off. Save display settings and restore when app restarted.
+1.01: Add third screen mode with large clock and waypoint selection display to ease visibility in bright daylight.
+1.02: Add Kalman filter to smooth the speed and altitude values. Can be disabled in settings.

apps/speedalt/README.md

@@ -1,14 +1,14 @@
 # GPS Speed, Altimeter and Distance to Waypoint
 
-You can switch between two display modes. One showing speed and altitude (A) and one showing speed and distance to waypoint (D). 
+You can switch between three display modes. One showing speed and altitude (A), one showing speed and distance to waypoint (D) and a large dispay of time and selected waypoint. 
 
-Within each display mode one figure is displayed on the watch face using the largest possible characters depending on the number of digits. The other is in a smaller characters below that. Both are always visible. You can display the current or maximum observed speed/altitude values. Current time is always displayed. 
+Within the [A]ltitude and [D]istance displays modes one figure is displayed on the watch face using the largest possible characters depending on the number of digits. The other is in a smaller characters below that. Both are always visible. You can display the current or maximum observed speed/altitude values. Current time is always displayed. 
 
 The waypoints list is the same as that used with the [GPS Navigation](https://banglejs.com/apps/#gps%20navigation) app so the same set of waypoints can be used across both apps. Refer to that app for waypoint file information.
 
 ## Buttons and Controls
 
-BTN3 : Swaps the modes between Speed+[A]ltitude or Speed+[D]istance.
+BTN3 : Cycles the modes between Speed+[A]ltitude,  Speed+[D]istance and large Time/Waypoint
 
 ### [A]ltitude mode
 
@@ -20,9 +20,13 @@ BTN1 : Long press > 2 secs resets the recorded maximum values.
 
 BTN1 : Select next waypoint. Last fix distance from selected waypoint is displayed.
 
-### Both modes
+### Large mode
 
-BTN2 : Disables/Restores power saving timeout. Locks the screen on to enable reading for longer periods but uses maximum battery drain. Red LED (dot) at top of screen when screen is locked on. Press again to restore power saving timeouts.
+BTN1 : Select next waypoint.
+
+### All modes
+
+BTN2 : Disables/Restores power saving timeout. Locks the screen on and GPS in SuperE mode to enable reading for longer periods but uses maximum battery drain. Red LED (dot) at top of screen when screen is locked on. Press again to restore power saving timeouts.
 
 BTN3 : Long press exit and return to watch.
 
@@ -32,6 +36,10 @@ BTN4 : Left Display Tap : Swaps which figure is in the large display. You can ha
 
 Select the desired display units. Speed can be as per the default locale, kph, knots, mph or m/s. Distance can be km, miles or nautical miles. Altitude can be feet or metres. Select one of three colour schemes. Default (three colours), high contrast (all white on black) or night ( all red on black ). 
 
+## Kalman Filter
+
+This filter smooths the altitude and the speed values and reduces these values 'jumping around' from one GPS fix to the next. The down side of this is that if these values change rapidly ( eg. a quick change in altitude ) then it can take a few GPS fixes for the values to move to the new vlaues. Disabling the Kalman filter in the settings will cause the raw values to be displayed from each GPS fix as they are found.
+
 ## Loss of fix
 
 When the GPS obtains a fix the number of satellites is displayed as 'Sats:nn'. When unable to obtain a fix then the last known fix is used and the age of that fix in seconds is displayed as 'Age:nn'. Seeing 'Sats'  or 'Age' indicates whether the GPS has a current fix or not.  
@@ -138,6 +146,6 @@ Developed for my use in sailing, cycling and motorcycling. If you find this soft
 
 Many thanks to Gordon Williams. Awesome job.
 
-Special thanks also to @jeffmer, for the [GPS Navigation](https://banglejs.com/apps/#gps%20navigation) app and @hughbarney for the Low power GPS code development.
+Special thanks also to @jeffmer, for the [GPS Navigation](https://banglejs.com/apps/#gps%20navigation) app and @hughbarney for the Low power GPS code development and Wouter Bulten for the Kalman filter code.
 
 

apps/speedalt/app.js

@@ -1,9 +1,176 @@
 /*
 Speed and Altitude [speedalt]
 Mike Bennett mike[at]kereru.com
-1.16 : Use new GPS settings module
+1.00 : Use new GPS settings module
+1.01 : Third mode large clock display
+1.02 : add smoothing with kalman filter
 */
-var v = '1.20';
+var v = '1.02g';
+
+/*kalmanjs, Wouter Bulten, MIT, https://github.com/wouterbulten/kalmanjs */
+var KalmanFilter = (function () {
+  'use strict';
+
+  function _classCallCheck(instance, Constructor) {
+    if (!(instance instanceof Constructor)) {
+      throw new TypeError("Cannot call a class as a function");
+    }
+  }
+
+  function _defineProperties(target, props) {
+    for (var i = 0; i < props.length; i++) {
+      var descriptor = props[i];
+      descriptor.enumerable = descriptor.enumerable || false;
+      descriptor.configurable = true;
+      if ("value" in descriptor) descriptor.writable = true;
+      Object.defineProperty(target, descriptor.key, descriptor);
+    }
+  }
+
+  function _createClass(Constructor, protoProps, staticProps) {
+    if (protoProps) _defineProperties(Constructor.prototype, protoProps);
+    if (staticProps) _defineProperties(Constructor, staticProps);
+    return Constructor;
+  }
+
+  /**
+  * KalmanFilter
+  * @class
+  * @author Wouter Bulten
+  * @see {@link http://github.com/wouterbulten/kalmanjs}
+  * @version Version: 1.0.0-beta
+  * @copyright Copyright 2015-2018 Wouter Bulten
+  * @license MIT License
+  * @preserve
+  */
+  var KalmanFilter =
+  /*#__PURE__*/
+  function () {
+    /**
+    * Create 1-dimensional kalman filter
+    * @param  {Number} options.R Process noise
+    * @param  {Number} options.Q Measurement noise
+    * @param  {Number} options.A State vector
+    * @param  {Number} options.B Control vector
+    * @param  {Number} options.C Measurement vector
+    * @return {KalmanFilter}
+    */
+    function KalmanFilter() {
+      var _ref = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : {},
+          _ref$R = _ref.R,
+          R = _ref$R === void 0 ? 1 : _ref$R,
+          _ref$Q = _ref.Q,
+          Q = _ref$Q === void 0 ? 1 : _ref$Q,
+          _ref$A = _ref.A,
+          A = _ref$A === void 0 ? 1 : _ref$A,
+          _ref$B = _ref.B,
+          B = _ref$B === void 0 ? 0 : _ref$B,
+          _ref$C = _ref.C,
+          C = _ref$C === void 0 ? 1 : _ref$C;
+
+      _classCallCheck(this, KalmanFilter);
+
+      this.R = R; // noise power desirable
+
+      this.Q = Q; // noise power estimated
+
+      this.A = A;
+      this.C = C;
+      this.B = B;
+      this.cov = NaN;
+      this.x = NaN; // estimated signal without noise
+    }
+    /**
+    * Filter a new value
+    * @param  {Number} z Measurement
+    * @param  {Number} u Control
+    * @return {Number}
+    */
+
+
+    _createClass(KalmanFilter, [{
+      key: "filter",
+      value: function filter(z) {
+        var u = arguments.length > 1 && arguments[1] !== undefined ? arguments[1] : 0;
+
+        if (isNaN(this.x)) {
+          this.x = 1 / this.C * z;
+          this.cov = 1 / this.C * this.Q * (1 / this.C);
+        } else {
+          // Compute prediction
+          var predX = this.predict(u);
+          var predCov = this.uncertainty(); // Kalman gain
+
+          var K = predCov * this.C * (1 / (this.C * predCov * this.C + this.Q)); // Correction
+
+          this.x = predX + K * (z - this.C * predX);
+          this.cov = predCov - K * this.C * predCov;
+        }
+
+        return this.x;
+      }
+      /**
+      * Predict next value
+      * @param  {Number} [u] Control
+      * @return {Number}
+      */
+
+    }, {
+      key: "predict",
+      value: function predict() {
+        var u = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : 0;
+        return this.A * this.x + this.B * u;
+      }
+      /**
+      * Return uncertainty of filter
+      * @return {Number}
+      */
+
+    }, {
+      key: "uncertainty",
+      value: function uncertainty() {
+        return this.A * this.cov * this.A + this.R;
+      }
+      /**
+      * Return the last filtered measurement
+      * @return {Number}
+      */
+
+    }, {
+      key: "lastMeasurement",
+      value: function lastMeasurement() {
+        return this.x;
+      }
+      /**
+      * Set measurement noise Q
+      * @param {Number} noise
+      */
+
+    }, {
+      key: "setMeasurementNoise",
+      value: function setMeasurementNoise(noise) {
+        this.Q = noise;
+      }
+      /**
+      * Set the process noise R
+      * @param {Number} noise
+      */
+
+    }, {
+      key: "setProcessNoise",
+      value: function setProcessNoise(noise) {
+        this.R = noise;
+      }
+    }]);
+
+    return KalmanFilter;
+  }();
+
+  return KalmanFilter;
+
+}());
+
+
 var buf = Graphics.createArrayBuffer(240,160,2,{msb:true});
 
 // Load fonts
@@ -19,13 +186,13 @@ var tmrLP;            // Timer for delay in switching to low power after screen
 var max = {};
 max.spd = 0;
 max.alt = 0;
+max.n = 0;    // counter. Only start comparing for max after a certain number of fixes to allow kalman filter to have smoohed the data.
 
 var emulator = (process.env.BOARD=="EMSCRIPTEN")?1:0;  // 1 = running in emulator. Supplies test values;
 
 var wp = {};        // Waypoint to use for distance from cur position.
 
 function nxtWp(inc){
-  if (cfg.modeA) return;
   cfg.wp+=inc;
   loadWp();
 }
@@ -98,6 +265,15 @@ function drawFix(dat) {
   
 }
 
+function drawClock() {
+  if (!canDraw) return;
+  buf.clear();
+  drawTime();
+  drawWP();
+  g.reset();
+  g.drawImage(img,0,40);
+}
+
 function drawPrimary(n,u) {
   
   // Primary Display
@@ -107,7 +283,7 @@ function drawPrimary(n,u) {
   
   if ( l <= 7 ) s=48;
   if ( l <= 6 ) s=55;
-  if ( l <= 5 ) s=68;
+  if ( l <= 5 ) s=66;
   if ( l <= 4 ) s=85;
   if ( l <= 3 ) s=110;
         
@@ -116,10 +292,12 @@ function drawPrimary(n,u) {
   buf.setFontVector(s);
   buf.drawString(n,110,0);
  
+ 
+  
     // Primary Units
   buf.setFontAlign(1,-1,3); //right
   buf.setColor(2);  
-  buf.setFontVector(25);
+  buf.setFontVector(35);
   buf.drawString(u,210,0);  
 }
 
@@ -141,16 +319,26 @@ function drawSecondary(n,u) {
   // Secondary Units
   buf.setFontAlign(-1,1); //left, bottom
   buf.setColor(2);  
-  buf.setFontVector(25);
+  buf.setFontVector(30);
   buf.drawString(u,s,135);
 }
 
 function drawTime() {
-  var x = 0;
+  var x, y;
-  var y = 160;
   
+  if ( cfg.modeA == 2 ) {
+    x=120;
+    y=0;
+    buf.setFontAlign(0,-1); 
+    buf.setFontVector(80);
+  }
+  else {
+    x = 0;
+    y = 160;
+    buf.setFontAlign(-1,1);
     buf.setFont("7x11Numeric7Seg", 2);
-  buf.setFontAlign(-1,1); //left, bottom
+  }
+
 
   buf.setColor(0);
   buf.drawString(time,x,y);
@@ -161,12 +349,20 @@ function drawTime() {
 
 function drawWP() {
   var nm = wp.name;
-  if ( nm == undefined || nm == 'NONE' || cfg.modeA ) nm = '';
+  if ( nm == undefined || nm == 'NONE' || cfg.modeA ==1 ) nm = '';
-  
-  buf.setFontAlign(-1,1); //left, bottom
   buf.setColor(2);  
+  
+  if ( cfg.modeA == 0 ) {  // dist mode
+    buf.setFontAlign(-1,1); //left, bottom
     buf.setFontVector(20);
-  buf.drawString(nm.substring(0,6),77,160);  
+    buf.drawString(nm.substring(0,6),72,160);  
+  }
+
+  if ( cfg.modeA == 2 ) {  // clock/large mode
+    buf.setFontAlign(0,1); //left, bottom
+    buf.setFontVector(55);
+    buf.drawString(nm.substring(0,6),120,160);  
+  }
   
 }
 
@@ -177,31 +373,31 @@ function drawSats(sats) {
   buf.setFontAlign(1,1); //right, bottom
   buf.drawString(sats,240,160);  
 
-  buf.setFontVector(20);
+  buf.setFontVector(30);
   buf.setColor(2); 
   
-  if ( cfg.modeA ) buf.drawString("A",240,140);
+  if ( cfg.modeA == 1 ) {
-  else buf.drawString("D",240,140);
+    buf.drawString('A',240,140);
-    
+    if ( showMax ) {
-  if ( showMax && cfg.modeA ) {
       buf.setFontAlign(0,1); //centre, bottom
-    buf.drawString("MAX",120,164);
+      buf.drawString('MAX',120,164);
     }
-  
+  }
-
+  if ( cfg.modeA == 0 ) buf.drawString('D',240,140);
 }
 
 function onGPS(fix) {
   
  if ( emulator ) {
     fix.fix = 1;
-    fix.speed = 10;
+    fix.speed = 10 + (Math.random()*5);
-    fix.alt = 354;
+    fix.alt = 354 + (Math.random()*50);
     fix.lat = -38.92;
     fix.lon = 175.7613350;   
     fix.course = 245;
     fix.satellites = 12;
     fix.time = new Date();
+    fix.smoothed = 0;
   }
 
   var m;
@@ -212,8 +408,18 @@ function onGPS(fix) {
   var age = '---';
 
   if (fix.fix) lf = fix;
+
     if (lf.fix) {
 
+    // Smooth data
+    if ( lf.smoothed !== 1 ) {
+      if ( cfg.spdFilt ) lf.speed = spdFilter.filter(lf.speed);
+      if ( cfg.altFilt ) lf.alt = altFilter.filter(lf.alt);
+      lf.smoothed = 1;
+      if ( max.n <= 15 ) max.n++;
+    }
+  
+    
     // Speed
     if ( cfg.spd == 0 ) {
       m = require("locale").speed(lf.speed).match(/([0-9,\.]+)(.*)/); // regex splits numbers from units
@@ -224,12 +430,12 @@ function onGPS(fix) {
     
     if ( sp < 10 ) sp = sp.toFixed(1);
     else sp = Math.round(sp);
-    if (parseFloat(sp) > parseFloat(max.spd) ) max.spd = parseFloat(sp);
+    if (parseFloat(sp) > parseFloat(max.spd) && max.n > 15 ) max.spd = parseFloat(sp);
 
     // Altitude
     al = lf.alt;
     al = Math.round(parseFloat(al)/parseFloat(cfg.alt));
-    if (parseFloat(al) > parseFloat(max.alt) ) max.alt = parseFloat(al);
+    if (parseFloat(al) > parseFloat(max.alt) && max.n > 15 ) max.alt = parseFloat(al);
 
     // Distance to waypoint
     di = distance(lf,wp);
@@ -239,7 +445,7 @@ function onGPS(fix) {
     age = Math.max(0,Math.round(getTime())-(lf.time.getTime()/1000));
   }
       
-  if ( cfg.modeA ) {
+  if ( cfg.modeA == 1 ) {
     if ( showMax ) 
       drawFix({
         speed:max.spd,
@@ -259,7 +465,7 @@ function onGPS(fix) {
         fix:lf.fix
       }); // Show speed/altitude
   }
-  else {
+  if ( cfg.modeA == 0 )  {
     // Show speed/distance
     if ( di <= 0 ) 
       drawFix({
@@ -280,6 +486,10 @@ function onGPS(fix) {
         fix:lf.fix
       });
   }
+  if ( cfg.modeA == 2 )  {
+    // Large clock
+    drawClock();
+  }
 
 }
 
@@ -288,12 +498,12 @@ function setButtons(){
   // Spd+Dist : Select next waypoint
   setWatch(function(e) {
     var dur = e.time - e.lastTime;
-    if ( cfg.modeA ) {
+    if ( cfg.modeA == 1 ) {
       // Spd+Alt mode - Switch between fix and MAX
       if ( dur < 2 ) showMax = !showMax;   // Short press toggle fix/max display
       else { max.spd = 0; max.alt = 0; }  // Long press resets max values.
     }
-    else nxtWp(1);  // Spd+Dist mode - Select next waypoint
+    else nxtWp(1);  // Spd+Dist or Clock mode - Select next waypoint
     onGPS(lf);
   }, BTN1, { edge:"falling",repeat:true});
   
@@ -315,7 +525,8 @@ function setButtons(){
   
   // Toggle between alt or dist
   setWatch(function(e){
-    cfg.modeA = !cfg.modeA;
+    cfg.modeA = cfg.modeA+1;
+    if ( cfg.modeA > 2 ) cfg.modeA = 0;
     savSettings();
     onGPS(lf); 
   }, BTN3, {repeat:true,edge:"falling"});
@@ -357,7 +568,7 @@ function savSettings() {
 function setLpMode(m) {
   if (tmrLP) {clearInterval(tmrLP);tmrLP = false;} // Stop any scheduled drop to low power
   if ( !gpssetup ) return;
-  gpssetup.setPowerMode({power_mode:m})
+  gpssetup.setPowerMode({power_mode:m});
 }
 
 // =Main Prog
@@ -373,9 +584,14 @@ cfg.dist = cfg.dist||1000;// Multiplier for distnce unit conversions.
 cfg.dist_unit = cfg.dist_unit||'km';  // Displayed altitude units
 cfg.colour = cfg.colour||0;          // Colour scheme.
 cfg.wp = cfg.wp||0;        // Last selected waypoint for dist
-cfg.modeA = cfg.modeA||0;    // 0 = [D], 1 = [A]
+cfg.modeA = cfg.modeA||0;    // 0 = [D]ist, 1 = [A]ltitude, 2 = [C]lock
 cfg.primSpd = cfg.primSpd||0;    // 1 = Spd in primary, 0 = Spd in secondary
 
+cfg.spdFilt = cfg.spdFilt==undefined?true:cfg.spdFilt; 
+cfg.altFilt = cfg.altFilt==undefined?true:cfg.altFilt;
+
+if ( cfg.spdFilt ) var spdFilter = new KalmanFilter({R: 0.1 , Q: 1 });
+if ( cfg.altFilt ) var altFilter = new KalmanFilter({R: 0.01, Q: 2 });
 
 loadWp();
 
@@ -433,4 +649,4 @@ else {
 Bangle.on('GPS', onGPS);
 
 setButtons();
-setInterval(updateClock, 30000);
+setInterval(updateClock, 10000);

apps/speedalt/settings.js

@@ -30,12 +30,14 @@
     writeSettings();
   }
 
+  
   const appMenu = {
     '': {'title': 'GPS Speed Alt'},
     '< Back': back,
     '< Load GPS Adv Sport': ()=>{load('speedalt.app.js');},
     'Units' : function() { E.showMenu(unitsMenu); },
-    'Colours' : function() { E.showMenu(colMenu); }/*,
+    'Colours' : function() { E.showMenu(colMenu); },
+    'Kalman Filter' : function() { E.showMenu(kalMenu); }/*,
     'Vibrate' : {
     value : settings.buzz,
     format : v => v?"On":"Off",
@@ -48,14 +50,14 @@
     '< Back': function() { E.showMenu(appMenu); },
     'default (spd)' : function() { setUnits(0,''); },
     'Kph (spd)' : function() { setUnits(1,'kph'); },
-    'Knots (spd)' : function() { setUnits(1.852,'knots'); },
+    'Knots (spd)' : function() { setUnits(1.852,'kts'); },
     'Mph (spd)' : function() { setUnits(1.60934,'mph'); },
     'm/s (spd)' : function() { setUnits(3.6,'m/s'); },
     'Km (dist)' : function() { setUnitsDist(1000,'km'); },
-    'Miles (dist)' : function() { setUnitsDist(1609.344,'miles'); },
+    'Miles (dist)' : function() { setUnitsDist(1609.344,'mi'); },
     'Nm (dist)' : function() { setUnitsDist(1852.001,'nm'); },
     'Meters (alt)' : function() { setUnitsAlt(1,'m'); },
-    'Feet (alt)' : function() { setUnitsAlt(0.3048,'feet'); }
+    'Feet (alt)' : function() { setUnitsAlt(0.3048,'ft'); }
   };
 
   const colMenu = {
@@ -66,7 +68,22 @@
     'Night' : function() { setColour(2); }
   };
   
+  const kalMenu = {
+    '': {'title': 'Kalman Filter'},
+    '< Back': function() { E.showMenu(appMenu); },
+    'Speed' : {
+    value : settings.spdFilt,
+    format : v => v?"On":"Off",
+    onchange : () => { settings.spdFilt = !settings.spdFilt; writeSettings(); }
+    },
+    'Altitude' : {
+    value : settings.altFilt,
+    format : v => v?"On":"Off",
+    onchange : () => { settings.altFilt = !settings.altFilt; writeSettings(); }
+    }
+  };
+
   
   E.showMenu(appMenu);
 
-})
+});