Skip to content

Commit

Permalink
Add variance to radar (#444)
Browse files Browse the repository at this point in the history
  • Loading branch information
@DTTerastar
DTTerastar authored Dec 5, 2023
1 parent eed72ee commit 629c726
Show file tree
Hide file tree
Showing 7 changed files with 198 additions and 35 deletions.
2 changes: 1 addition & 1 deletion BUILDING.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

Git Clone, Open folder in VSCode, it'll ask to install dependencies, click yes, you'll need to install .NET SDK 8.0 as well then type into the terminal:

`dotnet watch`
`dotnet watch --project src`

Browse to [http://localhost:5279/]

Expand Down
17 changes: 13 additions & 4 deletions src/Converters/NodeDistanceConverter.cs
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Text.Json;
using System.Text.Json.Serialization;
using ESPresense.Models;
Expand All @@ -7,17 +9,24 @@ namespace ESPresense.Converters;

public class NodeDistanceConverter : JsonConverter<ConcurrentDictionary<string, DeviceNode>>
{
private static readonly JsonConverter<IDictionary<string, double>> DefaultDictConverter =
(JsonConverter<IDictionary<string, double>>)JsonSerializerOptions.Default.GetConverter(typeof(IDictionary<string, double>));
private static readonly JsonConverter<IDictionary<string, object>> DefaultDictConverter =
(JsonConverter<IDictionary<string, object>>)JsonSerializerOptions.Default.GetConverter(typeof(IDictionary<string, object>));

public override ConcurrentDictionary<string, DeviceNode>? Read(ref Utf8JsonReader reader, Type typeToConvert, JsonSerializerOptions options)
{
// Your existing read implementation or throw if not needed.
throw new NotImplementedException();
}

public override void Write(Utf8JsonWriter writer, ConcurrentDictionary<string, DeviceNode> distances, JsonSerializerOptions options)
{
var d = distances.Where(a => a.Value.Current).ToDictionary(a => a.Key, a => a.Value.Distance);
// Creating a dictionary with both dist and var
var d = distances.Where(a => a.Value.Current).ToDictionary(
a => a.Key,
a => new { dist = a.Value.Distance, var = a.Value.Variance } as object
);

// Using the dictionary converter to write this new structure
DefaultDictConverter.Write(writer, d, options);
}
}
}
122 changes: 122 additions & 0 deletions src/Locators/MLEMultilateralizer.cs
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,122 @@
using ESPresense.Extensions;
using ESPresense.Models;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.Optimization;
using MathNet.Spatial.Euclidean;
using Serilog;

namespace ESPresense.Locators;

public class MLEMultilateralizer(Device device, Floor floor, State state) : ILocate
{
public bool Locate(Scenario scenario)
{
double Error(IList<double> x, DeviceNode dn)
{
double expectedDistance = Math.Sqrt(Math.Pow(dn.Node!.Location.X - x[0], 2) +
Math.Pow(dn.Node!.Location.Y - x[1], 2) +
Math.Pow(dn.Node!.Location.Z - x[2], 2));
double error = dn.Distance - expectedDistance;
double fallbackVariance = 1e-6; // This should be a small positive number
double variance = dn.Variance ?? fallbackVariance;
variance = Math.Max(variance, fallbackVariance); // Ensure variance is never zero

return error * error / (2 * variance);
}

var confidence = scenario.Confidence;

var nodes = device.Nodes.Values.Where(a => a.Current && (a.Node?.Floors?.Contains(floor) ?? false)).OrderBy(a => a.Distance).ToArray();
var pos = nodes.Select(a => a.Node!.Location).ToArray();

scenario.Minimum = nodes.Min(a => (double?)a.Distance);
scenario.LastHit = nodes.Max(a => a.LastHit);
scenario.Fixes = pos.Length;

if (pos.Length <= 1)
{
scenario.Room = null;
scenario.Confidence = 0;
scenario.Error = null;
scenario.Floor = null;
return false;
}

scenario.Floor = floor;

var guess = confidence < 5
? Point3D.MidPoint(pos[0], pos[1])
: scenario.Location;
try
{
if (pos.Length < 3 || floor.Bounds == null)
{
confidence = 1;
scenario.Location = guess;
}
else
{
var lowerBound = Vector<double>.Build.DenseOfArray(new[] { floor.Bounds[0].X, floor.Bounds[0].Y, floor.Bounds[0].Z, 0.5 });
var upperBound = Vector<double>.Build.DenseOfArray(new[] { floor.Bounds[1].X, floor.Bounds[1].Y, floor.Bounds[1].Z, 1.5 });
var obj = ObjectiveFunction.Value(
x =>
{
var distanceFromBoundingBox = lowerBound.Subtract(x)
.PointwiseMaximum(x.Subtract(upperBound))
.PointwiseMaximum(0)
.L2Norm();
return (distanceFromBoundingBox > 0 ? Math.Pow(5, 1 + distanceFromBoundingBox) : 0) + Math.Pow(5 * (1 - x[3]), 2) + nodes
.Select((dn, i) => new { err = Error(x, dn), weight = state?.Weighting?.Get(i, nodes.Length) ?? 1.0 })
.Average(a => a.weight * Math.Pow(a.err, 2));
});

var initialGuess = Vector<double>.Build.DenseOfArray(new[]
{
Math.Max(floor.Bounds[0].X, Math.Min(floor.Bounds[1].X, guess.X)),
Math.Max(floor.Bounds[0].Y, Math.Min(floor.Bounds[1].Y, guess.Y)),
Math.Max(floor.Bounds[0].Z, Math.Min(floor.Bounds[1].Z, guess.Z)),
scenario.Scale ?? 1.0
});
var centroid = Point3D.Centroid(nodes.Select(n => n.Node!.Location).Take(3)).ToVector();
var vectorToCentroid = centroid.Subtract(initialGuess.SubVector(0, 3)).Normalize(2);
var scaleDelta = 0.05 * initialGuess[3];
var initialPerturbation = Vector<double>.Build.DenseOfEnumerable(vectorToCentroid.Append(scaleDelta));
var solver = new NelderMeadSimplex(1e-7, 10000);
var result = solver.FindMinimum(obj, initialGuess, initialPerturbation);
var minimizingPoint = result.MinimizingPoint.PointwiseMinimum(upperBound).PointwiseMaximum(lowerBound);
scenario.Location = new Point3D(minimizingPoint[0], minimizingPoint[1], minimizingPoint[2]);
scenario.Scale = minimizingPoint[3];
scenario.Fixes = pos.Length;
scenario.Error = result.FunctionInfoAtMinimum.Value;
scenario.Iterations = result switch
{
MinimizationWithLineSearchResult mwl =>
mwl.IterationsWithNonTrivialLineSearch + mwl.TotalLineSearchIterations,
_ => result.Iterations
};

scenario.ReasonForExit = result.ReasonForExit;
confidence = (int)Math.Min(100, Math.Max(10, 100.0 - (Math.Pow(scenario.Minimum ?? 1, 2) + Math.Pow(10 * (1 - (scenario.Scale ?? 1)), 2) + (scenario.Minimum + result.FunctionInfoAtMinimum.Value ?? 10.00))));
}
}
catch (MaximumIterationsException)
{
scenario.ReasonForExit = ExitCondition.ExceedIterations;
confidence = 1;
scenario.Location = guess;
}
catch (Exception ex)
{
confidence = 0;
scenario.Location = new Point3D();
Log.Error("Error finding location for {0}: {1}", device, ex.Message);
}

scenario.Confidence = confidence;

if (confidence <= 0) return false;
if (Math.Abs(scenario.Location.DistanceTo(scenario.LastLocation)) < 0.1) return false;
scenario.Room = floor.Rooms.Values.FirstOrDefault(a => a.Polygon?.EnclosesPoint(scenario.Location.ToPoint2D()) ?? false);
return true;
}
}
21 changes: 9 additions & 12 deletions src/Models/DeviceNode.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,21 +7,28 @@ public class DeviceNode

public double Distance { get; set; }
public double Rssi { get; set; }
public double RefRssi { get; set; }
public double? Variance { get; set; }

public DateTime? LastHit { get; set; }
public int Hits { get; set; }

public double LastDistance { get; set; }

public bool Current => DateTime.UtcNow - LastHit < TimeSpan.FromSeconds(Node?.Config?.Timeout ?? 30);
public double RefRssi { get; set; }

public bool ReadMessage(DeviceMessage payload)
{
Rssi = payload.Rssi;
RefRssi = payload.RefRssi;
Variance = payload.Variance;
NewName(payload.Name);
return NewDistance(payload.Distance);
var moved = Math.Abs(LastDistance - payload.Distance) > 0.25;
if (moved) LastDistance = payload.Distance;
Distance = payload.Distance;
LastHit = DateTime.UtcNow;
Hits++;
return moved;
}

private void NewName(string? name)
Expand All @@ -32,14 +39,4 @@ private void NewName(string? name)
Device.Name = name;
Device.Check = true;
}

