Programming for Robotics 1, Chapter 3: Challenge 2 solution (a slip option, and the drift it causes)

Idea: slip means the wheel turns and the encoder counts it, but the robot does not move as far. In the model that is a smaller ground distance for the same wheel angle, which is exactly what scale_left and scale_right already do, so SlipModel only has to change the scale for the physics steps on which a slip happens and put it back afterwards. The encoders read the wheel angle, which is untouched, so odometry sees a perfect straight line and the truth wanders off it.
    - Each physics step, each wheel independently: with probability p = 0.05 its ground distance is multiplied by (1 - loss) = 0.5.
    - The model has its own random.Random(seed): the result depends only on the seed, and a run can be repeated (the same rule as every sensor in the chapter).
    - The Simulator creates its own RobotModel; the new one is put in its place (sim.model = ...) before any command is given, with the same start pose.

Prediction by algebra, so that the result is checked and not just printed. At duty 0.5 the wheels run at 0.20 m/s, so in one 5 ms physics step a wheel rolls  d0 = 0.20 x 0.005 = 1 mm.
    Distance.   A wheel's ground distance is a fraction (1 - loss) of d0 on a share p of the steps and d0 on the others, so on average
                true / odometry = 1 - p x loss = 1 - 0.05 x 0.5 = 0.975.
    Heading.    On one step the heading changes by (loss on the right - loss on the left) / L. Each loss is d0 x loss with probability p, otherwise 0, and the two wheels are independent, so
                variance per step = (d0 x loss / L)^2 x 2 p (1 - p),     since a Bernoulli variable with probability p has variance p (1 - p).
                The steps are independent, so the variances add. The sum of d0^2 over the run is  D x d0  (D = 3.0 m of travel, d0 per step; the speed is constant once the motors are up to speed):
                variance = D d0 (loss / L)^2 2 p (1 - p) = 3.0 x 0.001 x (0.5 / 0.15)^2 x 0.095 = 0.003167 rad^2,    standard deviation 0.0563 rad = 3.22 degrees.
                The mean is zero: left and right slips are equally likely.

CODE:
class SlipModel(RobotModel):
    """A RobotModel in which, on a random share of the physics steps, a wheel loses part of its ground distance."""

    def __init__(self, params, rng, slip_prob=0.05, slip_loss=0.5, **kwargs):
        RobotModel.__init__(self, params, **kwargs)
        self.rng = rng
        self.slip_prob = slip_prob                     # chance per wheel per physics step
        self.slip_loss = slip_loss                     # share of that step's ground distance that is lost

    def step(self, dt_s):
        p = self.p
        base = (p.scale_left, p.scale_right)
        p.scale_left, p.scale_right = [b * (1 - self.slip_loss) if self.rng.random() < self.slip_prob else b for b in base]
        RobotModel.step(self, dt_s)                    # the encoder angles are unaffected: the wheels did turn
        p.scale_left, p.scale_right = base


def slip_run(seed, distance_m=3.0, slip_prob=0.05, slip_loss=0.5):
    sim = Simulator(World(10.0, 4.0), pose=(1.0, 2.0, 0.0))
    sim.model = SlipModel(sim.params, random.Random(seed), slip_prob, slip_loss, x=1.0, y=2.0, theta=0.0)
    odo = OdometryTracker(RobotParams())
    sim.set_duty(0.5, 0.5)
    while odo.x < distance_m:
        sim.advance(0.02)
        odo.update(*sim.ticks())
    sim.set_duty(0.0, 0.0)
    for _ in range(50):
        sim.advance(0.02)
        odo.update(*sim.ticks())
    x, y, theta = sim.pose()
    return odo.x, x - 1.0, y - 2.0, theta


runs = [slip_run(seed) for seed in range(200)]
n = len(runs)
odo_d = sum(r[0] for r in runs) / n
true_d = sum(math.hypot(r[1], r[2]) for r in runs) / n
heads = [r[3] for r in runs]
mean_h = sum(heads) / n
std_h = math.sqrt(sum((h - mean_h) ** 2 for h in heads) / (n - 1))
rms_y = math.sqrt(sum(r[2] ** 2 for r in runs) / n)

p, loss, step_m, track = 0.05, 0.5, 0.2 * 0.005, 0.150
theory_ratio = 1 - p * loss
theory_std = math.sqrt(3.0 * step_m * (loss / track) ** 2 * 2 * p * (1 - p))     # sum of d^2 = D x d0, d0 = 1 mm per step
print("slip: 5 % of wheel-steps lose 50 % of their distance, 200 runs of 'drive 3 m by odometry'")
print("odometry distance %.3f m, true distance %.3f m, ratio %.4f (theory 1 - p x loss = %.4f)" % (odo_d, true_d, true_d / odo_d, theory_ratio))
print("heading error: mean %+.2f deg (theory 0), std %.2f deg (theory %.2f deg)" % (math.degrees(mean_h), math.degrees(std_h), math.degrees(theory_std)))
print("sideways error: rms %.1f cm" % (100 * rms_y))
END CODE
OUTPUT:
slip: 5 % of wheel-steps lose 50 % of their distance, 200 runs of 'drive 3 m by odometry'
odometry distance 3.020 m, true distance 2.944 m, ratio 0.9750 (theory 1 - p x loss = 0.9750)
heading error: mean -0.02 deg (theory 0), std 3.17 deg (theory 3.22 deg)
sideways error: rms 9.2 cm
END OUTPUT

Reading the output: the true distance is 0.9750 of the odometry distance, the predicted figure; the heading error has a mean of -0.02 degrees (zero within the noise of 200 runs: the standard error of the mean is 3.2 / sqrt(200) = 0.23 degrees) and a standard deviation of 3.17 degrees against the 3.22 predicted (a standard deviation estimated from 200 runs has an error of about 5 %, so 3.17 is well inside it). The sideways error, which grows as the heading wanders, is 9.2 cm rms after 3 m.

Check on method: slip is a RANDOM error. Compare it with the 1 % tyre of the chapter, which gave exactly the same drift every run (0.30 m and 11.5 degrees): a systematic error can be calibrated away, as Robotics Fundamentals 4 section 6 says, but this one cannot; only a sensor that looks at the world, or the heading from a gyro, can correct it. Two traps in the code: (1) the scale must be put back after the step (the model keeps its true radius error), and (2) the draw must come from the model's own random.Random, not from the global random module, or the runs stop being repeatable. The theory uses the total distance, so it also fails if the loop is cut short: the run length (D) is part of the answer.