hrmmar: Add fft elimination algorithm to remove MA

The fft with size=256 is calculated over a window of 8 seconds with step size 2
seconds. Sample rate of acceleration (12.5Hz) and PPG sensor (25Hz) is unchanged, so the algorithm runs with 12.5Hz.
Generally the maximum peak in fft spectrum in an interval of -5..+10BPM of
last one is used.

The low fft resolution limits the accuracy to ~3BPM, but mean error
is ~5-10BPM.

If firmware reports confidence >= 90% its value is used.

apps/hrmmar/README.md

@@ -0,0 +1,11 @@
+# HRM Motion Artifacts removal
+
+Measurements from the build in PPG-Sensor (Photoplethysmograph) is sensitive to motion and can be corrupted with Motion Artifacts (MA). This module allows to remove these.
+
+## Settings
+
+* **MA removal**
+
+Select the algorithm to Remove Motion artifacts:
+  - None: (default) No Motion Artifact removal.
+  - fft elim: (*experimental*) Remove Motion Artifacts by cutting out the frequencies from the HRM frequency spectrum that are noisy in acceleration spectrum. Under motion this can report a heart rate that is closer to the real one but will fail if motion frequency and heart rate overlap.

apps/hrmmar/boot.js

@@ -0,0 +1,40 @@
+{
+  let bpm_corrected; // result of algorithm
+
+  const updateHrm = (bpm) => {
+    bpm_corrected = bpm;
+  };
+
+  Bangle.on('HRM', (hrm) => {
+    if (bpm_corrected > 0) {
+      // replace bpm data in event
+      hrm.bpm_orig = hrm.bpm;
+      hrm.confidence_orig = hrm.confidence;
+      hrm.bpm = bpm_corrected;
+      hrm.confidence = 0;
+    }
+  });
+
+  let run = () => {
+    const settings = Object.assign({
+        mAremoval: 0
+    }, require("Storage").readJSON("hrmmar.json", true) || {});
+
+    // select motion artifact removal algorithm
+    switch(settings.mAremoval) {
+      case 1:
+        require("hrmfftelim").run(settings, updateHrm);
+      break;
+    }
+  }
+
+  // override setHRMPower so we can run our code on HRM enable
+  const oldSetHRMPower = Bangle.setHRMPower;
+  Bangle.setHRMPower = function(on, id) {
+    if (on && run !== undefined) {
+      run();
+      run = undefined; // Make sure we run only once
+    }
+    return oldSetHRMPower(on, id);
+  };
+}

apps/hrmmar/fftelim.js

