From d3b823087c922b1a5adb3ef9f617f2aac15f4c19 Mon Sep 17 00:00:00 2001 From: dvdrw Date: Fri, 5 Jun 2026 13:19:54 +0200 Subject: [PATCH] feat(filter): constrain particles to rooms, not bounding box --- lib/localiser/domain/sensors.ex | 6 -- lib/localiser/localisation/filter/particle.ex | 65 +++++++++++++++++-- lib/localiser/localisation/sensor/server.ex | 1 + lib/localiser/mqtt/router.ex | 4 ++ 4 files changed, 65 insertions(+), 11 deletions(-) diff --git a/lib/localiser/domain/sensors.ex b/lib/localiser/domain/sensors.ex index 88121db..0080837 100644 --- a/lib/localiser/domain/sensors.ex +++ b/lib/localiser/domain/sensors.ex @@ -129,12 +129,6 @@ defmodule Localiser.Domain.Sensors do end end - def enroll_sensor(%Sensor{} = sensor, room_id) do - sensor - |> Sensor.changeset(%{room_id: room_id}) - |> Repo.update() - end - def add_calibration(%Sensor{} = sensor, attrs) do attrs = Map.put(attrs, :sensor_id, sensor.id) diff --git a/lib/localiser/localisation/filter/particle.ex b/lib/localiser/localisation/filter/particle.ex index 7b63282..76ffb81 100644 --- a/lib/localiser/localisation/filter/particle.ex +++ b/lib/localiser/localisation/filter/particle.ex @@ -41,14 +41,16 @@ defmodule Localiser.Localisation.Filter.Particle do } floor_bounds = derive_bounds(rooms) + room_bounds = derive_room_bounds(rooms) n = params.n_particles - particles = for _ <- 1..n, do: sample_uniform(floor_bounds) + particles = for _ <- 1..n, do: sample_in_rooms(room_bounds, floor_bounds) weights = List.duplicate(1.0 / n, n) state = Map.merge(params, %{ particles: particles, weights: weights, floor_bounds: floor_bounds, + room_bounds: room_bounds, last_update_ms: nil }) @@ -105,8 +107,9 @@ defmodule Localiser.Localisation.Filter.Particle do case state.movement_model do :random_walk -> Enum.map(particles, fn {x, y} -> - reflect_bounds( + constrain_to_rooms( {x + randn(sigma_eff), y + randn(sigma_eff)}, + state.room_bounds, state.floor_bounds ) end) @@ -127,8 +130,9 @@ defmodule Localiser.Localisation.Filter.Particle do {vx2, vy2} end - {nx, ny} = reflect_bounds( + {nx, ny} = constrain_to_rooms( {x + vx2 + randn(sigma_eff), y + vy2 + randn(sigma_eff)}, + state.room_bounds, state.floor_bounds ) @@ -164,7 +168,7 @@ defmodule Localiser.Localisation.Filter.Particle do {new_p, new_w} = Enum.reduce(0..(state.n_particles - 1), {[], []}, fn i, {ps, ws} -> if MapSet.member?(low_idxs, i) do - {[sample_uniform(state.floor_bounds) | ps], [mean_w | ws]} + {[sample_in_rooms(state.room_bounds, state.floor_bounds) | ps], [mean_w | ws]} else {[elem(particles_arr, i) | ps], [elem(weights_arr, i) | ws]} end @@ -225,7 +229,7 @@ defmodule Localiser.Localisation.Filter.Particle do jittered = Enum.map(new_particles, fn p -> {x, y} = particle_pos(p) - reflect_bounds({x + randn(jitter_sigma), y + randn(jitter_sigma)}, bounds) + constrain_to_rooms({x + randn(jitter_sigma), y + randn(jitter_sigma)}, state.room_bounds, bounds) end) uniform_w = List.duplicate(1.0 / state.n_particles, state.n_particles) @@ -327,6 +331,57 @@ defmodule Localiser.Localisation.Filter.Particle do defp ensure_velocity({x, y}), do: {x, y, 0.0, 0.0} defp ensure_velocity({x, y, vx, vy}), do: {x, y, vx, vy} + defp constrain_to_rooms({x, y}, [], fallback_bounds) do + reflect_bounds({x, y}, fallback_bounds) + end + + defp constrain_to_rooms({x, y}, room_bounds, _fallback) do + if Enum.any?(room_bounds, fn {x0, y0, x1, y1} -> + x >= x0 and x <= x1 and y >= y0 and y <= y1 + end) do + {x, y} + else + {x0, y0, x1, y1} = + Enum.min_by(room_bounds, fn {x0, y0, x1, y1} -> + dx = max(0.0, max(x0 - x, x - x1)) + dy = max(0.0, max(y0 - y, y - y1)) + dx * dx + dy * dy + end) + + {clamp(x, x0, x1), clamp(y, y0, y1)} + end + end + + defp clamp(v, lo, hi), do: max(lo, min(hi, v)) + + defp sample_in_rooms([], fallback_bounds), do: sample_uniform(fallback_bounds) + + defp sample_in_rooms(room_bounds, _fallback) do + areas = Enum.map(room_bounds, fn {x0, y0, x1, y1} -> (x1 - x0) * (y1 - y0) end) + total = Enum.sum(areas) + r = :rand.uniform() * total + + {x0, y0, x1, y1} = + Enum.zip(room_bounds, areas) + |> Enum.reduce_while(r, fn {bounds, area}, acc -> + if acc <= area, do: {:halt, bounds}, else: {:cont, acc - area} + end) + |> then(fn + {_, _, _, _} = b -> b + _ -> List.last(room_bounds) + end) + + sample_uniform({x0, y0, x1, y1}) + end + + defp derive_room_bounds(rooms) do + Enum.map(rooms, fn r -> + ox = r.x || 0.0 + oy = r.y || 0.0 + {ox, oy, ox + (r.width || 0.0), oy + (r.height || 0.0)} + end) + end + defp derive_bounds([]) do @fallback_bounds end diff --git a/lib/localiser/localisation/sensor/server.ex b/lib/localiser/localisation/sensor/server.ex index 5a0045d..b61701c 100644 --- a/lib/localiser/localisation/sensor/server.ex +++ b/lib/localiser/localisation/sensor/server.ex @@ -248,6 +248,7 @@ defmodule Localiser.Localisation.Sensor.Server do # Scan mode: tag not yet committed - advertise all (tag_id, rssi) pairs for UI selection. @impl true def handle_cast({:calibration_reading, tag_id, rssi}, %{mode: {:calibration_mode, nil, _}} = state) do + Logger.error("Calibration reading in scan mode: tag_id=#{tag_id} rssi=#{rssi}") broadcast_calibration(state.sensor_id, {:calibration_scan_reading, state.sensor_id, tag_id, rssi}) {:noreply, state} end diff --git a/lib/localiser/mqtt/router.ex b/lib/localiser/mqtt/router.ex index 0112e59..280b02a 100644 --- a/lib/localiser/mqtt/router.ex +++ b/lib/localiser/mqtt/router.ex @@ -51,6 +51,10 @@ defmodule Localiser.MQTT.Router do end defp handle_rssi(sensor_id, payload) do + if sensor_id == "anchor_73f460" do + Logger.error("Sensor send payload: #{payload}") + end + case Jason.decode(payload) do {:ok, %{"id" => id, "rssi" => rssi, "type" => type} = decoded} -> reading = %{