feat: Added Kalman filtering of BLE RSSI values

The signal strength of BLE peripherals is very noisy, that's why we use
a Kalman filter to smoothen the data. Each peripheral gets its own
filter instance, as the historic values play a role when filtering.

package.json

@@ -31,6 +31,7 @@
     "config": "^3.2.4",
     "democracy": "^3.1.3",
     "js-yaml": "^3.13.1",
+    "kalmanjs": "^1.1.0",
     "lodash": "^4.17.15",
     "mathjs": "^6.5.0",
     "nest-emitter": "^1.1.0",

src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.spec.ts

@@ -1,3 +1,5 @@
+import KalmanFilter from 'kalmanjs';
+
 const mockNoble = {
   on: jest.fn()
 };
@@ -8,6 +10,13 @@ jest.mock(
   },
   { virtual: true }
 );
+jest.mock('kalmanjs', () => {
+  return jest.fn().mockImplementation(() => {
+    return {
+      filter: z => z
+    };
+  });
+});
 
 import { Peripheral } from '@abandonware/noble';
 import { ConfigService } from '../../config/config.service';
@@ -343,6 +352,54 @@ describe('BluetoothLowEnergyService', () => {
     expect(service.isOnWhitelist('test123')).toBeFalsy();
   });
 
+  it('should filter the measured RSSI of the peripherals', () => {
+    const sensor = new Sensor('testid', 'Test');
+    entitiesService.has.mockReturnValue(true);
+    entitiesService.get.mockReturnValue(sensor);
+    jest
+      .spyOn(service, 'handleNewDistance')
+      .mockImplementation(() => undefined);
+    const filterSpy = jest.spyOn(service, 'filterRssi').mockReturnValue(-50);
+
+    service.handleDiscovery({
+      id: '12:ab:cd:12:cd',
+      rssi: -45,
+      advertisement: {
+        localName: 'Test BLE Device'
+      }
+    } as Peripheral);
+
+    expect(filterSpy).toHaveBeenCalledWith('12:ab:cd:12:cd', -45);
+    expect(sensor.state).toBe(0.2);
+  });
+
+  it('should reuse existing Kalman filters for the same id', () => {
+    const sensor = new Sensor('testid', 'Test');
+    entitiesService.has.mockReturnValue(true);
+    entitiesService.get.mockReturnValue(sensor);
+    jest
+      .spyOn(service, 'handleNewDistance')
+      .mockImplementation(() => undefined);
+
+    service.handleDiscovery({
+      id: 'id1',
+      rssi: -45,
+      advertisement: {}
+    } as Peripheral);
+    service.handleDiscovery({
+      id: 'id2',
+      rssi: -67,
+      advertisement: {}
+    } as Peripheral);
+    service.handleDiscovery({
+      id: 'id1',
+      rssi: -56,
+      advertisement: {}
+    } as Peripheral);
+
+    expect(KalmanFilter).toHaveBeenCalledTimes(2);
+  });
+
   it('should pass distance information to existing room presence sensors', () => {
     const sensor = new RoomPresenceDistanceSensor('test', 'Test', 0);
     entitiesService.has.mockReturnValue(true);

src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.ts

@@ -19,13 +19,18 @@ import { EntityCustomization } from '../../entities/entity-customization.interfa
 import { SensorConfig } from '../home-assistant/sensor-config';
 import { RoomPresenceDistanceSensor } from '../room-presence/room-presence-distance.sensor';
 import { SchedulerRegistry } from '@nestjs/schedule';
+import KalmanFilter from 'kalmanjs';
 
 export const NEW_DISTANCE_CHANNEL = 'bluetooth-low-energy.new-distance';
 
 @Injectable()
 export class BluetoothLowEnergyService
   implements OnModuleInit, OnApplicationBootstrap {
   private readonly config: BluetoothLowEnergyConfig;
+  private filterMap: Map<string, KalmanFilter> = new Map<
+    string,
+    KalmanFilter
+  >();
 
   constructor(
     private readonly entitiesService: EntitiesService,
@@ -68,6 +73,7 @@ export class BluetoothLowEnergyService
 
     if (this.isOnWhitelist(tag.id)) {
       tag = this.applyOverrides(tag);
+      tag.rssi = this.filterRssi(tag.id, tag.rssi);
 
       const sensorId = slugify(`ble ${_.lowerCase(tag.id)}`);
       let sensor: Entity;
@@ -141,6 +147,24 @@ export class BluetoothLowEnergyService
       : whitelist.includes(id);
   }
 
+  /**
+   * Applies the Kalman filter based on the historic values with the same tag id.
+   *
+   * @param tagId - Tag id that matches the measured device
+   * @param rssi - Measured signal strength
+   * @returns Smoothed signal strength value
+   */
+  filterRssi(tagId: string, rssi: number): number {
+    if (this.filterMap.has(tagId)) {
+      return this.filterMap.get(tagId).filter(rssi);
+    } else {
+      // filter params taken from: https://www.researchgate.net/publication/316501991_An_Improved_BLE_Indoor_Localization_with_Kalman-Based_Fusion_An_Experimental_Study
+      const kalman = new KalmanFilter({ R: 1.4, Q: 0.065 });
+      this.filterMap.set(tagId, kalman);
+      return kalman.filter(rssi);
+    }
+  }
+
   /**
    * Creates and registers a new distance sensor (this machine <> peripheral).
    *

typings/kalmanjs/index.d.ts

@@ -0,0 +1,19 @@
+declare module 'kalmanjs' {
+  export default class KalmanFilter {
+    constructor(options?: FilterOptions);
+    filter(z: number, u?: number): number;
+    predict(u?: number): number;
+    uncertainty(): number;
+    lastMeasurement(): number;
+    setMeasurementNoise(noise: number): void;
+    setProcessNoise(noise: number): void;
+  }
+
+  interface FilterOptions {
+    R?: number;
+    Q?: number;
+    A?: number;
+    B?: number;
+    C?: number;
+  }
+}