feat: implement multidistance rssi->distance model parameter estimation

lib/localiser/localisation/calibration.ex

@@ -0,0 +1,86 @@
+defmodule Localiser.Localisation.Calibration do
+  @moduledoc false
+
+  # Returns [{rssi, is_outlier}] using Tukey IQR fences on the full sample list.
+  def classify_outliers([]), do: []
+
+  def classify_outliers(samples) do
+    {lo, hi} = iqr_fences(samples)
+    Enum.map(samples, fn rssi -> {rssi, rssi < lo or rssi > hi} end)
+  end
+
+  # Live hint: is this single reading an outlier relative to the samples collected so far?
+  # Returns false when fewer than 5 existing samples (not enough signal).
+  def outlier?(_reading, existing) when length(existing) < 5, do: false
+
+  def outlier?(reading, existing) do
+    {lo, hi} = iqr_fences(existing)
+    reading < lo or reading > hi
+  end
+
+  # OLS regression for RSSI = A - 10n * log10(d).
+  # stages :: [%{distance: float, mean_rssi: float}]
+  # Requires at least 2 stages with distinct distances.
+  # Returns {:ok, {rssi_ref :: integer, path_loss_exp :: float}} or {:error, :insufficient_data}.
+  def least_squares(stages) when length(stages) < 2, do: {:error, :insufficient_data}
+
+  def least_squares(stages) do
+    points = Enum.map(stages, fn %{distance: d, mean_rssi: rssi} ->
+      {:math.log10(d), rssi}
+    end)
+
+    xs = Enum.map(points, &elem(&1, 0))
+    ys = Enum.map(points, &elem(&1, 1))
+
+    x_bar = mean(xs)
+    y_bar = mean(ys)
+
+    cov_xy = xs |> Enum.zip(ys) |> Enum.reduce(0.0, fn {x, y}, acc ->
+      acc + (x - x_bar) * (y - y_bar)
+    end)
+
+    var_x = Enum.reduce(xs, 0.0, fn x, acc -> acc + (x - x_bar) * (x - x_bar) end)
+
+    if var_x == 0.0 do
+      {:error, :insufficient_data}
+    else
+      beta = cov_xy / var_x
+      a = y_bar - beta * x_bar
+
+      path_loss_exp = -beta / 10.0
+      rssi_ref = round(a)
+
+      {:ok, {rssi_ref, path_loss_exp}}
+    end
+  end
+
+  # --- Private ---
+
+  defp iqr_fences(samples) do
+    sorted = Enum.sort(samples)
+    n = length(sorted)
+
+    q1 = percentile(sorted, n, 0.25)
+    q3 = percentile(sorted, n, 0.75)
+    iqr = q3 - q1
+
+    {q1 - 1.5 * iqr, q3 + 1.5 * iqr}
+  end
+
+  defp percentile(sorted, n, p) do
+    idx = p * (n - 1)
+    lo = floor(idx)
+    hi = ceil(idx)
+
+    if lo == hi do
+      Enum.at(sorted, lo) * 1.0
+    else
+      frac = idx - lo
+      Enum.at(sorted, lo) * (1 - frac) + Enum.at(sorted, hi) * frac
+    end
+  end
+
+  defp mean(list) do
+    Enum.sum(list) / length(list)
+  end
+end

lib/localiser/localisation/filter/particle.ex

@@ -8,7 +8,7 @@ defmodule Localiser.Localisation.Filter.Particle do
   @default_velocity_noise     0.3    # σ m/s per update (constant velocity model)
   @default_max_speed          2.0    # m per update cap (constant velocity model)
   @default_likelihood_model   :laplacian
-  @default_sensor_sigma       3.0    # σ (Gaussian) or b (Laplacian) in metres
+  @default_sensor_sigma       1.0    # σ (Gaussian) or b (Laplacian) in metres
   @default_weight_floor       1.0e-6
   @default_injection_fraction 0.03
   @default_resample_threshold 0.5

lib/localiser/localisation/sensor/server.ex

@@ -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