feat: implement multidistance rssi->distance model parameter estimation

2026-05-21 18:31:07 +02:00
parent 7b02a37abe
commit bacf56156b
9 changed files with 438 additions and 78 deletions
@@ -5,12 +5,19 @@ defmodule Localiser.Localisation.Sensor.Server do

  alias Localiser.Domain.Sensors
  alias Localiser.Domain.Schema.{Sensor, SensorCalibration}
+  alias Localiser.Localisation.Calibration
  alias Localiser.MQTT.Connection, as: MQTTConnection

  @default_rssi_ref -59
  @default_path_loss_exp 2.0
+  @default_samples 30

-  # mode: :ok | {:calibrating, buffer :: [integer()], target :: pos_integer()}
+  # mode:
+  #   :ok
+  #   {:calibration_mode, completed_stages}
+  #   {:calibrating_stage, distance, samples, completed_stages}
+  #
+  # completed_stages :: [%{distance: float, mean_rssi: float, readings: [{rssi, is_outlier}]}]
  defstruct [:sensor_id, :sensor_db_id, :floor_x, :floor_y, :rssi_ref, :path_loss_exp, mode: :ok]

  def start_link({sensor, room}) do
@@ -21,28 +28,42 @@ defmodule Localiser.Localisation.Sensor.Server do
    {:via, Registry, {Localiser.Registry, {:sensor, sensor_id}}}
  end

-  # Returns %{sensor_id, floor_x, floor_y, distance, rssi, tx_power} for a raw RSSI reading.
-  # tx_power is the beacon-advertised expected RSSI at 1 m (nil if not available).
  def measure(sensor_id, rssi, tx_power \\ nil) do
    GenServer.call(via(sensor_id), {:measure, rssi, tx_power})
  end

-  # Returns true if the sensor is currently collecting calibration samples.
+  # Returns true only when a stage is actively collecting (used by RSSI.Buffer).
  def calibrating?(sensor_id) do
    GenServer.call(via(sensor_id), :calibrating?)
  end

-  # Feeds a raw RSSI value into the calibration buffer.
+  # Returns a JSON-serialisable snapshot of the current calibration state for channel join.
+  def calibration_state(sensor_id) do
+    GenServer.call(via(sensor_id), :calibration_state)
+  end
+
+  # Feeds a raw RSSI value into the active stage buffer. Ignored between stages.
  def calibration_reading(sensor_id, rssi) do
    GenServer.cast(via(sensor_id), {:calibration_reading, rssi})
  end

-  # Starts calibration mode. sample_target: number of RSSI samples to collect.
-  def begin_calibration(sensor_id, sample_target \\ 50) do
-    GenServer.cast(via(sensor_id), {:begin_calibration, sample_target})
+  # Puts the sensor into calibration mode (between-stages). Returns :ok.
+  def begin_calibration_mode(sensor_id) do
+    GenServer.call(via(sensor_id), :begin_calibration_mode)
  end

-  # Aborts an in-progress calibration without saving.
+  # Starts collecting samples for a given distance. Returns {:ok, samples_needed} or {:error, reason}.
+  def start_stage(sensor_id, distance) do
+    GenServer.call(via(sensor_id), {:start_stage, distance})
+  end
+
+  # Runs OLS regression over completed stages and saves the result. Requires >= 2 stages.
+  # Returns {:ok, %{rssi_ref: integer, path_loss_exp: float}} or {:error, reason}.
+  def finish_calibration(sensor_id) do
+    GenServer.call(via(sensor_id), :finish_calibration)
+  end
+
+  # Aborts calibration from any state, discarding all stages.
  def abort_calibration(sensor_id) do
    GenServer.cast(via(sensor_id), :abort_calibration)
  end
@@ -84,37 +105,144 @@ defmodule Localiser.Localisation.Sensor.Server do

  @impl true
  def handle_call(:calibrating?, _from, state) do
-    {:reply, match?({:calibrating, _, _}, state.mode), state}
+    {:reply, match?({:calibrating_stage, _, _, _}, state.mode), state}
  end

  @impl true
