Worksheet

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))

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

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

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?

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

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

The task: your own odometry

Drive at least 90 cm with a corner in it, running your own odometry, and print my x: and my y: at the end. position() is not allowed anywhere in this task, not even to print. Turn with drive() inside your loop: a blocking turn_right(angle=90) runs the world without running your loop, so the turn never reaches your estimate.

from bugbot import *
import math
connect()

DT = 0.1
x = y = h = 0.0

Plan your program here, then type it in and press Run.

Do it on the robot
www.bugbotlab.com/learn/u3-2-your-own-odometry/
The simulator checks it and tells you when it passes. Nothing to install, no account.

Challenges

1. Compare your estimate with odometry() after the same run. They should agree closely; if they do not, one of you has a bug.
2. Plot your heading against heading() for a run with three turns in it.
3. Add the trapezium rule to the velocity as well as the midpoint to the heading. Does it help on this drive?