@@ -0,0 +1,190 @@
+exports.run = (settings, updateHrm) => {
+  const SAMPLE_RATE = 12.5;
+  const NUM_POINTS = 256; // fft size
+  const ACC_PEAKS = 2; // remove this number of ACC peaks
+
+  // ringbuffers
+  const hrmvalues = new Int16Array(8*SAMPLE_RATE);
+  const accvalues = new Int16Array(8*SAMPLE_RATE);
+  // fft  buffers
+  const hrmfftbuf = new Int16Array(NUM_POINTS);
+  const accfftbuf = new Int16Array(NUM_POINTS);
+  let BPM_est_1 = 0;
+  let BPM_est_2 = 0;
+
+  let hrmdata;
+  let idx=0, wraps=0;
+
+  // init settings
+  Bangle.setOptions({hrmPollInterval: 40, powerSave: false}); // hrm=25Hz
+  Bangle.setPollInterval(80); // 12.5Hz
+
+  calcfft = (values, idx, normalize, fftbuf) => {
+    fftbuf.fill(0);
+    let i_out=0;
+    let avg = 0;
+    if (normalize) {
+      const sum = values.reduce((a, b) => a + b, 0);
+      avg = sum/values.length;
+    }
+    // sort ringbuffer to fft buffer
+    for(let i_in=idx; i_in<values.length; i_in++, i_out++) {
+      fftbuf[i_out] = values[i_in]-avg;
+    }
+    for(let i_in=0; i_in<idx; i_in++, i_out++) {
+      fftbuf[i_out] = values[i_in]-avg;
+    }
+
+    E.FFT(fftbuf);
+    return fftbuf;
+  };
+
+  getMax = (values) => {
+    let maxVal = -Number.MAX_VALUE;
+    let maxIdx = 0;
+
+    values.forEach((value,i) => {
+      if (value > maxVal) {
+        maxVal = value;
+        maxIdx = i;
+      }
+    });
+    return {idx: maxIdx, val: maxVal};
+  };
+
+  getSign = (value) => {
+    return value < 0 ? -1 : 1;
+  };
+
+  // idx in fft buffer to frequency
+  getFftFreq = (idx, rate, size) => {
+    return idx*rate/(size-1);
+  };
+
+  // frequency to idx in fft buffer
+  getFftIdx = (freq, rate, size) => {
+    return Math.round(freq*(size-1)/rate);
+  };
+
+  calc2ndDeriative = (values) => {
+    const result = new Int16Array(values.length-2);
+    for(let i=1; i<values.length-1; i++) {
+      const diff = values[i+1]-2*values[i]+values[i-1];
+      result[i-1] = diff;
+    }
+    return result;
+  };
+
+  const minFreqIdx = getFftIdx(1.0, SAMPLE_RATE, NUM_POINTS); // 60 BPM
+  const maxFreqIdx = getFftIdx(3.0, SAMPLE_RATE, NUM_POINTS); // 180 BPM
+  let rangeIdx = [0, maxFreqIdx-minFreqIdx]; // range of search for the next estimates
+  const freqStep=getFftFreq(1, SAMPLE_RATE, NUM_POINTS)*60;
+  const maxBpmDiffIdxDown = Math.ceil(5/freqStep); // maximum down BPM
+  const maxBpmDiffIdxUp = Math.ceil(10/freqStep); // maximum up BPM
+
+  calculate = (idx) => {
+    // fft
+    const ppg_fft = calcfft(hrmvalues, idx, true, hrmfftbuf).subarray(minFreqIdx, maxFreqIdx+1);
+    const acc_fft = calcfft(accvalues, idx, false, accfftbuf).subarray(minFreqIdx, maxFreqIdx+1);
+
+    // remove spectrum that have peaks in acc fft from ppg fft
+    const accGlobalMax = getMax(acc_fft);
+    const acc2nddiff = calc2ndDeriative(acc_fft); // calculate second derivative
+    for(let iClean=0; iClean < ACC_PEAKS; iClean++) {
+      // get max peak in ACC
+      const accMax = getMax(acc_fft);
+
+      if (accMax.val >= 10 && accMax.val/accGlobalMax.val > 0.75) {
+        // set all values in PPG FFT to zero until second derivative of ACC has zero crossing
+        for (let k = accMax.idx-1; k>=0; k--) {
+          ppg_fft[k] = 0;
+          acc_fft[k] = -Math.abs(acc_fft[k]); // max(acc_fft) should no longer find this
+          if (k-2 > 0 && getSign(acc2nddiff[k-1-2]) != getSign(acc2nddiff[k-2]) && Math.abs(acc_fft[k]) < accMax.val*0.75) {
+            break;
+          }
+        }
+        // set all values in PPG FFT to zero until second derivative of ACC has zero crossing
+        for (let k = accMax.idx; k < acc_fft.length-1; k++) {
+          ppg_fft[k] = 0;
+          acc_fft[k] = -Math.abs(acc_fft[k]); // max(acc_fft) should no longer find this
+          if (k-2 >= 0 && getSign(acc2nddiff[k+1-2]) != getSign(acc2nddiff[k-2]) && Math.abs(acc_fft[k]) < accMax.val*0.75) {
+            break;
+          }
+        }
+      }
+    }
+
+    // bpm result is maximum peak in PPG fft
+    const hrRangeMax = getMax(ppg_fft.subarray(rangeIdx[0], rangeIdx[1]));
+    const hrTotalMax = getMax(ppg_fft);
+    const maxDiff = hrTotalMax.val/hrRangeMax.val;
+    let idxMaxPPG = hrRangeMax.idx+rangeIdx[0]; // offset range limit
+
+    if ((maxDiff > 3 && idxMaxPPG != hrTotalMax.idx) || hrRangeMax.val === 0) { // prevent tracking from loosing the real heart rate by checking the full spectrum
+      if (hrTotalMax.idx > idxMaxPPG) {
+        idxMaxPPG = idxMaxPPG+Math.ceil(6/freqStep); // step 6 BPM up into the direction of max peak
+      } else {
+        idxMaxPPG = idxMaxPPG-Math.ceil(2/freqStep); // step 2 BPM down into the direction of max peak
+      }
+    }
+
+    idxMaxPPG = idxMaxPPG + minFreqIdx;
+    const BPM_est_0 = getFftFreq(idxMaxPPG, SAMPLE_RATE, NUM_POINTS)*60;
+
+    // smooth with moving average
+    let BPM_est_res;
+    if (BPM_est_2 > 0) {
+      BPM_est_res = 0.9*BPM_est_0 + 0.05*BPM_est_1 + 0.05*BPM_est_2;
+    } else {
+      BPM_est_res = BPM_est_0;
+    }
+
+    return BPM_est_res.toFixed(1);
+  };
+
+  Bangle.on('HRM-raw', (hrm) => {
+    hrmdata = hrm;
+  });
+
+  Bangle.on('accel', (acc) => {
+    if (hrmdata !== undefined) {
+      hrmvalues[idx] = hrmdata.filt;
+      accvalues[idx] = acc.x*1000 + acc.y*1000 + acc.z*1000;
+      idx++;
+      if (idx >= 8*SAMPLE_RATE) {
+        idx = 0;
+        wraps++;
+      }
+
+      if (idx % (SAMPLE_RATE*2) == 0) { // every two seconds
+        if (wraps === 0) { // use rate of firmware until hrmvalues buffer is filled
+          updateHrm(undefined);
+          BPM_est_2 = BPM_est_1;
+          BPM_est_1 = hrmdata.bpm;
+        } else {
+          let bpm_result;
+          if (hrmdata.confidence >= 90) { // display firmware value if good
+            bpm_result = hrmdata.bpm;
+            updateHrm(undefined);
+          } else {
+            bpm_result = calculate(idx);
+            bpm_corrected = bpm_result;
+            updateHrm(bpm_result);
+          }
+          BPM_est_2 = BPM_est_1;
+          BPM_est_1 = bpm_result;
+
+          // set search range of next BPM
+          const est_res_idx = getFftIdx(bpm_result/60, SAMPLE_RATE, NUM_POINTS)-minFreqIdx;
+          rangeIdx = [est_res_idx-maxBpmDiffIdxDown, est_res_idx+maxBpmDiffIdxUp];
+          if (rangeIdx[0] < 0) {
+            rangeIdx[0] = 0;
+          }
+          if (rangeIdx[1] > maxFreqIdx-minFreqIdx) {
+            rangeIdx[1] = maxFreqIdx-minFreqIdx;
+          }
+        }
+      }
+    }
+  });
+};

