|
| 1 | +using ESPresense.Models; |
| 2 | +using MathNet.Numerics.LinearAlgebra; |
| 3 | +using Microsoft.Extensions.Hosting; |
| 4 | +using Serilog; |
| 5 | + |
| 6 | +namespace ESPresense.Services; |
| 7 | + |
| 8 | +/// <summary> |
| 9 | +/// Continuously reconstructs a static RF-attenuation map per floor from the node-to-node links that |
| 10 | +/// are already streaming in. Each link's "excess path loss beyond free space" is treated as a line |
| 11 | +/// integral through an unknown attenuation field; many crisscrossing links let us invert for where |
| 12 | +/// the attenuation actually sits (walls, appliances, the refrigerator). Runs online, no acquisition |
| 13 | +/// step — the result is exposed for the calibration-page heatmap so a human can eyeball it ("that |
| 14 | +/// rectangle is my fridge") and, later, fed into the locator to down-weight blocked paths. |
| 15 | +/// </summary> |
| 16 | +public class RadioTomographyService : BackgroundService |
| 17 | +{ |
| 18 | + private readonly State _state; |
| 19 | + |
| 20 | + // Tunables (kept conservative for a first cut). |
| 21 | + private const double FreeSpaceExponent = 2.0; // n in rssi = ref - 10*n*log10(d) |
| 22 | + private const double CellSizeMeters = 1.0; |
| 23 | + private const int MaxCells = 1200; // bound the inverse-problem size |
| 24 | + private const int MinLinksPerFloor = 6; |
| 25 | + private const double Regularization = 0.15; // ridge strength, relative to data scale |
| 26 | + private const double DecayDbPerCycle = 1.0; // how fast the per-link "clean" peak forgets |
| 27 | + private static readonly TimeSpan Interval = TimeSpan.FromSeconds(30); |
| 28 | + |
| 29 | + // Per-link rolling "clean" (least-shadowed) RSSI, keyed "tx|rx". The decaying max strips out |
| 30 | + // transient people walking through a link while keeping the persistent static structure. |
| 31 | + private readonly Dictionary<string, double> _cleanRssi = new(); |
| 32 | + |
| 33 | + public TomographyResult Latest { get; private set; } = new(); |
| 34 | + |
| 35 | + public RadioTomographyService(State state) |
| 36 | + { |
| 37 | + _state = state; |
| 38 | + } |
| 39 | + |
| 40 | + protected override async Task ExecuteAsync(CancellationToken stoppingToken) |
| 41 | + { |
| 42 | + while (!stoppingToken.IsCancellationRequested) |
| 43 | + { |
| 44 | + try |
| 45 | + { |
| 46 | + Latest = Compute(); |
| 47 | + } |
| 48 | + catch (Exception ex) |
| 49 | + { |
| 50 | + Log.Warning(ex, "Radio tomography compute failed"); |
| 51 | + } |
| 52 | + await Task.Delay(Interval, stoppingToken); |
| 53 | + } |
| 54 | + } |
| 55 | + |
| 56 | + private TomographyResult Compute() |
| 57 | + { |
| 58 | + var result = new TomographyResult { Updated = DateTime.UtcNow }; |
| 59 | + |
| 60 | + foreach (var floor in _state.Floors.Values) |
| 61 | + { |
| 62 | + if (floor.Bounds is not { Length: 2 }) continue; |
| 63 | + var tf = ComputeFloor(floor); |
| 64 | + if (tf != null) result.Floors.Add(tf); |
| 65 | + } |
| 66 | + |
| 67 | + return result; |
| 68 | + } |
| 69 | + |
| 70 | + private TomographyFloor? ComputeFloor(Floor floor) |
| 71 | + { |
| 72 | + double minX = Math.Min(floor.Bounds![0].X, floor.Bounds[1].X); |
| 73 | + double minY = Math.Min(floor.Bounds[0].Y, floor.Bounds[1].Y); |
| 74 | + double maxX = Math.Max(floor.Bounds[0].X, floor.Bounds[1].X); |
| 75 | + double maxY = Math.Max(floor.Bounds[0].Y, floor.Bounds[1].Y); |
| 76 | + double w = maxX - minX, h = maxY - minY; |
| 77 | + if (w <= 0 || h <= 0) return null; |
| 78 | + |
| 79 | + // Pick a cell size that keeps the grid under the size cap. |
| 80 | + double cell = CellSizeMeters; |
| 81 | + while (Math.Ceiling(w / cell) * Math.Ceiling(h / cell) > MaxCells) cell *= 1.5; |
| 82 | + int cols = (int)Math.Ceiling(w / cell); |
| 83 | + int rows = (int)Math.Ceiling(h / cell); |
| 84 | + int cellCount = cols * rows; |
| 85 | + |
| 86 | + bool OnFloor(Node n) => n.HasLocation && (n.Floors?.Any(f => f.Id == floor.Id) ?? false); |
| 87 | + |
| 88 | + // Gather links between nodes on this floor. |
| 89 | + var rows_W = new List<double[]>(); |
| 90 | + var ys = new List<double>(); |
| 91 | + var coverage = new double[cellCount]; |
| 92 | + |
| 93 | + foreach (var tx in _state.Nodes.Values) |
| 94 | + { |
| 95 | + if (!OnFloor(tx)) continue; |
| 96 | + foreach (var meas in tx.RxNodes.Values) |
| 97 | + { |
| 98 | + var rx = meas.Rx; |
| 99 | + if (rx == null || !OnFloor(rx) || !meas.Current || meas.Rssi == 0) continue; |
| 100 | + |
| 101 | + double d = Math.Sqrt(Math.Pow(tx.Location.X - rx.Location.X, 2) + Math.Pow(tx.Location.Y - rx.Location.Y, 2)); |
| 102 | + if (d < 0.5) continue; |
| 103 | + |
| 104 | + double clean = UpdateClean($"{tx.Id}|{rx.Id}", meas.Rssi); |
| 105 | + double freeExpected = meas.RefRssi - 10.0 * FreeSpaceExponent * Math.Log10(d); |
| 106 | + double excess = freeExpected - clean; // dB of loss beyond free space |
| 107 | + if (excess < 0) excess = 0; // can't be "less than free space" (ignore rare constructive multipath) |
| 108 | + |
| 109 | + var rowW = new double[cellCount]; |
| 110 | + RasterizeRay(tx.Location.X, tx.Location.Y, rx.Location.X, rx.Location.Y, minX, minY, cell, cols, rows, rowW); |
| 111 | + double rayLen = rowW.Sum(); |
| 112 | + if (rayLen <= 0) continue; |
| 113 | + |
| 114 | + for (int c = 0; c < cellCount; c++) coverage[c] += rowW[c]; |
| 115 | + rows_W.Add(rowW); |
| 116 | + ys.Add(excess); |
| 117 | + } |
| 118 | + } |
| 119 | + |
| 120 | + if (rows_W.Count < MinLinksPerFloor) return null; |
| 121 | + |
| 122 | + var attenuation = SolveRidge(rows_W, ys, cellCount); |
| 123 | + |
| 124 | + var tf = new TomographyFloor |
| 125 | + { |
| 126 | + FloorId = floor.Id, |
| 127 | + FloorName = floor.Name, |
| 128 | + MinX = minX, |
| 129 | + MinY = minY, |
| 130 | + CellSize = cell, |
| 131 | + Cols = cols, |
| 132 | + Rows = rows, |
| 133 | + Attenuation = attenuation, |
| 134 | + Coverage = coverage, |
| 135 | + Links = rows_W.Count, |
| 136 | + MaxAttenuation = attenuation.Length > 0 ? attenuation.Max() : 0 |
| 137 | + }; |
| 138 | + return tf; |
| 139 | + } |
| 140 | + |
| 141 | + private double UpdateClean(string key, double rssi) |
| 142 | + { |
| 143 | + if (_cleanRssi.TryGetValue(key, out var prev)) |
| 144 | + { |
| 145 | + double clean = Math.Max(rssi, prev - DecayDbPerCycle); |
| 146 | + _cleanRssi[key] = clean; |
| 147 | + return clean; |
| 148 | + } |
| 149 | + _cleanRssi[key] = rssi; |
| 150 | + return rssi; |
| 151 | + } |
| 152 | + |
| 153 | + /// <summary>Accumulate per-cell path length for the segment by fine sampling.</summary> |
| 154 | + private static void RasterizeRay(double x0, double y0, double x1, double y1, |
| 155 | + double minX, double minY, double cell, int cols, int rows, double[] rowW) |
| 156 | + { |
| 157 | + double dx = x1 - x0, dy = y1 - y0; |
| 158 | + double len = Math.Sqrt(dx * dx + dy * dy); |
| 159 | + if (len <= 0) return; |
| 160 | + int steps = Math.Max(1, (int)Math.Ceiling(len / (cell * 0.25))); |
| 161 | + double stepLen = len / steps; |
| 162 | + for (int s = 0; s < steps; s++) |
| 163 | + { |
| 164 | + double t = (s + 0.5) / steps; |
| 165 | + double px = x0 + dx * t, py = y0 + dy * t; |
| 166 | + int col = (int)((px - minX) / cell); |
| 167 | + int row = (int)((py - minY) / cell); |
| 168 | + if (col < 0 || col >= cols || row < 0 || row >= rows) continue; |
| 169 | + rowW[row * cols + col] += stepLen; |
| 170 | + } |
| 171 | + } |
| 172 | + |
| 173 | + /// <summary>Non-negative ridge regression: min ||Wx - y||^2 + λ||x||^2, then clamp x >= 0.</summary> |
| 174 | + private static double[] SolveRidge(List<double[]> rowsW, List<double> ys, int cellCount) |
| 175 | + { |
| 176 | + int L = rowsW.Count; |
| 177 | + var W = Matrix<double>.Build.Dense(L, cellCount, (i, j) => rowsW[i][j]); |
| 178 | + var y = Vector<double>.Build.Dense(L, i => ys[i]); |
| 179 | + |
| 180 | + var wtw = W.TransposeThisAndMultiply(W); // C x C, SPD after ridge |
| 181 | + double meanDiag = 0; |
| 182 | + for (int i = 0; i < cellCount; i++) meanDiag += wtw[i, i]; |
| 183 | + meanDiag = cellCount > 0 ? meanDiag / cellCount : 1.0; |
| 184 | + double lambda = Regularization * meanDiag + 1e-9; |
| 185 | + for (int i = 0; i < cellCount; i++) wtw[i, i] += lambda; |
| 186 | + |
| 187 | + var wty = W.TransposeThisAndMultiply(y); |
| 188 | + var x = wtw.Cholesky().Solve(wty); |
| 189 | + |
| 190 | + var result = new double[cellCount]; |
| 191 | + for (int i = 0; i < cellCount; i++) result[i] = Math.Max(0, x[i]); |
| 192 | + return result; |
| 193 | + } |
| 194 | +} |
0 commit comments