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

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: Kalman filters explained

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.
The program's parts: calibrate the gyro once, then each tick a filter predicts from flow and the turn rate, corrects from tag 9, and a controller drives from the estimate1. calibrategyro bias, 5 sturn rateimu()[1]speedflow()tag 9apriltags()subtract2. the filterpredictx, y, hp += Qcorrectwhen in viewestimatex, y, hp3. controlvector to thetarget 105, 105drive()4. plotsplot()
The four parts in the order the list gives them. The calibration happens once, before the robot moves. Everything else runs every tick: the filter keeps the estimate, the controller only ever reads the estimate, never the truth, and the plots let you tell afterwards which of the two went wrong.

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. In a test run that drove up the line and then across, but kept taking fixes on the way across, the robot finished 44 cm further up the mat than its estimate said, and missed the target.

Tag 9 straight up the mat. On its line the range is the gap up the mat; off to the side the range is longer, and taking it as the gap puts the robot too far downtag 9startrange 95= the gapgap 50range 70.7what the fix measurestag 9: 155 cm up the matfrom the startreally at y = 105155 − 70.7 = 84.320.7 cm too low
Two places the camera can read tag 9. Straight below it, the range is the gap up the mat. At 50 cm to the side, the range is the slanting line, 70.7 cm against a gap of 50. A fix that treats it as the gap measures y as 84.3, 20.7 cm too low, and every such fix pulls the estimate down the mat towards it.

Note the order of blame in point 4 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

This is only the skeleton: it dead reckons, with no gyro calibration and no tag fix, and p grows but is never used. On this short run it still arrives. It prints 102.6 and 100.7, and the simulator's truth is 105.7 and 95.7, so the estimate is about 6 cm out. A full program, with the calibration and a gated fix on a first leg up the tag's line, finished less than 2 cm from where it thought it was.

The skeleton's run on the mat, and a close-up of where it stopped: the path it believed it drove, the path it really drove, and the targettarget zonestarttarget 105, 105estimate102.6, 100.7truth105.7, 95.7close-up of the end
The solid line is the path the skeleton believed it drove, the dashed one where it really went. It stopped when its estimate came within 5 cm of the target (the green circle in the close-up), and ended 5.9 cm from where it thought it was. Plotting the two together like this is how you tell a filter fault from a controller fault.

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.