apps/hrmmar/metadata.json

@@ -0,0 +1,18 @@
+{
+  "id": "hrmmar",
+  "name": "HRM Motion Artifacts removal",
+  "shortName":"HRM MA removal",
+  "icon": "app.png",
+  "version":"0.01",
+  "description": "Removes Motion Artifacts in Bangle.js's heart rate sensor data.",
+  "type": "bootloader",
+  "tags": "health",
+  "supports": ["BANGLEJS","BANGLEJS2"],
+  "readme": "README.md",
+  "storage": [
+    {"name":"hrmmar.boot.js","url":"boot.js"},
+    {"name":"hrmfftelim","url":"fftelim.js"},
+    {"name":"hrmmar.settings.js","url":"settings.js"}
+  ],
+  "data": [{"name":"hrmmar.json"}]
+}

apps/hrmmar/settings.js

@@ -0,0 +1,26 @@
+(function(back) {
+  var FILE = "hrmmar.json";
+  // Load settings
+  var settings = Object.assign({
+   mAremoval: 0,
+  }, require('Storage').readJSON(FILE, true) || {});
+
+  function writeSettings() {
+    require('Storage').writeJSON(FILE, settings);
+  }
+
+  // Show the menu
+  E.showMenu({
+    "" : { "title" : "HRM MA removal" },
+    "< Back" : () => back(),
+    'MA removal': {
+      value: settings.mAremoval,
+      min: 0, max: 1,
+      format: v => ["None", "fft elim."][v],
+      onchange: v => {
+        settings.mAremoval = v;
+        writeSettings();
+      }
+    },
+  });
+})