feat: implement particle filter
This commit is contained in:
@@ -1,21 +1,349 @@
|
||||
defmodule Localiser.Localisation.Filter.Particle do
|
||||
@behaviour Localiser.Localisation.Filter.Behaviour
|
||||
|
||||
# ── Default parameters ──────────────────────────────────────────────────────
|
||||
|
||||
@default_n_particles 300
|
||||
@default_process_noise 0.5 # σ m per update
|
||||
@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_weight_floor 1.0e-6
|
||||
@default_injection_fraction 0.03
|
||||
@default_resample_threshold 0.5
|
||||
@default_jitter_sigma 0.1
|
||||
@default_movement_model :random_walk
|
||||
@default_dropout_threshold 2_500 # ms
|
||||
@default_dropout_noise_mult 3.0
|
||||
@fallback_bounds {0.0, 0.0, 10.0, 10.0}
|
||||
|
||||
# ── Behaviour callbacks ─────────────────────────────────────────────────────
|
||||
|
||||
@impl true
|
||||
def init(_sensors, _opts) do
|
||||
# TODO: initialise particle set, weights, noise parameters
|
||||
{:ok, %{}}
|
||||
def init(_sensors, opts) do
|
||||
rooms = Keyword.get(opts, :rooms, [])
|
||||
|
||||
params = %{
|
||||
n_particles: Keyword.get(opts, :n_particles, @default_n_particles),
|
||||
movement_model: Keyword.get(opts, :movement_model, @default_movement_model),
|
||||
process_noise: Keyword.get(opts, :process_noise, @default_process_noise),
|
||||
velocity_noise: Keyword.get(opts, :velocity_noise, @default_velocity_noise),
|
||||
max_speed: Keyword.get(opts, :max_speed, @default_max_speed),
|
||||
likelihood_model: Keyword.get(opts, :likelihood_model, @default_likelihood_model),
|
||||
sensor_sigma: Keyword.get(opts, :sensor_sigma, @default_sensor_sigma),
|
||||
weight_floor: Keyword.get(opts, :weight_floor, @default_weight_floor),
|
||||
injection_fraction: Keyword.get(opts, :injection_fraction, @default_injection_fraction),
|
||||
resample_threshold: Keyword.get(opts, :resample_threshold, @default_resample_threshold),
|
||||
jitter_sigma: Keyword.get(opts, :jitter_sigma, @default_jitter_sigma),
|
||||
dropout_threshold_ms: Keyword.get(opts, :dropout_threshold_ms, @default_dropout_threshold),
|
||||
dropout_noise_mult: Keyword.get(opts, :dropout_noise_mult, @default_dropout_noise_mult)
|
||||
}
|
||||
|
||||
floor_bounds = derive_bounds(rooms)
|
||||
n = params.n_particles
|
||||
particles = for _ <- 1..n, do: sample_uniform(floor_bounds)
|
||||
weights = List.duplicate(1.0 / n, n)
|
||||
|
||||
state = Map.merge(params, %{
|
||||
particles: particles,
|
||||
weights: weights,
|
||||
floor_bounds: floor_bounds,
|
||||
last_update_ms: nil
|
||||
})
|
||||
|
||||
{:ok, state}
|
||||
end
|
||||
|
||||
@impl true
|
||||
def update(state, _measurements) do
|
||||
# TODO: predict step, weight update by likelihood, resample
|
||||
{:ok, {0.0, 0.0}, state}
|
||||
def update(state, measurements) do
|
||||
now = System.monotonic_time(:millisecond)
|
||||
dt_ms = if state.last_update_ms, do: now - state.last_update_ms, else: 0
|
||||
|
||||
dropout? = state.last_update_ms != nil and
|
||||
dt_ms > state.dropout_threshold_ms
|
||||
|
||||
{particles, weights} =
|
||||
{state.particles, state.weights}
|
||||
|> predict(state, dt_ms, dropout?)
|
||||
|> inject(state)
|
||||
|> reweight(measurements, state)
|
||||
|> maybe_resample(state)
|
||||
|
||||
{x_est, y_est} = weighted_mean(particles, weights)
|
||||
|
||||
new_state = %{state |
|
||||
particles: particles,
|
||||
weights: weights,
|
||||
last_update_ms: now
|
||||
}
|
||||
|
||||
{:ok, {x_est, y_est}, new_state}
|
||||
end
|
||||
|
||||
@impl true
|
||||
def estimate(_state) do
|
||||
# TODO: weighted mean of particle set
|
||||
{{0.0, 0.0}, 0.0}
|
||||
def estimate(state) do
|
||||
{x_est, y_est} = weighted_mean(state.particles, state.weights)
|
||||
confidence = n_eff(state.weights) / state.n_particles
|
||||
{{x_est, y_est}, confidence}
|
||||
end
|
||||
|
||||
# ── Predict step ─────────────────────────────────────────────────────────────
|
||||
|
||||
defp predict({particles, weights}, state, dt_ms, dropout?) do
|
||||
base_noise = state.process_noise
|
||||
|
||||
sigma_eff =
|
||||
if dropout? do
|
||||
mult = min(state.dropout_noise_mult * :math.sqrt(dt_ms / state.dropout_threshold_ms), 5.0)
|
||||
base_noise * mult
|
||||
else
|
||||
base_noise
|
||||
end
|
||||
|
||||
new_particles =
|
||||
case state.movement_model do
|
||||
:random_walk ->
|
||||
Enum.map(particles, fn {x, y} ->
|
||||
reflect_bounds(
|
||||
{x + randn(sigma_eff), y + randn(sigma_eff)},
|
||||
state.floor_bounds
|
||||
)
|
||||
end)
|
||||
|
||||
:constant_velocity ->
|
||||
Enum.map(particles, fn particle ->
|
||||
{x, y, vx, vy} = ensure_velocity(particle)
|
||||
|
||||
vx2 = vx + randn(state.velocity_noise)
|
||||
vy2 = vy + randn(state.velocity_noise)
|
||||
speed = :math.sqrt(vx2 * vx2 + vy2 * vy2)
|
||||
|
||||
{vx2, vy2} =
|
||||
if speed > state.max_speed do
|
||||
s = state.max_speed / speed
|
||||
{vx2 * s, vy2 * s}
|
||||
else
|
||||
{vx2, vy2}
|
||||
end
|
||||
|
||||
{nx, ny} = reflect_bounds(
|
||||
{x + vx2 + randn(sigma_eff), y + vy2 + randn(sigma_eff)},
|
||||
state.floor_bounds
|
||||
)
|
||||
|
||||
{nx, ny, vx2, vy2}
|
||||
end)
|
||||
end
|
||||
|
||||
{new_particles, weights}
|
||||
end
|
||||
|
||||
# ── Inject random particles ───────────────────────────────────────────────
|
||||
|
||||
defp inject({particles, weights}, state) do
|
||||
n_inject = floor(state.injection_fraction * state.n_particles)
|
||||
|
||||
if n_inject == 0 do
|
||||
{particles, weights}
|
||||
else
|
||||
mean_w = Enum.sum(weights) / length(weights)
|
||||
|
||||
# Find the n_inject indices with the lowest weights
|
||||
indexed = Enum.with_index(weights)
|
||||
low_idxs =
|
||||
indexed
|
||||
|> Enum.sort_by(fn {w, _} -> w end)
|
||||
|> Enum.take(n_inject)
|
||||
|> Enum.map(fn {_, i} -> i end)
|
||||
|> MapSet.new()
|
||||
|
||||
particles_arr = List.to_tuple(particles)
|
||||
weights_arr = List.to_tuple(weights)
|
||||
|
||||
{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]}
|
||||
else
|
||||
{[elem(particles_arr, i) | ps], [elem(weights_arr, i) | ws]}
|
||||
end
|
||||
end)
|
||||
|
||||
{Enum.reverse(new_p), Enum.reverse(new_w)}
|
||||
end
|
||||
end
|
||||
|
||||
# ── Reweight by sensor likelihood ─────────────────────────────────────────
|
||||
|
||||
defp reweight({particles, weights}, [], _state), do: {particles, weights}
|
||||
|
||||
defp reweight({particles, weights}, measurements, state) do
|
||||
new_weights =
|
||||
Enum.zip(particles, weights)
|
||||
|> Enum.map(fn {particle, w} ->
|
||||
{px, py} = particle_pos(particle)
|
||||
|
||||
w_updated =
|
||||
Enum.reduce(measurements, max(w, state.weight_floor), fn m, acc ->
|
||||
d_pred = :math.sqrt((px - m.floor_x) * (px - m.floor_x) +
|
||||
(py - m.floor_y) * (py - m.floor_y))
|
||||
|
||||
l = case state.likelihood_model do
|
||||
:gaussian -> gaussian_likelihood(m.distance, d_pred, state.sensor_sigma)
|
||||
:laplacian -> laplacian_likelihood(m.distance, d_pred, state.sensor_sigma)
|
||||
end
|
||||
|
||||
acc * max(l, state.weight_floor)
|
||||
end)
|
||||
|
||||
w_updated
|
||||
end)
|
||||
|
||||
sum = Enum.sum(new_weights)
|
||||
|
||||
normalised =
|
||||
if sum > 0.0 do
|
||||
Enum.map(new_weights, &(&1 / sum))
|
||||
else
|
||||
List.duplicate(1.0 / state.n_particles, state.n_particles)
|
||||
end
|
||||
|
||||
{particles, normalised}
|
||||
end
|
||||
|
||||
# ── Conditional systematic resample ───────────────────────────────────────
|
||||
|
||||
defp maybe_resample({particles, weights}, state) do
|
||||
neff = n_eff(weights)
|
||||
|
||||
if neff / state.n_particles < state.resample_threshold do
|
||||
new_particles = systematic_resample(particles, weights, state.n_particles)
|
||||
jitter_sigma = state.jitter_sigma
|
||||
bounds = state.floor_bounds
|
||||
|
||||
jittered =
|
||||
Enum.map(new_particles, fn p ->
|
||||
{x, y} = particle_pos(p)
|
||||
reflect_bounds({x + randn(jitter_sigma), y + randn(jitter_sigma)}, bounds)
|
||||
end)
|
||||
|
||||
uniform_w = List.duplicate(1.0 / state.n_particles, state.n_particles)
|
||||
{jittered, uniform_w}
|
||||
else
|
||||
{particles, weights}
|
||||
end
|
||||
end
|
||||
|
||||
# ── Helpers ────────────────────────────────────────────────────────────────
|
||||
|
||||
defp systematic_resample(particles, weights, n) do
|
||||
cdf = Enum.scan(weights, 0.0, &(&1 + &2))
|
||||
u0 = :rand.uniform() / n
|
||||
|
||||
p_tuple = List.to_tuple(particles)
|
||||
|
||||
Enum.reduce(1..n, {[], 0, u0}, fn _, {acc, j, u} ->
|
||||
j2 = advance_cdf(cdf, j, u)
|
||||
{[elem(p_tuple, j2) | acc], j2, u + 1.0 / n}
|
||||
end)
|
||||
|> elem(0)
|
||||
|> Enum.reverse()
|
||||
end
|
||||
|
||||
defp advance_cdf(cdf, j, u) do
|
||||
cdf_len = length(cdf)
|
||||
if j >= cdf_len - 1 do
|
||||
cdf_len - 1
|
||||
else
|
||||
if Enum.at(cdf, j) >= u do
|
||||
j
|
||||
else
|
||||
advance_cdf(cdf, j + 1, u)
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
defp weighted_mean(particles, weights) do
|
||||
{x_sum, y_sum} =
|
||||
Enum.zip(particles, weights)
|
||||
|> Enum.reduce({0.0, 0.0}, fn {p, w}, {xs, ys} ->
|
||||
{px, py} = particle_pos(p)
|
||||
{xs + w * px, ys + w * py}
|
||||
end)
|
||||
|
||||
{x_sum, y_sum}
|
||||
end
|
||||
|
||||
defp n_eff(weights) do
|
||||
sq_sum = Enum.reduce(weights, 0.0, fn w, acc -> acc + w * w end)
|
||||
if sq_sum > 0.0, do: 1.0 / sq_sum, else: 0.0
|
||||
end
|
||||
|
||||
defp gaussian_likelihood(d_obs, d_pred, sigma) do
|
||||
diff = d_obs - d_pred
|
||||
:math.exp(-0.5 * diff * diff / (sigma * sigma))
|
||||
end
|
||||
|
||||
defp laplacian_likelihood(d_obs, d_pred, b) do
|
||||
:math.exp(-abs(d_obs - d_pred) / b)
|
||||
end
|
||||
|
||||
defp reflect_bounds({x, y}, {x_min, y_min, x_max, y_max}) do
|
||||
{reflect_1d(x, x_min, x_max), reflect_1d(y, y_min, y_max)}
|
||||
end
|
||||
|
||||
defp reflect_1d(v, lo, hi) do
|
||||
range = hi - lo
|
||||
|
||||
if range <= 0.0 do
|
||||
lo
|
||||
else
|
||||
v2 = v - lo
|
||||
# Fold into [0, range] using triangular reflection
|
||||
period = 2.0 * range
|
||||
v3 = v2 - period * Float.floor(v2 / period)
|
||||
lo + if v3 <= range, do: v3, else: period - v3
|
||||
end
|
||||
end
|
||||
|
||||
defp sample_uniform({x_min, y_min, x_max, y_max}) do
|
||||
{:rand.uniform() * (x_max - x_min) + x_min,
|
||||
:rand.uniform() * (y_max - y_min) + y_min}
|
||||
end
|
||||
|
||||
# Box-Muller transform for N(0, σ)
|
||||
defp randn(sigma) do
|
||||
u1 = max(:rand.uniform(), 1.0e-15)
|
||||
u2 = :rand.uniform()
|
||||
sigma * :math.sqrt(-2.0 * :math.log(u1)) * :math.cos(2.0 * :math.pi() * u2)
|
||||
end
|
||||
|
||||
# Strip velocity component to get {x, y} regardless of motion model
|
||||
defp particle_pos({x, y}), do: {x, y}
|
||||
defp particle_pos({x, y, _vx, _vy}), do: {x, y}
|
||||
|
||||
# Ensure particle has velocity component (for switching to CV mid-run)
|
||||
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 derive_bounds([]) do
|
||||
@fallback_bounds
|
||||
end
|
||||
|
||||
defp derive_bounds(rooms) do
|
||||
coords =
|
||||
Enum.flat_map(rooms, fn r ->
|
||||
ox = r.offset_x || 0.0
|
||||
oy = r.offset_y || 0.0
|
||||
w = r.width || 0.0
|
||||
h = r.height || 0.0
|
||||
[{ox, oy}, {ox + w, oy + h}]
|
||||
end)
|
||||
|
||||
xs = Enum.map(coords, &elem(&1, 0))
|
||||
ys = Enum.map(coords, &elem(&1, 1))
|
||||
|
||||
{Enum.min(xs), Enum.min(ys), Enum.max(xs), Enum.max(ys)}
|
||||
end
|
||||
end
|
||||
|
||||
Reference in New Issue
Block a user