From f1a2713493ea867d5b02bc4828e4092b24cc598d Mon Sep 17 00:00:00 2001 From: DTTerastar Date: Wed, 19 Mar 2025 21:36:06 -0400 Subject: [PATCH] WIP --- src/Models/Config.cs | 4 +- src/Optimizers/CombinedOptimizer.cs | 199 ++++++++++--- .../GlobalAbsorptionRxTxOptimizer.cs | 2 +- src/Optimizers/RxTxAdjRssiOptimizer.cs | 270 ++++++++++++++++++ .../TwoStageRxAdjAbsorptionOptimizer.cs | 6 +- .../WeightedJointRxAdjAbsorptionOptimizer.cs | 2 +- 6 files changed, 436 insertions(+), 47 deletions(-) create mode 100644 src/Optimizers/RxTxAdjRssiOptimizer.cs diff --git a/src/Models/Config.cs b/src/Models/Config.cs index c8608f43..36ad6a65 100644 --- a/src/Models/Config.cs +++ b/src/Models/Config.cs @@ -134,8 +134,8 @@ public partial class ConfigOptimization [YamlIgnore] public double AbsorptionMin => Limits.TryGetValue("absorption_min", out var val) ? val : 2; [YamlIgnore] public double AbsorptionMax => Limits.TryGetValue("absorption_max", out var val) ? val : 4; - [YamlIgnore] public double TxRefRssiMin => Limits.TryGetValue("tx_ref_rssi_min", out var val) ? val : -70; - [YamlIgnore] public double TxRefRssiMax => Limits.TryGetValue("tx_ref_rssi_max", out var val) ? val : -50; + [YamlIgnore] public double TxRefRssiMin => Limits.TryGetValue("tx_ref_rssi_min", out var val) ? val : -80; + [YamlIgnore] public double TxRefRssiMax => Limits.TryGetValue("tx_ref_rssi_max", out var val) ? val : -40; [YamlIgnore] public double RxAdjRssiMin => Limits.TryGetValue("rx_adj_rssi_min", out var val) ? val : -5; [YamlIgnore] public double RxAdjRssiMax => Limits.TryGetValue("rx_adj_rssi_max", out var val) ? val : 30; diff --git a/src/Optimizers/CombinedOptimizer.cs b/src/Optimizers/CombinedOptimizer.cs index 81ca2938..51f4f7fa 100644 --- a/src/Optimizers/CombinedOptimizer.cs +++ b/src/Optimizers/CombinedOptimizer.cs @@ -8,10 +8,12 @@ namespace ESPresense.Optimizers; public class CombinedOptimizer : IOptimizer { private readonly State _state; + private readonly Serilog.ILogger _logger; public CombinedOptimizer(State state) { _state = state; + _logger = Log.ForContext(); } public string Name => "Two-Step Optimized Combined RxAdjRssi and Absorption"; @@ -24,7 +26,11 @@ public OptimizationResults Optimize(OptimizationSnapshot os, Dictionary g).ToList(); var uniqueDeviceIds = allNodes.SelectMany(n => new[] { n.Rx.Id, n.Tx.Id }).Distinct().ToList(); - if (allNodes.Count < 3) return results; + if (allNodes.Count < 3) + { + _logger.Information("Not enough nodes for optimization (need at least 3, found {Count})", allNodes.Count); + return results; + } try { @@ -39,21 +45,22 @@ public OptimizationResults Optimize(OptimizationSnapshot os, Dictionary OptimizeNodeAbsorptions(List allNodes, List uniqueDeviceIds, Dictionary rxAdjRssiDict, Dictionary<(string, string), double> pathAbsorptionDict, ConfigOptimization optimization, Dictionary existingSettings) { - // Fix: Use ObjectiveFunction.Gradient() instead of ValueAndGradient - var obj = ObjectiveFunction.Gradient( - x => { - var nodeAbsorptionDict = new Dictionary(); - for (int i = 0; i < uniqueDeviceIds.Count; i++) + // Create reasonable initial guesses + var initialGuess = Vector.Build.Dense(uniqueDeviceIds.Count * 2); + for (int i = 0; i < uniqueDeviceIds.Count; i++) + { + // Include more intelligent initial guesses based on naive distance model + // Attempt to calculate a reasonable starting point based on physics model + double estimatedRxAdjRssi = 0; + double estimatedAbsorption = 2.5; // Middle of typical range (between 2-3) + + // If we have data from existing nodes, try to extract better initial guesses + var existingMeasurements = allNodes.Where(n => + n.Rx.Id == uniqueDeviceIds[i] || n.Tx.Id == uniqueDeviceIds[i]).ToList(); + + if (existingMeasurements.Any()) + { + // Estimate parameters based on known distances and RSSI + // This is a simplified approach, but provides a better starting point + var avgDistance = existingMeasurements.Average(m => m.Rx.Location.DistanceTo(m.Tx.Location)); + var avgRssi = existingMeasurements.Average(m => m.Rssi); + + // Heuristic formula based on RSSI model + if (avgDistance > 0 && !double.IsNaN(avgRssi)) { var absorption = x[i]; existingSettings.TryGetValue(uniqueDeviceIds[i], out var nodeSettings); @@ -148,9 +172,9 @@ private Dictionary OptimizeNodeAbsorptions(List allNode return CalculateError(allNodes, rxAdjRssiDict, nodeAbsorptionDict: nodeAbsorptionDict); }, + // Function to compute gradient x => { - var nodeAbsorptionDict = new Dictionary(); - for (int i = 0; i < uniqueDeviceIds.Count; i++) + try { nodeAbsorptionDict[uniqueDeviceIds[i]] = x[i]; } @@ -160,7 +184,24 @@ private Dictionary OptimizeNodeAbsorptions(List allNode double epsilon = 1e-5; double baseError = CalculateError(allNodes, rxAdjRssiDict, nodeAbsorptionDict: nodeAbsorptionDict); - for (int i = 0; i < x.Count; i++) + // Compute gradient numerically + var gradient = Vector.Build.Dense(x.Count); + double h = 1e-5; // Step size for finite difference + + for (int i = 0; i < x.Count; i++) + { + var xPlus = x.Clone(); + xPlus[i] += h; + + var paramsPlus = CreateDeviceParamsFromVector(xPlus, uniqueDeviceIds, optimization); + var errorPlus = CalculateError(allNodes, paramsPlus); + + gradient[i] = (errorPlus - baseError) / h; + } + + return gradient; + } + catch (Exception ex) { var tempDict = new Dictionary(nodeAbsorptionDict); tempDict[uniqueDeviceIds[i]] += epsilon; @@ -168,9 +209,11 @@ private Dictionary OptimizeNodeAbsorptions(List allNode var errorPlusEps = CalculateError(allNodes, rxAdjRssiDict, nodeAbsorptionDict: tempDict); gradient[i] = (errorPlusEps - baseError) / epsilon; } + } + ); - return gradient; - }); + // ConjugateGradientMinimizer only takes 3 tolerance parameters, not a maximum iteration count + var solver = new ConjugateGradientMinimizer(1e-3, 1000); // Initial guess uses node setting if available, else global midpoint var initialGuess = Vector.Build.Dense(uniqueDeviceIds.Count); @@ -189,41 +232,117 @@ private Dictionary OptimizeNodeAbsorptions(List allNode var nodeAbsorptions = new Dictionary(); for (int i = 0; i < uniqueDeviceIds.Count; i++) { - nodeAbsorptions[uniqueDeviceIds[i]] = result.MinimizingPoint[i]; + result = solver.FindMinimum(objGradient, initialGuess); + _logger.Information("Optimization completed: Iterations={0}, Status={1}, Error={2}", + result.Iterations, result.ReasonForExit, result.FunctionInfoAtMinimum.Value); + } + catch (Exception ex) + { + _logger.Error(ex, "Optimization failed"); + + // Return default values if optimization fails + var defaultParams = new Dictionary(); + foreach (var id in uniqueDeviceIds) + { + defaultParams[id] = new DeviceParameters + { + RxAdjRssi = 0, + Absorption = (optimization?.AbsorptionMax + optimization?.AbsorptionMin) / 2 ?? 3.0 + }; + } + + return (defaultParams, double.MaxValue); + } + + // Extract optimized parameters + var deviceParams = new Dictionary(); + for (int i = 0; i < uniqueDeviceIds.Count; i++) + { + deviceParams[uniqueDeviceIds[i]] = new DeviceParameters + { + RxAdjRssi = result.MinimizingPoint[i], + Absorption = result.MinimizingPoint[i + uniqueDeviceIds.Count] + }; } - return nodeAbsorptions; + return (deviceParams, result.FunctionInfoAtMinimum.Value); } - private double CalculateError(List nodes, Dictionary rxAdjRssiDict, - Dictionary nodeAbsorptionDict = null, Dictionary<(string, string), double> pathAbsorptionDict = null) + private Dictionary CreateDeviceParamsFromVector(Vector x, List uniqueDeviceIds, ConfigOptimization optimization) { - return nodes.Select(n => + var deviceParams = new Dictionary(); + + for (int i = 0; i < uniqueDeviceIds.Count; i++) { - var distance = n.Rx.Location.DistanceTo(n.Tx.Location); - var rxAdjRssi = rxAdjRssiDict[n.Rx.Id]; - var txAdjRssi = rxAdjRssiDict[n.Tx.Id]; - double absorption; + var rxAdjRssi = x[i]; + var absorption = x[i + uniqueDeviceIds.Count]; - if (pathAbsorptionDict != null) + // Enforce constraints by clamping values to valid ranges + rxAdjRssi = Math.Clamp(rxAdjRssi, + optimization?.RxAdjRssiMin ?? -20, + optimization?.RxAdjRssiMax ?? 20); + + absorption = Math.Clamp(absorption, + optimization?.AbsorptionMin ?? 1.5, + optimization?.AbsorptionMax ?? 4.5); + + deviceParams[uniqueDeviceIds[i]] = new DeviceParameters { - var pathKey = (Min(n.Rx.Id, n.Tx.Id), Max(n.Rx.Id, n.Tx.Id)); - absorption = pathAbsorptionDict[pathKey]; - } - else if (nodeAbsorptionDict != null) + RxAdjRssi = rxAdjRssi, + Absorption = absorption + }; + } + + return deviceParams; + } + + private double CalculateError(List nodes, Dictionary deviceParams) + { + double totalError = 0; + int count = 0; + + foreach (var node in nodes) + { + try { - absorption = (nodeAbsorptionDict[n.Rx.Id] + nodeAbsorptionDict[n.Tx.Id]) / 2; + if (!deviceParams.TryGetValue(node.Rx.Id, out var rxParams) || + !deviceParams.TryGetValue(node.Tx.Id, out var txParams)) + { + continue; + } + + var distance = node.Rx.Location.DistanceTo(node.Tx.Location); + var rxAdjRssi = rxParams.RxAdjRssi; + var txAdjRssi = txParams.RxAdjRssi; + + // Use average of both device absorptions + var absorption = (rxParams.Absorption + txParams.Absorption) / 2; + + // Safeguard against negative or zero absorption + if (absorption <= 0.1) + { + absorption = 0.1; + } + + // Calculate distance based on RSSI + var calculatedDistance = Math.Pow(10, (-59 + rxAdjRssi + txAdjRssi - node.Rssi) / (10.0d * absorption)); + + // Skip invalid calculations + if (double.IsNaN(calculatedDistance) || double.IsInfinity(calculatedDistance)) + { + continue; + } + + // Squared error + totalError += Math.Pow(distance - calculatedDistance, 2); + count++; } - else + catch (Exception ex) { - throw new ArgumentException("Either nodeAbsorptionDict or pathAbsorptionDict must be provided"); + _logger.Warning(ex, "Error calculating distance for node {Rx} to {Tx}", node.Rx.Id, node.Tx.Id); } + } - var calculatedDistance = Math.Pow(10, (-59 + rxAdjRssi + txAdjRssi - n.Rssi) / (10.0d * absorption)); - return Math.Pow(distance - calculatedDistance, 2); - }).Average(); + return count > 0 ? totalError / count : double.MaxValue; } - - private static string Min(string a, string b) => string.Compare(a, b) < 0 ? a : b; - private static string Max(string a, string b) => string.Compare(a, b) >= 0 ? a : b; -} +} \ No newline at end of file diff --git a/src/Optimizers/GlobalAbsorptionRxTxOptimizer.cs b/src/Optimizers/GlobalAbsorptionRxTxOptimizer.cs index 1023a2e2..3c69892f 100644 --- a/src/Optimizers/GlobalAbsorptionRxTxOptimizer.cs +++ b/src/Optimizers/GlobalAbsorptionRxTxOptimizer.cs @@ -338,4 +338,4 @@ public OptimizationResults Optimize(OptimizationSnapshot os, Dictionary "Rx Tx Adj Rssi"; + + public OptimizationResults Optimize(OptimizationSnapshot os) + { + var or = new OptimizationResults(); + var optimization = _state.Config?.Optimization; + + // Group all valid measurements + var allRxNodes = os.ByRx().SelectMany(g => g).ToList(); + + if (allRxNodes.Count < 3) + { + Log.Warning("Not enough valid measurements for optimization"); + return or; + } + + // Get unique Rx and Tx nodes + var uniqueRxIds = allRxNodes.Select(n => n.Rx.Id).Distinct().ToList(); + var uniqueTxIds = allRxNodes.Select(n => n.Tx.Id).Distinct().ToList(); + + // Map parameter indices + var rxIndexMap = new Dictionary(); + var txIndexMap = new Dictionary(); + int paramIndex = 0; + + // Each Rx node has two parameters: rxAdjRssi and absorption + foreach (var rxId in uniqueRxIds) + { + rxIndexMap[rxId] = paramIndex; + paramIndex += 2; + } + + // Each Tx node has one parameter: txRefRssi + foreach (var txId in uniqueTxIds) + { + txIndexMap[txId] = paramIndex; + paramIndex++; + } + + int totalParams = paramIndex; + + var targetAbsorption = optimization.AbsorptionMin + (optimization.AbsorptionMax - optimization.AbsorptionMin) / 2.0; + double penaltyWeight = 10; + + // Pre-calculate weights for each node based on RssiVar + var nodeWeights = new Dictionary(); + double totalWeight = 0; + + foreach (var node in allRxNodes) + { + // Inverse variance weighting - use 1/variance as weight + double weight = 1.0; + if (node.RssiVar > 0) + { + weight = 1.0 / Math.Max(node.RssiVar.Value, 0.1); // Adding minimum to avoid extreme weights + } + + nodeWeights[node] = weight; + totalWeight += weight; + } + + // Normalize weights if needed + if (totalWeight > 0) + { + foreach (var node in allRxNodes) + { + nodeWeights[node] = nodeWeights[node] / totalWeight * allRxNodes.Count; + } + } + + // Define asymmetric error function that penalizes impossible situations + // (when estimated distance is less than actual map distance) + Func calculateDistanceError = (calculated, map) => + { + if (calculated < map) + { + // This is physically impossible (can't be closer than map distance) + // Apply higher penalty with asymmetric factor + return Math.Pow(map - calculated, 4); + } + else + { + // Regular error calculation for the normal case (estimated >= actual) + // This means there could be an obstacle causing signal attenuation + return Math.Pow(map - calculated, 2); + } + }; + + var objectiveFunction = ObjectiveFunction.Gradient( + x => + { + double error = 0; + double weightSum = 0; + + foreach (var node in allRxNodes) + { + int rxBaseIndex = rxIndexMap[node.Rx.Id]; + int txBaseIndex = txIndexMap[node.Tx.Id]; + + double rxAdjRssi = x[rxBaseIndex]; + double absorption = x[rxBaseIndex + 1]; + double txRefRssi = x[txBaseIndex]; + + double calculatedDistance = Math.Pow(10, (txRefRssi + rxAdjRssi - node.Rssi) / (10.0 * absorption)); + double mapDistance = node.Rx.Location.DistanceTo(node.Tx.Location); + + // Get the weight for this node + double weight = nodeWeights[node]; + weightSum += weight; + + // Apply weight to the asymmetric error + error += weight * calculateDistanceError(calculatedDistance, mapDistance); + + // Regularization: Penalize absorption deviation from the middle value + error += weight * penaltyWeight * Math.Pow(absorption - targetAbsorption, 2); + } + + return weightSum > 0 ? error / weightSum : error; + }, + x => + { + var grad = Vector.Build.Dense(totalParams); + double h = 1e-6; + for (int i = 0; i < totalParams; i++) + { + var xPlus = x.Clone(); + var xMinus = x.Clone(); + xPlus[i] = x[i] + h; + xMinus[i] = x[i] - h; + + double fPlus = 0; + double fMinus = 0; + double weightSumPlus = 0; + double weightSumMinus = 0; + + foreach (var node in allRxNodes) + { + int rxBaseIndex = rxIndexMap[node.Rx.Id]; + int txBaseIndex = txIndexMap[node.Tx.Id]; + double weight = nodeWeights[node]; + + { + double rxAdjRssi = xPlus[rxBaseIndex]; + double absorption = xPlus[rxBaseIndex + 1]; + double txRefRssi = xPlus[txBaseIndex]; + double calculatedDistance = Math.Pow(10, (txRefRssi + rxAdjRssi - node.Rssi) / (10.0 * absorption)); + double mapDistance = node.Rx.Location.DistanceTo(node.Tx.Location); + + fPlus += weight * (calculateDistanceError(calculatedDistance, mapDistance) + + penaltyWeight * Math.Pow(absorption - targetAbsorption, 2)); + weightSumPlus += weight; + } + { + double rxAdjRssi = xMinus[rxBaseIndex]; + double absorption = xMinus[rxBaseIndex + 1]; + double txRefRssi = xMinus[txBaseIndex]; + double calculatedDistance = Math.Pow(10, (txRefRssi + rxAdjRssi - node.Rssi) / (10.0 * absorption)); + double mapDistance = node.Rx.Location.DistanceTo(node.Tx.Location); + + fMinus += weight * (calculateDistanceError(calculatedDistance, mapDistance) + + penaltyWeight * Math.Pow(absorption - targetAbsorption, 2)); + weightSumMinus += weight; + } + } + + fPlus = weightSumPlus > 0 ? fPlus / weightSumPlus : fPlus; + fMinus = weightSumMinus > 0 ? fMinus / weightSumMinus : fMinus; + grad[i] = (fPlus - fMinus) / (2 * h); + } + return grad; + } + ); + + + // Build lower and upper bound vectors + var lowerBound = Vector.Build.Dense(totalParams); + var upperBound = Vector.Build.Dense(totalParams); + + // For Rx nodes: rxAdjRssi and absorption bounds + foreach (var rxId in uniqueRxIds) + { + int baseIndex = rxIndexMap[rxId]; + lowerBound[baseIndex] = optimization.RxAdjRssiMin; + upperBound[baseIndex] = optimization.RxAdjRssiMax; + lowerBound[baseIndex + 1] = optimization.AbsorptionMin; + upperBound[baseIndex + 1] = optimization.AbsorptionMax; + } + + // For Tx nodes: txRefRssi bounds + foreach (var txId in uniqueTxIds) + { + lowerBound[txIndexMap[txId]] = optimization.TxRefRssiMin; + upperBound[txIndexMap[txId]] = optimization.TxRefRssiMax; + } + + // Initialize with a reasonable guess (ensure within bounds) + var initialGuess = Vector.Build.Dense(totalParams); + foreach (var rxId in uniqueRxIds) + { + int baseIndex = rxIndexMap[rxId]; + initialGuess[baseIndex] = 0; // initial rxAdjRssi + initialGuess[baseIndex + 1] = ((optimization.AbsorptionMax - optimization.AbsorptionMin) / 2.0) + optimization.AbsorptionMin; + } + foreach (var txId in uniqueTxIds) + { + initialGuess[txIndexMap[txId]] = -59; + } + + try + { + // Use the bounded BFGS solver + var solver = new BfgsBMinimizer(1e-8, 1e-8, 1e-8, 10000); + var result = solver.FindMinimum(objectiveFunction, lowerBound, upperBound, initialGuess); + + // Process Rx node results + foreach (var rxId in uniqueRxIds) + { + int baseIndex = rxIndexMap[rxId]; + double rxAdjRssi = result.MinimizingPoint[baseIndex]; + double absorption = result.MinimizingPoint[baseIndex + 1]; + + // Ensure values are within bounds (should be already) + rxAdjRssi = Math.Max(optimization.RxAdjRssiMin, Math.Min(rxAdjRssi, optimization.RxAdjRssiMax)); + absorption = Math.Max(optimization.AbsorptionMin, Math.Min(absorption, optimization.AbsorptionMax)); + + var n = or.Nodes.GetOrAdd(rxId); + n.RxAdjRssi = rxAdjRssi; + n.Absorption = absorption; + n.Error = result.FunctionInfoAtMinimum.Value; + } + + // Process Tx node results + foreach (var txId in uniqueTxIds) + { + double txRefRssi = result.MinimizingPoint[txIndexMap[txId]]; + txRefRssi = Math.Max(optimization.TxRefRssiMin, Math.Min(txRefRssi, optimization.TxRefRssiMax)); + + var n = or.Nodes.GetOrAdd(txId); + n.TxRefRssi = txRefRssi; + n.Error = result.FunctionInfoAtMinimum.Value; + } + } + catch (Exception ex) + { + Log.Error("Error optimizing all nodes: {0}", ex.Message); + } + + return or; + } +} \ No newline at end of file diff --git a/src/Optimizers/TwoStageRxAdjAbsorptionOptimizer.cs b/src/Optimizers/TwoStageRxAdjAbsorptionOptimizer.cs index bb4b0155..c9b74eae 100644 --- a/src/Optimizers/TwoStageRxAdjAbsorptionOptimizer.cs +++ b/src/Optimizers/TwoStageRxAdjAbsorptionOptimizer.cs @@ -70,7 +70,7 @@ public OptimizationResults Optimize(OptimizationSnapshot os, Dictionary x, Measure dn) { - double exponent = (-59 + x[0] - dn.Rssi) / (10.0d * x[1]); + double exponent = (txRefRssi + x[0] - dn.Rssi) / (10.0d * x[1]); return (x[1] > 0 && !double.IsInfinity(exponent)) ? Math.Pow(10, exponent) : double.MaxValue; }