Robotics Fundamentals 1, Chapter 6: Challenge 3 solution (dead reckoning, frames, wrapping, GPS and feedback)

Formulas: x(world) = x + x_r cos(theta) - y_r sin(theta) ;  y(world) = y + x_r sin(theta) + y_r cos(theta)
          wrap(a) = a - 360 floor((a + 180) / 360) in degrees, or atan2(sin a, cos a) ;  angle mean = atan2(sum sin, sum cos)
          sideways error = s sin(e) ;  gyro error = b t
          1 degree of latitude = 111.32 km ;  1 degree of longitude = 111.32 cos(latitude) km

(a) theta = -30 degrees: cos = 0.8660, sin = -0.5000.  (x_r, y_r) = (2.0, 1.0).
    x(world) = 3 + 2.0 x 0.8660 - 1.0 x (-0.5000) = 3 + 1.7321 + 0.5000 = 5.232 m
    y(world) = 2 + 2.0 x (-0.5000) + 1.0 x 0.8660 = 2 - 1.0000 + 0.8660 = 1.866 m
    The landmark is at (5.23, 1.87) in the world frame.

(b) Wrapping to -180 up to +180 degrees:
        200  ->  200 - 360 = -160
        -270 ->  -270 + 360 = 90
        450  ->  450 - 360 = 90
    4.0 radians:  4.0 - 2 pi = 4.0 - 6.2832 = -2.2832 radians (which is -130.8 degrees).
    Facing -175, target +170:
        Naive error = target - current = 170 - (-175) = 345 degrees (a turn nearly all the way round).
        Wrapped error = wrap(345) = 345 - 360 = -15 degrees.
        A negative error is clockwise (counter-clockwise is positive), so the robot should turn 15 degrees CLOCKWISE (through 180), not 345 counter-clockwise.
    Mean of 355, 5 and 10 degrees:
        sin sum = -0.08716 + 0.08716 + 0.17365 = 0.17365;   cos sum = 0.99619 + 0.99619 + 0.98481 = 2.97720
        mean = atan2(0.17365, 2.97720) = 3.34 degrees  (as expected: the three are 355 = -5, 5 and 10, whose mean is 10/3 = 3.33).
        The wrong naive mean is (355 + 5 + 10) / 3 = 123.3 degrees, pointing the wrong way altogether.

(c) The two wheel-slip turns: 2 turns x 3 degrees = 6 degrees of error in the wheel heading, whatever the time taken.
    The gyro error is 0.05 x t.
        60 s mission:   wheels 6 degrees,   gyro 0.05 x 60 = 3 degrees   -> the gyro is better.
        600 s mission:  wheels 6 degrees,   gyro 0.05 x 600 = 30 degrees -> the wheels are better (the gyro drifted all the while the robot sat or drove straight).
    The errors match at t = 6 / 0.05 = 120 s.
    Sideways error on a final 4 m run:
        6 degrees:   4 x sin 6 = 4 x 0.10453 = 0.418 m.
        30 degrees:  4 x sin 30 = 4 x 0.5000 = 2.00 m.
    Neither sensor alone is best for every mission length, which is why the chapter fuses them (and uses absolute references to reset what remains).

(d) Latitude changes by 0.00003 degrees:  0.00003 x 111.32 km = 0.00003 x 111320 m = 3.34 m north.
    Longitude changes by -2.99995 - (-3.00000) = +0.00005 degrees (east).  One degree of longitude at 55 degrees north = 111.32 x cos 55 = 111.32 x 0.57358 = 63.85 km.
        0.00005 x 63850 m = 3.19 m east.
    Distance = sqrt(3.34^2 + 3.19^2) = sqrt(11.16 + 10.19) = sqrt(21.35) = 4.62 m.
    Bearing (clockwise from north) = atan2(east, north) = atan2(3.19, 3.34) = 43.7 degrees.
    At 1.5 m/s and 5 Hz the robot moves 1.5 / 5 = 0.30 m between fixes.
    A 3 m accuracy cannot suit a 0.6 m wide corridor: the position error is five times the width of the corridor, so the fix cannot even say which side of the corridor the robot is on.
    A better source (RTK to a centimetre or two, or landmarks and odometry) is needed there; indoors there is no GPS signal anyway.

(e) Open loop:  distance = 0.255 x 2.0 = 0.51 m.  Error = 0.60 - 0.51 = 0.09 m, which is 15 % short.
    Closed loop:  counts for 0.6 m = 0.6 / 0.0001547 = 3878 counts (3878.4).
    The run ends when the count reaches 3878, and takes 0.6 / 0.255 = 2.35 s at the sagged speed (instead of 2.0 s).
    The final error is about one count, 0.15 mm, plus any slip. The battery voltage no longer changes the distance, only the time taken.
    Feedback has traded the accuracy of the distance for the (harmless) variation of the time.