diff --git a/package-lock.json b/package-lock.json index 55b6e839..c9e8d9d9 100644 --- a/package-lock.json +++ b/package-lock.json @@ -6337,6 +6337,11 @@ "verror": "1.10.0" } }, + "kalmanjs": { + "version": "1.1.0", + "resolved": "https://registry.npmjs.org/kalmanjs/-/kalmanjs-1.1.0.tgz", + "integrity": "sha512-Gkm2DAKIX8geuDcEJ92e80EmQ6TyIzWBJk6AFk+aDLofQ4G/KoDe1RCY9WMuXFe0NQxsTi7HYh9BVQneOtlvkg==" + }, "kind-of": { "version": "6.0.2", "resolved": "https://registry.npmjs.org/kind-of/-/kind-of-6.0.2.tgz", diff --git a/package.json b/package.json index 7b95face..7e1615d0 100644 --- a/package.json +++ b/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", diff --git a/src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.spec.ts b/src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.spec.ts index 2fe8e690..bf3b1775 100644 --- a/src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.spec.ts +++ b/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); diff --git a/src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.ts b/src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.ts index f2c1eba1..3188cdd1 100644 --- a/src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.ts +++ b/src/integrations/bluetooth-low-energy/bluetooth-low-energy.service.ts @@ -19,6 +19,7 @@ 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'; @@ -26,6 +27,10 @@ export const NEW_DISTANCE_CHANNEL = 'bluetooth-low-energy.new-distance'; export class BluetoothLowEnergyService implements OnModuleInit, OnApplicationBootstrap { private readonly config: BluetoothLowEnergyConfig; + private filterMap: Map = 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). * diff --git a/typings/kalmanjs/index.d.ts b/typings/kalmanjs/index.d.ts new file mode 100644 index 00000000..f994a6bd --- /dev/null +++ b/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; + } +}