From 67d48dad7d6977d1db1a737c7c588bcdd93389ec Mon Sep 17 00:00:00 2001 From: nujw Date: Tue, 2 Mar 2021 12:56:48 +1300 Subject: [PATCH 1/9] Add Kalman filtering to smooth speed and alt. --- apps/speedalt/app.js | 196 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 192 insertions(+), 4 deletions(-) diff --git a/apps/speedalt/app.js b/apps/speedalt/app.js index 4752fed85..7cddba28f 100644 --- a/apps/speedalt/app.js +++ b/apps/speedalt/app.js @@ -3,8 +3,177 @@ Speed and Altitude [speedalt] Mike Bennett mike[at]kereru.com 1.16 : Use new GPS settings module 1.21 : Third mode large clock display +1.25 : add smoothing with kalman filter */ -var v = '1.24'; +var v = '1.25'; + +/*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 spdFilter = new KalmanFilter({R: 0.01, Q: 3}); +var altFilter = new KalmanFilter({R: 0.01, Q: 3}); + + var buf = Graphics.createArrayBuffer(240,160,2,{msb:true}); // Load fonts @@ -223,13 +392,14 @@ function onGPS(fix) { if ( emulator ) { fix.fix = 1; - fix.speed = 10; - fix.alt = 354; + fix.speed = 10 + (Math.random()*5); + 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; @@ -240,7 +410,22 @@ function onGPS(fix) { var age = '---'; if (fix.fix) lf = fix; - if (lf.fix) { + + if (lf.fix) { + + // Smooth data + if ( lf.smoothed !== 1 ) { + + print('u: '+lf.speed+' '+lf.alt); + + lf.speed = spdFilter.filter(lf.speed); + lf.alt = altFilter.filter(lf.alt); + + print('f: '+lf.speed+' '+lf.alt); + + lf.smoothed = 1; + } + // Speed if ( cfg.spd == 0 ) { @@ -410,6 +595,9 @@ 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.spd = 1; +cfg.spd_unit = 'kph'; + loadWp(); /* From 15ec3dc05dc8adfd6e6fdca524dc269d71af6338 Mon Sep 17 00:00:00 2001 From: nujw Date: Tue, 2 Mar 2021 13:07:08 +1300 Subject: [PATCH 2/9] Update apps.json Add kalman filtering to smooth speed and alt. --- apps.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps.json b/apps.json index 7b7e6f2c7..1f39b4ade 100644 --- a/apps.json +++ b/apps.json @@ -2640,7 +2640,7 @@ "name": "GPS Adventure Sports", "shortName":"GPS Adv Sport", "icon": "app.png", - "version":"1.01", + "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", From 70b231ee18a0f0b61c96b81c3a19f3715198792f Mon Sep 17 00:00:00 2001 From: nujw Date: Tue, 2 Mar 2021 13:29:21 +1300 Subject: [PATCH 3/9] Tweaks to filter settings --- apps/speedalt/app.js | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/apps/speedalt/app.js b/apps/speedalt/app.js index 7cddba28f..e24ca3e49 100644 --- a/apps/speedalt/app.js +++ b/apps/speedalt/app.js @@ -3,9 +3,9 @@ Speed and Altitude [speedalt] Mike Bennett mike[at]kereru.com 1.16 : Use new GPS settings module 1.21 : Third mode large clock display -1.25 : add smoothing with kalman filter +1.01b : add smoothing with kalman filter */ -var v = '1.25'; +var v = '1.01b'; /*kalmanjs, Wouter Bulten, MIT, https://github.com/wouterbulten/kalmanjs */ var KalmanFilter = (function () { @@ -171,7 +171,7 @@ var KalmanFilter = (function () { }()); var spdFilter = new KalmanFilter({R: 0.01, Q: 3}); -var altFilter = new KalmanFilter({R: 0.01, Q: 3}); +var altFilter = new KalmanFilter({R: 0.01, Q: 10}); var buf = Graphics.createArrayBuffer(240,160,2,{msb:true}); @@ -575,7 +575,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 @@ -594,10 +594,6 @@ cfg.wp = cfg.wp||0; // Last selected waypoint for dist 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.spd = 1; -cfg.spd_unit = 'kph'; - loadWp(); /* From a116b9583974f02d081c90aa837f8144481ee7a8 Mon Sep 17 00:00:00 2001 From: nujw Date: Tue, 2 Mar 2021 13:32:22 +1300 Subject: [PATCH 4/9] Change ver --- apps/speedalt/app.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/speedalt/app.js b/apps/speedalt/app.js index e24ca3e49..30542590f 100644 --- a/apps/speedalt/app.js +++ b/apps/speedalt/app.js @@ -5,7 +5,7 @@ Mike Bennett mike[at]kereru.com 1.21 : Third mode large clock display 1.01b : add smoothing with kalman filter */ -var v = '1.01b'; +var v = '1.02b'; /*kalmanjs, Wouter Bulten, MIT, https://github.com/wouterbulten/kalmanjs */ var KalmanFilter = (function () { From e706c44ebbf7a08282bef13b01ddc2019b9946a6 Mon Sep 17 00:00:00 2001 From: nujw Date: Tue, 2 Mar 2021 13:36:42 +1300 Subject: [PATCH 5/9] Update app.js --- apps/speedalt/app.js | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/apps/speedalt/app.js b/apps/speedalt/app.js index 30542590f..0b339bd84 100644 --- a/apps/speedalt/app.js +++ b/apps/speedalt/app.js @@ -3,9 +3,9 @@ Speed and Altitude [speedalt] Mike Bennett mike[at]kereru.com 1.16 : Use new GPS settings module 1.21 : Third mode large clock display -1.01b : add smoothing with kalman filter +1.02 : add smoothing with kalman filter */ -var v = '1.02b'; +var v = '1.02c'; /*kalmanjs, Wouter Bulten, MIT, https://github.com/wouterbulten/kalmanjs */ var KalmanFilter = (function () { @@ -415,14 +415,8 @@ function onGPS(fix) { // Smooth data if ( lf.smoothed !== 1 ) { - - print('u: '+lf.speed+' '+lf.alt); - lf.speed = spdFilter.filter(lf.speed); lf.alt = altFilter.filter(lf.alt); - - print('f: '+lf.speed+' '+lf.alt); - lf.smoothed = 1; } From c6e96119d9b71c8f02f2eb2197571f71f2ffc306 Mon Sep 17 00:00:00 2001 From: nujw Date: Wed, 3 Mar 2021 09:31:17 +1300 Subject: [PATCH 6/9] Adjust kalman filter settings. --- apps/speedalt/app.js | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/apps/speedalt/app.js b/apps/speedalt/app.js index 0b339bd84..396c93a5e 100644 --- a/apps/speedalt/app.js +++ b/apps/speedalt/app.js @@ -5,7 +5,7 @@ Mike Bennett mike[at]kereru.com 1.21 : Third mode large clock display 1.02 : add smoothing with kalman filter */ -var v = '1.02c'; +var v = '1.02d'; /*kalmanjs, Wouter Bulten, MIT, https://github.com/wouterbulten/kalmanjs */ var KalmanFilter = (function () { @@ -170,8 +170,8 @@ var KalmanFilter = (function () { }()); -var spdFilter = new KalmanFilter({R: 0.01, Q: 3}); -var altFilter = new KalmanFilter({R: 0.01, Q: 10}); +var spdFilter = new KalmanFilter({R: 0.01, Q: 2}); +var altFilter = new KalmanFilter({R: 0.01, Q: 2}); var buf = Graphics.createArrayBuffer(240,160,2,{msb:true}); @@ -189,6 +189,7 @@ 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; @@ -375,7 +376,7 @@ function drawSats(sats) { buf.setFontAlign(1,1); //right, bottom buf.drawString(sats,240,160); - buf.setFontVector(40); + buf.setFontVector(30); buf.setColor(2); if ( cfg.modeA == 1 ) { @@ -418,6 +419,7 @@ function onGPS(fix) { lf.speed = spdFilter.filter(lf.speed); lf.alt = altFilter.filter(lf.alt); lf.smoothed = 1; + if ( max.n <= 15 ) max.n++; } @@ -431,12 +433,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); From d3cd557e68092362ca015e431bb881d490714471 Mon Sep 17 00:00:00 2001 From: nujw Date: Fri, 5 Mar 2021 10:25:31 +1300 Subject: [PATCH 7/9] Tweak Kalman filter setting for speed. --- apps/speedalt/app.js | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/speedalt/app.js b/apps/speedalt/app.js index 396c93a5e..f69b955a1 100644 --- a/apps/speedalt/app.js +++ b/apps/speedalt/app.js @@ -5,7 +5,7 @@ Mike Bennett mike[at]kereru.com 1.21 : Third mode large clock display 1.02 : add smoothing with kalman filter */ -var v = '1.02d'; +var v = '1.02e'; /*kalmanjs, Wouter Bulten, MIT, https://github.com/wouterbulten/kalmanjs */ var KalmanFilter = (function () { @@ -170,7 +170,7 @@ var KalmanFilter = (function () { }()); -var spdFilter = new KalmanFilter({R: 0.01, Q: 2}); +var spdFilter = new KalmanFilter({R: 0.1, Q: 1}); var altFilter = new KalmanFilter({R: 0.01, Q: 2}); From bef2955ba07058879df642f54deb142da9cc5303 Mon Sep 17 00:00:00 2001 From: nujw Date: Fri, 5 Mar 2021 17:27:05 +1300 Subject: [PATCH 8/9] Add Kalman Filter switch on/off to settings menu. --- apps/speedalt/app.js | 15 +++++++++------ apps/speedalt/settings.js | 20 ++++++++++++++++++-- 2 files changed, 27 insertions(+), 8 deletions(-) diff --git a/apps/speedalt/app.js b/apps/speedalt/app.js index f69b955a1..04bf58380 100644 --- a/apps/speedalt/app.js +++ b/apps/speedalt/app.js @@ -5,7 +5,7 @@ Mike Bennett mike[at]kereru.com 1.21 : Third mode large clock display 1.02 : add smoothing with kalman filter */ -var v = '1.02e'; +var v = '1.02g'; /*kalmanjs, Wouter Bulten, MIT, https://github.com/wouterbulten/kalmanjs */ var KalmanFilter = (function () { @@ -170,9 +170,6 @@ var KalmanFilter = (function () { }()); -var spdFilter = new KalmanFilter({R: 0.1, Q: 1}); -var altFilter = new KalmanFilter({R: 0.01, Q: 2}); - var buf = Graphics.createArrayBuffer(240,160,2,{msb:true}); @@ -416,8 +413,8 @@ function onGPS(fix) { // Smooth data if ( lf.smoothed !== 1 ) { - lf.speed = spdFilter.filter(lf.speed); - lf.alt = altFilter.filter(lf.alt); + 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++; } @@ -590,6 +587,12 @@ cfg.wp = cfg.wp||0; // Last selected waypoint for dist 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(); /* diff --git a/apps/speedalt/settings.js b/apps/speedalt/settings.js index 7ecc2b940..6e417da56 100644 --- a/apps/speedalt/settings.js +++ b/apps/speedalt/settings.js @@ -29,13 +29,15 @@ settings.colour = c; 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", @@ -66,7 +68,21 @@ 'Night' : function() { setColour(2); } }; + const kalMenu = { + '': {'title': 'Kalman Filter'}, + '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); -}) +}); From c88b6d58b6ba0984407eafc58ebadd839ab66190 Mon Sep 17 00:00:00 2001 From: nujw Date: Fri, 5 Mar 2021 17:33:56 +1300 Subject: [PATCH 9/9] Fix Kalman menu --- apps/speedalt/settings.js | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/speedalt/settings.js b/apps/speedalt/settings.js index 6e417da56..488ba3b81 100644 --- a/apps/speedalt/settings.js +++ b/apps/speedalt/settings.js @@ -70,6 +70,7 @@ const kalMenu = { '': {'title': 'Kalman Filter'}, + '< Back': function() { E.showMenu(appMenu); }, 'Speed' : { value : settings.spdFilt, format : v => v?"On":"Off",