node-red-contrib-ibeacon-kalman/ibeacon-kalman.js

Kalman filter for iBeacon RSSI distance estimation

module.exports = function(RED) {
    function IbeaconKalmanNode(config) {
        RED.nodes.createNode(this, config);
        var node = this;
        
        // Параметры по умолчанию
        node.defaultTxPower = parseFloat(config.txPower || -59);
        node.defaultNValue = parseFloat(config.nValue || 2.0);
        node.processNoise = parseFloat(config.processNoise || 0.1);
        node.measurementNoise = parseFloat(config.measurementNoise || 1.0);
        
        // Инициализация фильтра
        node.resetFilter = function() {
            node.kalmanState = {
                estimate: 1.0,
                estimateError: 1.0,
                currentTxPower: node.defaultTxPower,
                currentNValue: node.defaultNValue
            };
        };
        node.resetFilter();

        node.on('input', function(msg) {
            try {
                // Получаем параметры из сообщения или используем значения по умолчанию
                const txPower = typeof msg.txPower !== 'undefined' 
                    ? parseFloat(msg.txPower) 
                    : node.kalmanState.currentTxPower;
                
                const nValue = typeof msg.nValue !== 'undefined'
                    ? parseFloat(msg.nValue)
                    : node.kalmanState.currentNValue;

                // Сброс фильтра при изменении ключевых параметров
                if (txPower !== node.kalmanState.currentTxPower || 
                    nValue !== node.kalmanState.currentNValue) {
                    
                    node.kalmanState.currentTxPower = txPower;
                    node.kalmanState.currentNValue = nValue;
                    node.kalmanState.estimate = 1.0;
                    node.kalmanState.estimateError = 1.0;
                    
                    node.warn(`Reset Kalman filter due to parameter change: 
                              TX=${txPower}dBm, n=${nValue}`);
                }

                // Конвертация RSSI в расстояние
                const rssi = parseFloat(msg.payload);
                const ratio = (txPower - rssi) / (10 * nValue);
                const rawDistance = Math.pow(10, ratio);

                // Обновление фильтра Калмана
                const predError = node.kalmanState.estimateError + node.processNoise;
                const kalmanGain = predError / (predError + node.measurementNoise);
                
                node.kalmanState.estimate = node.kalmanState.estimate + 
                                          kalmanGain * (rawDistance - node.kalmanState.estimate);
                node.kalmanState.estimateError = (1 - kalmanGain) * predError;

                // Формирование выходного сообщения
                msg.payload = {
                    rssi: rssi,
                    rawDistance: rawDistance,
                    filteredDistance: node.kalmanState.estimate,
                    txPower: txPower,
                    nValue: nValue,
                    timestamp: Date.now()
                };

                node.send(msg);
            } catch (e) {
                node.error("Processing error: " + e.message, msg);
            }
        });

        // Сброс при развертывании
        node.on('close', function() {
            node.resetFilter();
        });
    }
    RED.nodes.registerType("ibeacon-kalman", IbeaconKalmanNode);
};