Answers

U3.2 Write your own odometry

Odometry and drift · University · about 30 min

What this lesson is about

flow() and imu() in, a pose out, in about ten lines. Then check it against the truth.

• The update
• Which heading to rotate by
• The whole thing
• Why not use the heading straight from the IMU

Questions 6 marks in all

1. [1 mark]One odometry update, with a deliberately long 1 s step so the numbers are easy. The robot starts at heading 0, reads (vx, vy) = (0, 20) and turns at 60 deg/s. What does this print?

   import math
   DT = 1.0
   x = y = h = 0.0
   vx, vy, rate = 0.0, 20.0, 60.0
   h_mid = h + 0.5 * rate * DT
   a = math.radians(h_mid)
   x += (vx * math.cos(a) + vy * math.sin(a)) * DT
   y += (-vx * math.sin(a) + vy * math.cos(a)) * DT
   h += rate * DT
   print(round(h_mid, 1), round(x, 2), round(y, 2), round(h, 1))

   Answer:

   30.0 10.0 17.32 60.0

   The midpoint heading is 0 + 0.5 × 60 × 1 = 30. The 20 cm step is rotated by 30 degrees: x = 20 sin 30 = 10.0, y = 20 cos 30 = 17.32. The heading then moves on to 60.

2. [1 mark]Why does the odometry update rotate flow() by the heading before adding it to x and y?

   1. Aflow() measures in the body frame, and the state is kept in the world frame
   2. Bflow() measures in the world frame, and the state is kept in the body frame
   3. CIt corrects for the gyro bias
   4. DIt converts centimetres per second into centimetres
   Answer: A. The velocity arrives as right and forward, the position is kept as world x and y, and the rotation matrix from U2.2 connects them. Multiplying by dt is what turns a speed into a distance.

3. [1 mark]Why is rotating by the heading at the start of the step, rather than the midpoint heading, a problem worth fixing?

   1. AWhile turning it leans the same way every step, so it is a bias that accumulates all run
   2. BIt adds noise that averages out, so it only matters on short runs
   3. CIt makes the heading itself wrong, not the position
   4. DIt is only a problem when the robot is driving straight
   Answer: A. The error is small each step but always in the same direction, which makes it bias rather than noise. The midpoint heading removes most of it at no cost.

4. [1 mark]The loop step is 0.1 s and the robot turns at 60 deg/s. By how many degrees does the heading change within one step, which is the gap between turning first and moving first?

   Answer: 6. 60 × 0.1 = 6 degrees. The midpoint heading splits that difference, using 3 degrees past the starting heading.

5. [1 mark]How do the fused heading imu()[0] and the integrated gyro rate differ?

   1. AThe fused heading is noisy and wanders slowly but has no growing error; the integrated rate is smooth over a second but drifts without limit
   2. BThe integrated rate is noisy but never drifts; the fused heading is smooth but drifts without limit
   3. CBoth drift without limit at the same rate
   4. DThe fused heading is exact, so integrating the rate is never needed
   Answer: A. They fail differently, so each is better over a different timescale. Combining them, weighted by trust, is the complementary filter in U6.1.

6. [1 mark]In the task, why must the turn be made with drive() inside your loop rather than with turn_right(angle=90)?

   1. AA blocking turn runs the world without running your loop, so the turn never reaches your estimate
   2. Bturn_right() resets the gyro, which clears your heading
   3. Cturn_right() uses position() internally, which the task forbids
   4. Dturn_right() turns too fast for flow() to measure
   Answer: A. Odometry only knows what happened on the ticks it ran. A blocking call hides a whole corner from it.

The task: your own odometry

from bugbot import *
import math
connect()

DT = 0.1
x = y = h = 0.0

from bugbot import *
import math
connect()

DT = 0.1
x = y = h = 0.0

def step(n=1):
    """n odometry updates, one per tick, whatever the robot is doing."""
    global x, y, h
    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
        wait(DT)

forward(75)
step(45)
stop()
step(4)

drive(0, 0, 55)
while h < 88:
    step()
stop()
step(4)

forward(75)
step(30)
stop()
step(4)

print("my x:", round(x, 1))
print("my y:", round(y, 1))