Project: navigate on the estimate

Reach a target the robot cannot see, using nothing but the filter you wrote.

U6.7State estimationUniversity45 min

Do this lesson in the simulator

The test of an estimator is not how close its number is. It is whether you can steer by it.

The target in this task is a zone the robot cannot see: no line to it, no tag on it, nothing to home in on. The only way there is to know where you are, which means your filter, and to know where the target is, which means the frame your filter is keeping.

What the program contains

  1. Calibrate the gyro. Five seconds, as in U3.4.
  2. A filter that runs every tick: predict with flow() and the calibrated turn rate, carry a variance, correct from tag 9 when it is in view.
  3. A controller on top of the estimate: the vector to the target in your estimated frame, through the inverse kinematics from U2.
  4. Plots, so that when it misses you can see whether the estimator or the controller was at fault.

One trap is worth stating before you meet it. Tag 9 sits straight up the mat from the start, and its fourth number is a range, not a y coordinate. While the robot is on the tag's line the two are the same thing; as soon as it moves sideways they are not, and a fix taken there will drag the estimate somewhere it has never been. Either gate the fix to when your estimated x is small, or drive the first leg up the tag's line and turn for the target afterwards.

Note the order of blame in the paragraph above. When a run like this fails, the first question is whether the robot went where it thought it was going. Plot the estimate against the truth afterwards and the answer is immediate: if the estimate tracked and the robot still missed, the controller is wrong; if the estimate drifted, the filter is.

The structure

from bugbot import *
import math
connect()

DT = 0.1
V_MAX, V_LAT = 20.0, 15.0
TX, TY = 105.0, 105.0
x = y = h = 0.0
p = 4.0

def step(n=1):
    global x, y, h, p
    for i in range(n):
        vx, vy = flow()
        rate = imu()[1]
        a = math.radians(h + 0.5 * rate * DT)
        x += (vx * math.cos(a) + vy * math.sin(a)) * DT
        y += (-vx * math.sin(a) + vy * math.cos(a)) * DT
        h += rate * DT
        p += 0.6
        plot("x", x)
        plot("y", y)
        wait(DT)

for tick in range(200):
    dx, dy = TX - x, TY - y
    gap = math.hypot(dx, dy)
    if gap < 5:
        break
    speed = min(13.0, 0.6 * gap)
    wx, wy = speed * dx / gap, speed * dy / gap
    a = math.radians(h)
    drive(100 * (wx * math.sin(a) + wy * math.cos(a)) / V_MAX,
          100 * (wx * math.cos(a) - wy * math.sin(a)) / V_LAT, 0)
    step()
stop()
print("my x:", round(x, 1), "my y:", round(y, 1))

Run this in the simulator

Task: navigate on the estimate

Reach the green target, which is 105 across and 105 up from the start, using only your own estimate. Print my x: and my y: at the end, within 12 cm of the truth. position() is not allowed anywhere.

from bugbot import *
import math
connect()

DT = 0.1
TX, TY = 105.0, 105.0

Challenges

  1. Run it with the tag fix removed. How much worse is it?
  2. Plot your estimate and odometry() together. Is your filter beating the built-in dead reckoning?
  3. Make the robot drive a detour past the tag before turning for the target. Is a longer route with a fix better than a short route without one?

What comes next

This filter has one belief about where the robot is. U7 asks what happens when it has several, which is what you need when the robot might be in one of two places and the measurement cannot yet tell which. That is the particle filter, and it is how a robot works out where it is when it does not already roughly know.