-  def handle_cast({:begin_calibration, target}, state) do
+  def handle_call(:calibration_state, _from, state) do
+    snapshot = case state.mode do
+      :ok ->
+        %{status: "idle"}
+
+      {:calibration_mode, completed} ->
+        %{
+          status: "calibration_mode",
+          samples_needed: samples_needed(),
+          completed_stages: Enum.map(completed, &render_stage/1)
+        }
+
+      {:calibrating_stage, distance, samples, completed} ->
+        %{
+          status: "stage_active",
+          distance: distance,
+          samples_needed: samples_needed(),
+          stage_progress: {length(samples), samples_needed()},
+          completed_stages: Enum.map(completed, &render_stage/1)
+        }
+    end
+
+    {:reply, snapshot, state}
+  end
+
+  @impl true
+  def handle_call(:begin_calibration_mode, _from, state) do
    MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_start"}))
-    {:noreply, %{state | mode: {:calibrating, [], target}}}
+    broadcast_calibration(state.sensor_id, {:calibration_mode_entered, state.sensor_id, samples_needed()})
+    {:reply, :ok, %{state | mode: {:calibration_mode, []}}}
  end

  @impl true
-  def handle_cast(:abort_calibration, %{mode: {:calibrating, _, _}} = state) do
-    MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_stop"}))
-    {:noreply, %{state | mode: :ok}}
+  def handle_call({:start_stage, _distance}, _from, %{mode: {:calibrating_stage, _, _, _}} = state) do
+    {:reply, {:error, :already_active}, state}
  end

-  def handle_cast(:abort_calibration, state), do: {:noreply, state}
+  def handle_call({:start_stage, _distance}, _from, %{mode: :ok} = state) do
+    {:reply, {:error, :not_in_calibration_mode}, state}
+  end
+
+  def handle_call({:start_stage, distance}, _from, %{mode: {:calibration_mode, completed}} = state) do
+    n = samples_needed()
+    broadcast_calibration(state.sensor_id, {:stage_started, state.sensor_id, distance, length(completed)})
+    {:reply, {:ok, n}, %{state | mode: {:calibrating_stage, distance, [], completed}}}
+  end

  @impl true
-  def handle_cast({:calibration_reading, rssi}, %{mode: {:calibrating, buffer, target}} = state) do
-    buffer = [rssi | buffer]
+  def handle_call(:finish_calibration, _from, %{mode: {:calibration_mode, completed}} = state)
+      when length(completed) >= 2 do
+    case Calibration.least_squares(completed) do
+      {:ok, {rssi_ref, path_loss_exp}} ->
+        sensor_struct = %Sensor{id: state.sensor_db_id, sensor_id: state.sensor_id}

-    if length(buffer) >= target do
-      finalize_calibration(buffer, state)
+        case Sensors.add_calibration(sensor_struct, %{
+               rssi_ref: rssi_ref,
+               path_loss_exp: path_loss_exp,
+               calibrated_at: DateTime.utc_now()
+             }) do
+          {:ok, _} ->
+            Logger.info("[Sensor.Server] Calibration finished for #{state.sensor_id}: rssi_ref=#{rssi_ref} n=#{path_loss_exp}")
+            MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_stop"}))
+            broadcast_calibration(state.sensor_id, {:calibration_finished, state.sensor_id, rssi_ref, path_loss_exp})
+            Phoenix.PubSub.broadcast(Localiser.PubSub, "sensors", {:calibration_complete, state.sensor_id, rssi_ref, path_loss_exp})
+            result = %{rssi_ref: rssi_ref, path_loss_exp: path_loss_exp}
+            {:reply, {:ok, result}, %{state | rssi_ref: rssi_ref, path_loss_exp: path_loss_exp, mode: :ok}}
+
+          {:error, reason} ->
+            Logger.error("[Sensor.Server] Failed to save calibration for #{state.sensor_id}: #{inspect(reason)}")
+            {:reply, {:error, :save_failed}, state}
+        end
+
+      {:error, reason} ->
+        {:reply, {:error, reason}, state}
+    end
+  end
+
+  def handle_call(:finish_calibration, _from, %{mode: {:calibration_mode, _}} = state) do
+    {:reply, {:error, :insufficient_stages}, state}
+  end
+
+  def handle_call(:finish_calibration, _from, %{mode: {:calibrating_stage, _, _, _}} = state) do
+    {:reply, {:error, :stage_active}, state}
+  end
+
+  def handle_call(:finish_calibration, _from, state) do
+    {:reply, {:error, :not_in_calibration_mode}, state}
+  end
+
+  @impl true
+  def handle_cast(:abort_calibration, state) do
+    case state.mode do
+      :ok ->
+        {:noreply, state}
+
+      _ ->
+        MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_stop"}))
+        broadcast_calibration(state.sensor_id, {:calibration_cancelled, state.sensor_id})
+        {:noreply, %{state | mode: :ok}}
+    end
+  end
+
+  @impl true
+  def handle_cast({:calibration_reading, rssi}, %{mode: {:calibrating_stage, distance, samples, completed}} = state) do
+    n = samples_needed()
+    is_outlier = Calibration.outlier?(rssi, samples)
+    new_samples = [rssi | samples]
+    progress = {length(new_samples), n}
+
+    broadcast_calibration(state.sensor_id, {:calibration_reading, state.sensor_id, rssi, is_outlier, %{stage: progress}})
+
+    if length(new_samples) >= n do
+      classified = Calibration.classify_outliers(new_samples)
+      clean = for {r, false} <- classified, do: r
+
+      mean_rssi = if clean == [] do
+        mean(new_samples)
+      else
+        mean(clean)
+      end
+
+      stage = %{distance: distance, mean_rssi: mean_rssi, readings: classified}
+      new_completed = [stage | completed]
+
+      broadcast_calibration(state.sensor_id, {:stage_complete, state.sensor_id, distance, classified, mean_rssi})
+
+      {:noreply, %{state | mode: {:calibration_mode, new_completed}}}
    else
