close all
clear
clc

robot = Robot(0.3);

num_particles = 50;
x = 6 * rand(1, num_particles) - 3;
w = ones(size(x)) / length(x);

motion_sigma = 0.05;
measurement_sigma = 0.05;

dx = 1;
detected = false;
while dx ~= 0
    mean = sum(w .* x);
    [detected, dx] = robot.step(x, w, mean);
    x = x + dx + motion_sigma * randn(size(x));
    if detected
        w = w .* robot.prob(x, measurement_sigma);
    end
    w = w / sum(w);
    if max(w) / min(w) > 4 * num_particles
        [x, w] = robot.resample(x, w);
    end
end