private bool NewDistance(double d)
{
var moved = Math.Abs(LastDistance - d) > 0.25;
if (moved) LastDistance = d;
Distance = d;
LastHit = DateTime.UtcNow;
Hits++;
return moved;
}
}
63 changes: 48 additions & 15 deletions src/ui/src/lib/NodeMarker.svelte
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,35 +11,68 @@
const { xScale, yScale } = getContext('LayerCake');
const r = tweened(0, { duration: 100, easing: cubicOut });
const v = tweened(0, { duration: 1000, easing: cubicOut });
export let radarId: string | null = null;
export let radarId: string | undefined = null;
export let n: Node;
export let floor: Floor | undefined = null;
let radar: Device | undefined;
$: radar = $devices?.find((n) => n.id == radarId);
$: r.set(radiusOnIntersectionCircle(radar, n));
let radarDist: number | undefined;
$: radarDist = radar?.nodes[n.id]?.dist;
$: v.set(fixRadiusFromHeight(radar?.nodes[n.id]?.var));
$: r.set(fixRadiusFromHeight(radarDist));
let colors: ScaleOrdinal<string, string> = getContext('colors');
function radiusOnIntersectionCircle(d: Device | undefined, n: Node): number {
if (d == undefined) return 0;
var dr = d.nodes[n.id];
let innerStop: number = 0;
let outerStop: number = 1;
$: innerStop = 0.5 * ($r / ($r + $v));
$: outerStop = 1 - innerStop;
function fixRadiusFromHeight(dr: number | undefined): number {
if (dr == undefined) return 0;
var nz = n.point[2];
var dz = d.location.z;
var dz = (floor.bounds[1][2] - floor.bounds[0][2]) / 2.0;
var heightDifference = dz - nz;
// Check if the plane intersects the sphere
if (Math.abs(heightDifference) > dr) return 0;
var radius = Math.sqrt(Math.pow(dr, 2) - Math.pow(heightDifference, 2));
return radius;
}
}
function errorBarLength(dr: number, variance: number): { x: number; y: number } {
return { x: variance, y: variance };
}
</script>