-      {:noreply, %{state | mode: {:calibrating, buffer, target}}}
+      {:noreply, %{state | mode: {:calibrating_stage, distance, new_samples, completed}}}
    end
  end

  def handle_cast({:calibration_reading, _rssi}, state), do: {:noreply, state}

-  # Position updated (sensor dragged in layout).
  @impl true
  def handle_info({:sensor_enrolled, %Sensor{sensor_id: sid} = sensor, room}, %{sensor_id: sid} = state) do
    floor_x = (room.x || 0.0) + (sensor.x || 0.0)
@@ -122,45 +250,26 @@ defmodule Localiser.Localisation.Sensor.Server do
    {:noreply, %{state | floor_x: floor_x, floor_y: floor_y}}
  end

-  # Ignore PubSub messages not relevant to this server.
  def handle_info(_msg, state), do: {:noreply, state}

  # Private

-  defp finalize_calibration(buffer, state) do
-    rssi_ref = median(buffer)
-    sensor_struct = %Sensor{id: state.sensor_db_id, sensor_id: state.sensor_id}
-
-    case Sensors.add_calibration(sensor_struct, %{
-           rssi_ref: rssi_ref,
-           path_loss_exp: state.path_loss_exp,
-           calibrated_at: DateTime.utc_now()
-         }) do
-      {:ok, _calibration} ->
-        Logger.info("[Sensor.Server] Calibration complete for #{state.sensor_id}: rssi_ref=#{rssi_ref}")
-        MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_stop"}))
-        Phoenix.PubSub.broadcast(Localiser.PubSub, "sensors", {:calibration_complete, state.sensor_id})
-        {:noreply, %{state | rssi_ref: rssi_ref, mode: :ok}}
-
-      {:error, reason} ->
-        Logger.error("[Sensor.Server] Failed to save calibration for #{state.sensor_id}: #{inspect(reason)}")
-        {:noreply, %{state | mode: :ok}}
-    end
+  defp broadcast_calibration(sensor_id, message) do
+    Phoenix.PubSub.broadcast(Localiser.PubSub, "calibration:#{sensor_id}", message)
  end

-  defp median(list) do
-    sorted = Enum.sort(list)
-    len = length(sorted)
-    mid = div(len, 2)
-
-    if rem(len, 2) == 0 do
-      round((Enum.at(sorted, mid - 1) + Enum.at(sorted, mid)) / 2)
-    else
-      Enum.at(sorted, mid)
-    end
+  defp render_stage(%{distance: d, mean_rssi: r, readings: readings}) do
+    %{distance: d, mean_rssi: r, readings: Enum.map(readings, fn {rssi, outlier} -> %{rssi: rssi, outlier: outlier} end)}
+  end
+
+  defp samples_needed do
+    Application.get_env(:localiser, :calibration_samples, @default_samples)
+  end
+
+  defp mean(list) do
+    Enum.sum(list) / length(list)
  end

-  # d = 10 ^ ((rssi_ref - rssi) / (10 * n))
  defp rssi_to_distance(rssi, rssi_ref, path_loss_exp) do
    :math.pow(10.0, (rssi_ref - rssi) / (10.0 * path_loss_exp))
  end