<defs>
{#if $r > 0 && radar && radar.nodes[n.id]}
<radialGradient id="variance-gradient-{n.id}" cx="50%" cy="50%" r="{innerStop}" gradientUnits="objectBoundingBox">
<stop offset="0" style="stop-color:{colors(n.id)}; stop-opacity:0" />
<stop offset="{innerStop}" style="stop-color:{colors(n.id)}; stop-opacity:0" />
<stop offset="0.5" style="stop-color:{colors(n.id)}; stop-opacity:1" />
<stop offset="{outerStop}" style="stop-color:{colors(n.id)}; stop-opacity:0" />
<stop offset="1" style="stop-color:{colors(n.id)}; stop-opacity:0" />
</radialGradient>
{/if}
</defs>

<path d="M{$xScale(n.point[0])},{$yScale(n.point[1])} m -5,0 5,-5 5,5 -5,5 z" fill={colors(n.id)} />
<text x='{ $xScale(n.point[0]) + 7}' y='{ $yScale(n.point[1]) + 3.5}' fill='white' font-size='10px'>{n.name}</text>
{#if $r > 0}
<ellipse cx='{ $xScale(n.point[0]) }' cy='{ $yScale(n.point[1]) }' fill={colors(n.id)} stroke={colors(n.id)} fill-opacity='0.1' rx='{Math.abs($xScale(0) - $xScale($r))}' ry='{Math.abs($yScale(0) - $yScale($r))}' />
<text x='{ $xScale(n.point[0] - $r/2)}' y='{ $yScale(n.point[1] + $r/2)}' fill={colors(n.id)} font-size='12px'>{ radar?.nodes[n.id] ?? "" }</text>
<text x='{ $xScale(n.point[0] + $r/2)}' y='{ $yScale(n.point[1] - $r/2)}' fill={colors(n.id)} font-size='12px'>{ radar?.nodes[n.id] ?? "" }</text>
<text x={$xScale(n.point[0]) + 7} y={$yScale(n.point[1]) + 3.5} fill="white" font-size="10px">{n.name}</text>
{#if $r > 0 && radar && radar.nodes[n.id]}
<ellipse cx={$xScale(n.point[0])} cy={$yScale(n.point[1])} fill="none" stroke={colors(n.id)} rx={Math.abs($xScale(0) - $xScale($r))} ry={Math.abs($yScale(0) - $yScale($r))} stroke-width="2" />
<ellipse cx={$xScale(n.point[0])} cy={$yScale(n.point[1])} fill={`url(#variance-gradient-${n.id})`} rx={2*Math.abs($xScale(0)- $xScale(($r+$v)))} ry={2*Math.abs($yScale(0) - $yScale($r+$v))} fill-opacity="0.5" />
<g>
<line x1={$xScale(n.point[0] - errorBarLength(radar.nodes[n.id]?.dist, radar.nodes[n.id]?.var).x)} y1={$yScale(n.point[1])} x2={$xScale(n.point[0] + errorBarLength(radar.nodes[n.id]?.dist, radar.nodes[n.id]?.var).x)} y2={$yScale(n.point[1])} stroke={colors(n.id)} stroke-width="2" />
<line x1={$xScale(n.point[0])} y1={$yScale(n.point[1] - errorBarLength(radar.nodes[n.id]?.dist, radar.nodes[n.id]?.var).y)} x2={$xScale(n.point[0])} y2={$yScale(n.point[1] + errorBarLength(radar.nodes[n.id]?.dist, radar.nodes[n.id]?.var).y)} stroke={colors(n.id)} stroke-width="2" />
<text x={$xScale(n.point[0])} y={$yScale(n.point[1]) + 15} fill="white" font-size="10px">
{#if radar.nodes[n.id]?.var !== null && radar.nodes[n.id]?.var !== undefined}
{radar.nodes[n.id]?.dist.toFixed(2) ?? 'N/A'} ± {radar.nodes[n.id]?.var.toFixed(2)}
{:else}
{radar.nodes[n.id]?.dist.toFixed(2) ?? 'N/A'}
{/if}
</text>
</g>
{/if}
6 changes: 4 additions & 2 deletions src/ui/src/lib/Nodes.svelte
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<script lang="ts">
import { zoomIdentity } from 'd3-zoom';
import { config } from '$lib/stores';
import type { Node } from '$lib/types';
import type { Node, Floor } from '$lib/types';
import NodeMarker from './NodeMarker.svelte';
Expand All @@ -10,13 +10,15 @@
export let radarId: string | null = null;
let nodes: Node[] | undefined;
let floor: Floor | undefined;
$: nodes = floorId ? $config?.nodes?.filter((n) => n?.floors.includes(floorId)) : [];
$: floor = $config?.floors?.find((f) => f.id == floorId);
</script>

<g transform={transform.toString()}>
{#if nodes}
{#each nodes as n (n.id)}
<NodeMarker {n} {radarId} />
<NodeMarker {n} {radarId} {floor} />
{/each}
{/if}
</g>
2 changes: 1 addition & 1 deletion src/ui/src/lib/types.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ export interface Node {
export interface Device {
id: string;
name: string;
nodes: { [index: string]: number };
nodes: { [index: string]: { dist: number, var: number } };
room: { id: string, name: string };
floor: { id: string, name: string };
location: { x: number, y: number, z: number };
Expand Down

0 comments on commit 629c726

Please sign in to comment.