Position, Orientation and Feedback

Robotics Fundamentals

Course 1 · Chapter 6 · Position, Orientation and Feedback

Robotics Fundamentals 5 was about the sensors that look outward: distance, line, light and camera. This chapter is about the sensors that look inward and answer three questions every moving robot has to ask. How far have my wheels turned, and how fast? Which way am I facing, and am I tilted? And so, where am I? The answers are the robot's pose (its position and heading), and they are the raw material for everything in Robotics Fundamentals 7 onwards: a robot cannot correct an error it cannot measure. We start with encoders, which count wheel turns (the odometry of Robotics Fundamentals 4 depends on them), then the inertial measurement unit (gyroscope, accelerometer and compass), then the central trick of the chapter: no single sensor is trustworthy, but two sensors with opposite weaknesses can be combined into one good one (sensor fusion). We then use that to improve dead reckoning, see why GPS is not the answer indoors, tidy up reference frames and angle wrapping, and finish with why all of this matters: feedback.

Sensor specifications in this chapter (counts per revolution, scale factors, noise densities, GPS accuracy, update rates, magnetic field strengths) are typical or approximate values from memory, for the low-cost parts used on small robots. They vary widely between models, so read the datasheet of the part you buy. The formulas (resolution, quantisation, integration, atan2 tilt, the complementary filter and its time constant, the frame rotation, angle wrapping) are standard and exact for the idealised models described.

1. Pose, and two ways of knowing it

On a flat floor a robot's pose has three numbers: (x, y, θ), a position and a heading, as in Robotics Fundamentals 4. A flying or tilting robot adds height, pitch and roll. There are two families of sensor for finding it, and the difference between them runs through the whole chapter.

	Relative (incremental) sensing	Absolute sensing
What it tells you	How much the pose has changed since the last reading	Where you are, or which way you face, now, against something fixed
Examples	Incremental encoders, gyroscope, wheel odometry	Compass, GPS, absolute encoder, potentiometer, a wall or beacon at a known place
Error behaviour	Small, smooth readings, but the small errors are added up and the total drifts without limit	Each reading has its own error, which does not accumulate, but readings are noisier, slower or sometimes unavailable
Best at	Fast changes, short times	Slow changes, long times

The two columns are complementary. Relative sensors are good in the short term and bad in the long term; absolute sensors are the reverse. Nearly every technique in this chapter is a way of using an absolute reference to rein in the drift of a relative one.

2. Incremental (quadrature) encoders

A wheel encoder is attached to a motor or wheel shaft and produces a train of pulses as the shaft turns. A magnetic encoder has a small ring magnet on the shaft and two Hall sensors beside it; an optical encoder has a slotted disc passing between a light source and a detector. A single pulse train tells you how far the shaft has turned but not which way. The fix is to use two sensors placed so that their signals are a quarter of a cycle (90 degrees) apart. These are the A and B channels, and the arrangement is called quadrature.

 Forward (A leads B). Each state below lasts 5 characters.  State is written AB.

 state  00   10   11   01   00   10   11   01
 A      _____----------__________----------_____
 B      __________----------__________----------
 count       +1   +1   +1   +1   +1   +1   +1       (each change of A or B is one count)

 Reverse (B leads A): the same four states in the opposite order, 00 01 11 10 00 ...  and every change is -1.

 Four states per cycle, always in this order, so:   forward 00 -> 10 -> 11 -> 01 -> 00       reverse 00 -> 01 -> 11 -> 10 -> 00
 A step to the same state is 0;  a jump of two (00 to 11, or 01 to 10) means a state was MISSED: the count is now uncertain.

Counting every edge: 1x, 2x and 4x decoding

One full cycle of A has two edges, and B adds two more, so a cycle has four transitions. The CPR (cycles, or lines, per revolution) printed in a datasheet counts cycles of one channel. You choose how much of the signal to use: counting only the rising edge of A gives 1× (one count per cycle); counting both edges of A gives 2×; counting every edge of both A and B gives 4×, which is four times the resolution from the same encoder and is the usual choice. Watch the vocabulary: some datasheets quote “pulses per revolution” (PPR, one channel), others quote counts after 4× decoding. Always check which, because it changes every number below by a factor of four. In this chapter CPR means cycles per revolution of one channel, and the counts per revolution are 4 × CPR.

 Counts per revolution (shaft where the encoder sits):    4 x CPR
 If the encoder is on the motor, per revolution of the WHEEL:    counts = 4 x CPR x gear ratio
 Angular resolution of the wheel:    360 / (CPR x 4 x gear)  degrees per count          Distance per count:    pi D / (CPR x 4 x gear)     (D = wheel diameter)

 Worked example: a small geared motor with a magnetic encoder, CPR = 11 (typical), gear ratio 30:1, wheel D = 65 mm.
        Counts per motor revolution:   4 x 11 = 44                      Counts per wheel revolution:   44 x 30 = 1320
        Angular resolution:            360 / 1320 = 0.273 degrees per count
        Wheel circumference:           pi x 65 = 204.2 mm              Distance per count:   204.2 / 1320 = 0.1547 mm

 Fast, but how fast?   Wheel at 0.75 m/s (near the top speed of this motor, illustrative):    counts per second = 0.75 / 0.0001547 = 4848
        The motor shaft turns 30 times as fast as the wheel:   0.75 / 0.2042 = 3.67 wheel rev/s = 220 rpm,   motor 6600 rpm = 110 rev/s,   110 x 44 = 4840 counts per second  (the same, apart from rounding)

Because the encoder sits on the fast motor shaft, the gearbox multiplies the resolution for free: a fraction of a millimetre per count is typical, much finer than the sloppiness of the wheel and the floor. The price is the count rate: 4848 changes a second, each of which must be seen. A program that checks the pins in a loop (polling) will miss some, and a missed state breaks the count. The remedy is an interrupt: the hardware stops the main program the instant a pin changes, runs a very short routine, and returns. Interrupts are covered in Basic Electronics 10, together with the rules for writing a short interrupt routine; many microcontrollers also have a hardware counter that decodes quadrature with no software at all, and a dedicated decoder chip is another option.

Decoding in pseudocode

The decoder keeps the last state and looks up the move from the old state to the new one in a table. This is hardware-neutral Python-style pseudocode; the interrupt wiring is left to Programming for Robotics.

 # state = (A << 1) | B, so 00 = 0, 01 = 1, 10 = 2, 11 = 3.   Row = old state, column = new state.
 STEP = [  0, -1, +1,  0,       # old 00:  to 01 is -1 (reverse), to 10 is +1 (forward), to 11 is a missed state
          +1,  0,  0, -1,       # old 01
          -1,  0,  0, +1,       # old 10
           0, +1, -1,  0 ]      # old 11

 count = 0
 state = 0

 def on_edge(a, b):             # called by the interrupt whenever A or B changes
     global count, state
     new = (a << 1) | b
     count += STEP[(state << 2) | new]
     state = new

 def wheel_speed(count_now, count_before, dt, d_count):     # d_count = distance per count, metres
     return (count_now - count_before) * d_count / dt        # metres per second, negative when reversing

Speed from counts, and the quantisation error

Speed is the change in count over a time interval, v = Δcount × d_count / Δt, and it is the number the control loop of Robotics Fundamentals 7 will use. But counts are whole numbers. In one interval the encoder can only report 38 or 39 counts, never 38.8. Over a window of N counts the reading can be wrong by about one count, so the relative error is about 1 / N. This is quantisation error, and it is worst at low speed and over short intervals.

 Counts in one interval:   N = v dt / d_count          Speed resolution (one count):   v resolution = d_count / dt          Relative error about   1 / N

 Same encoder as above (d_count = 0.1547 mm = 0.0001547 m).
        v = 0.30 m/s,  interval 20 ms:     N = 0.30 x 0.020 / 0.0001547 = 38.8 counts       one count = 0.0077 m/s       error about 2.6 %
        v = 0.30 m/s,  interval 100 ms:    N = 193.9 counts                                  one count = 0.0015 m/s       error about 0.5 %
        v = 0.02 m/s,  interval 20 ms:     N = 2.6 counts                                    one count = 0.0077 m/s       error about 39 %   (the same 0.0077 m/s, but a huge fraction of a slow speed)

 The resolution of the speed reading, d_count / dt = 0.0001547 / 0.020 = 0.0077 m/s, does not depend on how fast the robot goes, only on the interval.
 The trade is the same as for every filter in Robotics Fundamentals 5:  a longer interval gives a finer speed but a more delayed one (here 100 ms of lag), which a control loop dislikes.
 At very low speeds, time the gap BETWEEN counts instead (speed = d_count / gap): the gap is long and can be measured finely, so the error stays small.

Two ways to get speed

Count over a fixed time (above) is simple and is best at high speed. Time between counts gives a good answer at low speed but is noisy at high speed, where the gaps are tiny, and gives nothing when the wheel is stopped. Some robots use the first at high speed and switch to the second at low speed, or smooth either with the exponential filter of Robotics Fundamentals 5.

What encoders cannot tell you

Where the wheel is, not where the robot is. The encoder counts shaft turns. If the wheel slips on the floor, or lifts, or the gearbox has backlash, the count says one thing and the ground says another (Robotics Fundamentals 4 section 6).
The starting position. An incremental encoder starts at zero whenever the power comes on. It counts changes, so it cannot know where the shaft was. That is the job of the absolute sensors below.
Missed counts. A noisy line, a slow interrupt or a long delay with interrupts switched off drops counts, and the count is wrong from then on, silently. A dropped state shows up as a jump of two in the table above.
Index pulse. Many encoders add a third channel, Z (index), that pulses once per revolution, which gives an absolute reference once per turn.

3. Absolute encoders and potentiometers

An absolute encoder reports the shaft angle directly, and still knows it after the power is cycled. A disc carries several concentric tracks of coded patterns, read together, so each position has a unique code; n tracks give 2ⁿ positions in one turn. Codes are usually Gray code, in which only one bit changes between neighbouring positions, so a reading taken exactly at a boundary is at worst one position out, not wildly wrong. Many low-cost absolute encoders are magnetic and send their number over a digital bus. They are used at joints (Robotics Fundamentals 9), where the arm must know its angle at switch-on without searching for a home position.

The simplest absolute angle sensor is the potentiometer (Basic Electronics 3): a resistive track and a wiper on the shaft form a voltage divider whose output is proportional to angle, read by an ADC (Basic Electronics 7 and 10). Its limits are the limited travel (typically 270 or 300 degrees, approximate), the dead zone at the end of the track, wear, and the noise of a sliding contact. Hobby servos use one internally to close their own position loop.

 Absolute encoder:   resolution = 360 / 2^n degrees          12 bit:  360 / 4096 = 0.0879 degrees         14 bit:  360 / 16384 = 0.0220 degrees
 Potentiometer on an n-bit ADC over a travel of T degrees:   resolution = T / 2^n          T = 270 degrees, 10 bit:   270 / 1024 = 0.264 degrees per count (before noise)

 Incremental versus absolute at a joint:   the incremental encoder on a geared motor above has 0.273 degrees per WHEEL count, and starts from zero at every power-up;
        the 12-bit absolute encoder is a little finer than the potentiometer, needs no home position, but is costlier. Often the answer is both: an absolute sensor for the
        starting angle and the fine incremental count for the changes.

4. The inertial measurement unit (IMU)

An IMU is a small chip (usually mounted on a breakout board and read over a serial bus such as I2C or SPI, mentioned in Basic Electronics 10) that combines several motion sensors, normally a three-axis gyroscope, a three-axis accelerometer, and sometimes a three-axis magnetometer (compass). Each measures a different thing, each is flawed in a different way, and the flaws are what the next section exploits. The sensors are tiny vibrating or flexing structures (MEMS) whose motion is converted to a voltage, then digitised inside the chip, so you receive integers and a scale factor that converts them to units.

Gyroscope: rate of turn

A gyroscope measures angular rate: how fast the robot is turning, in degrees per second (or radians per second), about each axis. It does not measure angle. To get the heading you integrate: add up the rate multiplied by the time step.

 Rate from the raw integer:    rate = raw / sensitivity          (sensitivity in LSB per degree per second, set by the range you select)
 Typical: a 16-bit gyro on the +-250 degree/s range:   sensitivity = 32768 / 250 = 131 LSB per (degree/s)       (the +-500 range gives 65.5, the +-2000 range 16.4)
 A raw reading of 262 means 262 / 131 = 2.0 degrees per second.

 Integration:   angle(new) = angle(old) + rate x dt          (dt = the true time between samples; use the measured time, not the intended one)

 Worked example 1, perfect gyro:  turning at 45 degrees/s, 100 samples a second (dt = 0.01 s) for 2 s.   200 steps of 0.45 degrees = 90.0 degrees.
 Worked example 2, wrong dt:  the program assumes dt = 0.0100 s but the loop really runs every 0.0105 s.   Each sample still sees the same rate, but the real time is 5 % longer,
        so the true turn is 94.5 degrees while the program reports 90.0: a 5 % scale error. Use a timer to measure dt, or run the gyro's own sample clock.

Drift from bias

A real gyro never reads exactly zero when still. It has a bias (zero-rate offset): a small constant rate that it reports even when stationary. It also has random noise on every sample. Integrating noise gives a slow random wander, but integrating a bias gives a steady run-away, because a constant added each step makes a straight-line growth in the angle.

 Heading error from a bias b after a time t:    error = b t

 Worked example: a gyro with a bias of 0.5 degrees/s (typical for a low-cost part straight from power-up, approximate), robot standing still.
        After  1 minute:   0.5 x 60 = 30 degrees          After 1 hour:   0.5 x 3600 = 1800 degrees (five whole turns)
 A robot that thinks it has turned 30 degrees after sitting still for a minute is useless for navigation.

 Cure 1: measure the bias while the robot is stationary at start-up (say 200 samples, 2 s at 100 Hz), average them, and subtract it from every later reading.
        The noise averages down as 1 over the square root of N (Robotics Fundamentals 5): with a noise of 0.05 degrees/s per sample, the bias is known to 0.05 / sqrt(200) = 0.0035 degrees/s.
        The residual 0.0035 degrees/s then drifts only 0.0035 x 60 = 0.2 degrees in a minute.
 Cure 2: the bias is not constant. It moves with temperature and over minutes (the “bias instability” of the datasheet), so any single calibration slowly goes out of date.

 The random-noise part is small by comparison:   per-sample noise 0.05 degrees/s at 100 Hz for 60 s (6000 samples) wanders by 0.05 x 0.01 x sqrt(6000) = 0.039 degrees.
 Bias dominates; the noise only adds a little uncertainty. After calibration the fault that remains is the slow bias wander, and that is what the compass or accelerometer must correct.

Accelerometer: tilt from gravity

An accelerometer measures acceleration along each axis, in units of g (one g is 9.81 m/s²) or m/s². Importantly, it cannot tell gravity from motion: a robot standing still reads one g, straight up, and that reading is exactly what lets us find the tilt. Tipping the robot moves part of gravity from one axis to another. With the sensor flat, a_z reads 1 g and the others read 0; tilted, the ratio of the axes gives the angle. Use atan2, not a plain arctangent, because atan2 takes both numbers separately, gives the correct quadrant (a full range of angle instead of just −90 to +90 degrees) and does not divide by zero.

 Tilt (roll about the x axis, pitch about the y axis; signs depend on how the chip is mounted):
        roll  = atan2( ay, az )            pitch = atan2( ax, sqrt( ay^2 + az^2 ) )
 Magnitude check:   |a| = sqrt( ax^2 + ay^2 + az^2 )   should be 1.00 g if the robot is not accelerating

 Worked example (robot standing still, nose tilted 15 degrees):    ax = sin 15 = 0.2588 g,    az = cos 15 = 0.9659 g,    ay = 0
        pitch = atan2( 0.2588, 0.9659 ) = 15.0 degrees        |a| = sqrt( 0.0670 + 0.9330 ) = 1.000 g   (consistent)

 Now the robot accelerates forward at 0.3 g (a hard start), still tilted 15 degrees:    ax = 0.2588 + 0.3 = 0.5588 g,   az = 0.9659 g
        pitch = atan2( 0.5588, 0.9659 ) = 30.1 degrees          error 15.1 degrees          |a| = sqrt( 0.3123 + 0.9330 ) = 1.116 g   (11.6 % above 1 g: a warning)

 What the accelerometer cannot do:  it cannot see rotation about the vertical axis (turning flat on the floor changes none of the three readings), so it can give roll and pitch but never heading.

The accelerometer is therefore accurate on average and over the long term (gravity does not drift) but wrong during motion, whenever the robot accelerates, brakes, turns a corner or bumps over a floor joint, and noisy from motor vibration. A simple safeguard is to trust the tilt only when the magnitude is close to 1 g, and ignore it otherwise; but as the example shows, a push below that threshold still slips through, so a better answer is needed. It is the exact opposite of the gyroscope: the gyro is smooth and fast but drifts; the accelerometer is noisy and fooled by motion but does not drift.

Magnetometer: the compass

A magnetometer measures the magnetic field along each axis, in microtesla (µT) or gauss (1 gauss = 100 µT). The Earth's field is roughly 25 to 65 µT in total (approximate, depending on where you are), with the horizontal part, the part that points toward magnetic north, typically about 15 to 30 µT in mid-latitudes. With the sensor flat, the heading follows from the two horizontal axes:

 Heading from the two horizontal axes:    heading = atan2( my, mx )      (this is an angle counter-clockwise from the sensor's x axis; the sign and axis conventions
        vary between chips, so check with a test. A compass bearing is measured clockwise from north, which is the other sense.)
 Declination:  magnetic north is not true north; the difference is a few degrees in many places, and you can look it up for your location.
 Tilt:  if the robot is not level, the heading is wrong unless the tilt is measured (accelerometer) and the reading is rotated back to the horizontal (“tilt compensation”).

 Disturbances.  Hard iron: a fixed magnet or a steel part rotating with the robot adds a constant OFFSET to the reading.  Soft iron: nearby iron bends the field and squashes the circle into an ellipse.
        Both are usually calibrated by turning the robot through a full circle and recording the largest and smallest value of each axis:    offset = ( max + min ) / 2

 Worked example (illustrative): a horizontal field of 18 microtesla; the robot faces so that the true reading would be (mx, my) = (18, 0), heading 0 degrees.
        A motor magnet adds a hard-iron offset of (+12, +5):   the sensor reads (30, 5),   heading = atan2( 5, 30 ) = 9.5 degrees,   an error of 9.5 degrees.
        A full turn gives mx from -6 to 30 and my from -13 to 23:   offset = ( (30 + -6) / 2, (23 + -13) / 2 ) = ( 12, 5 )   and subtracting it restores (18, 0) and 0 degrees.

A compass is an absolute heading with no drift, which is its great value, but a robot is a hostile place for one. Motor wires carry currents that make fields of their own (and the field changes with the motor current, so it cannot be calibrated away entirely), steel frames, batteries and the reinforced concrete of a building all bend the field, and the reading can jump near a large metal object. Mount the sensor away from the motors and wires, and treat it as a slow, rough correction rather than a precise one.

Units, scale factors and sample rate

Sensor	Measures	Typical units and scale (approximate)	Typical sample rate	Strong at	Weak at
Gyroscope	Rate of turn	Degrees per second; 131 LSB per degree/s on the ±250 range	100 to 1000 Hz or more	Fast, smooth, short-term heading and tilt change	Bias: drifts without limit
Accelerometer	Acceleration (gravity plus motion)	g; 16384 LSB per g on the ±2 g range	100 to 1000 Hz or more	Long-term tilt (roll and pitch); no drift	Wrong when accelerating; noisy; no heading
Magnetometer	Magnetic field	µT; a fraction of a µT per LSB	10 to 100 Hz	Absolute heading; no drift	Disturbed by motors, steel, wiring; needs calibration

Always read the sample rate and the scale you have set. A rate of 100 Hz means dt = 0.01 s, and the integration above depends on dt being right. Choosing a larger measurement range (±2000 degrees/s instead of ±250) stops a fast spin from saturating the sensor, but it makes each LSB coarser, so pick the smallest range that the robot's real motion will not exceed.

5. Sensor fusion in plain words

Sensor fusion means combining several imperfect measurements into one better estimate. The idea is not about averaging. It is about each sensor covering the other's weakness. The gyro is good at changes (the high-frequency part of the motion) but cannot hold an angle for long; the accelerometer is poor at changes (noisy, fooled by motion) but right about the angle on average (the low-frequency part). The obvious strategy: trust the gyro in the short term and the accelerometer in the long term.

The complementary filter

The simplest way to do that is one line. At each step, integrate the gyro as usual, then pull the result a small fraction of the way toward the angle the accelerometer gives. The gyro term supplies the fast changes; the accelerometer term slowly drags the angle back to the truth and cancels the drift.

 angle(new) = alpha x ( angle(old) + gyro x dt ) + ( 1 - alpha ) x accel_angle              alpha close to 1, such as 0.98 or 0.95
        gyro = the rate in degrees/s, bias already removed        accel_angle = the tilt from atan2 (section 4)

 alpha is the trust in the gyro each step, and ( 1 - alpha ) is the trust in the accelerometer.

 Time constant:    tau = alpha x dt / ( 1 - alpha )     (approximate; the exact value is dt / (-ln alpha))
        Meaning:   changes faster than about tau are taken from the gyro;   errors that last much longer than tau are corrected by the accelerometer.
 A bias b left in the gyro is held to a fixed angle error of  tau x b  instead of growing without limit.   Worked below.

 Worked example: dt = 0.01 s (100 Hz), alpha = 0.98.      tau = 0.98 x 0.01 / 0.02 = 0.49 s
        The robot is standing still at a true tilt of 10 degrees and the accelerometer reads 10 degrees, but the filter starts at 0 and the gyro has a bias of 0.5 degrees/s (so the gyro says
        it is turning at 0.5 degrees/s all the time).
        step k:          1        2        5        10       49       100      200
        angle (deg):   0.205    0.406    0.984    1.874    6.438    8.886    10.065      (computed step by step from the formula)
        The angle climbs toward 10 with a time constant near tau: after 49 steps (0.49 s = one tau) it has covered about 63 % of the gap (6.3 of 10 degrees).
        Final value:   accel_angle + tau x b = 10 + 0.49 x 0.5 = 10.245 degrees.   The gyro bias costs 0.245 degrees instead of 30 degrees a minute.

 Compare with each sensor alone (true angle 10 degrees, still robot):   gyro only: error 30 degrees after one minute and growing;   accelerometer only: right on average here.
 Now add a 0.2 s bump during which the accelerometer is wrong by 15 degrees (the hard-start example):   the filter takes only a fraction ( 1 - alpha^n ) of it:
        after 20 steps (0.2 s)  15 x ( 1 - 0.98^20 ) = 15 x 0.332 = 5.0 degrees of error at worst, instead of 15, and it then decays away with the same time constant.

Here is the filter as hardware-neutral Python-style pseudocode.

 def complementary(angle, gyro_rate, accel_angle, dt, alpha=0.98):
     # gyro_rate: degrees per second, bias already subtracted
     # accel_angle: degrees, from atan2 of the accelerometer axes
     return alpha * (angle + gyro_rate * dt) + (1 - alpha) * accel_angle

 # each loop:  rate = (gyro_raw / 131.0) - gyro_bias
 #             tilt = degrees(atan2(ax, az))
 #             angle = complementary(angle, rate, tilt, dt)

Choosing alpha

A value close to 1 (a long tau) trusts the gyro more: very smooth, but slow to correct drift and slow to recover from a bad start. A smaller alpha (a short tau) corrects faster but lets the noise and the motion errors of the accelerometer through. Pick tau to be a few times longer than the disturbances you want to ignore (a bump lasts a fraction of a second) and a few times shorter than the time the gyro takes to drift by an unacceptable amount. Values of tau between about 0.3 s and 1 s are common for a small robot (a rule of thumb, not a law).

The Kalman filter, conceptually

The Kalman filter is the more powerful relative of the complementary filter. It does the same job, and no matrix algebra is needed to understand what it does. It keeps two things: a best estimate (here, the angle, and also the gyro's bias) and a measure of how uncertain that estimate is. Each step has two halves. In predict, it moves the estimate forward using a model (the gyro rate), and the uncertainty grows, because every prediction adds error. In update, it takes a measurement (the accelerometer angle), compares it with the prediction, and corrects the estimate by a fraction of the difference, where the fraction is the Kalman gain. The gain is worked out each step from the two uncertainties: a trustworthy measurement gets a large gain; a noisy one, or a confident prediction, a small one. That is the improvement over the complementary filter, whose alpha is one fixed number chosen by hand. The Kalman filter changes its trust as conditions change (for example, it can trust the accelerometer less when the readings are noisy), and it can estimate the gyro's bias as a by-product. Its cost is complexity: a model, noise figures to be chosen, and more computation. For a small robot that only needs tilt, the complementary filter is usually enough; the Kalman filter earns its place when you fuse many sensors with different rates and accuracies (wheel odometry, IMU and GPS, for example). It is the foundation of most professional navigation software.

6. Dead reckoning

Dead reckoning means estimating where you are by starting from a known point and adding up your own motion, with no outside reference. Robotics Fundamentals 4 built it from wheel encoders: from the ticks of each wheel, ds = (d_R + d_L) / 2 gives the distance moved and dθ = (d_R − d_L) / L gives the heading change, where L is the track width. That second line is the weak point. It divides a small difference of two large numbers by a small number, so a one-per-cent error in either wheel, or in L, or a slip on one side, becomes a heading error.

The improvement is natural once we have a gyroscope: take the distance from the wheels and the heading from the gyro. The distance is a well-behaved measurement (equal slip on both wheels affects it only moderately), but the heading is the part that suffers from slip and an inexact track width, and a gyro does not care about the floor. The pose update of Robotics Fundamentals 4 is unchanged except that dθ comes from the gyro (rate × dt), or from a fusion of the gyro with the wheels.

Why the position error grows so fast

A heading error is not just an error in the direction; every metre travelled after it carries the robot sideways. If the heading is off by e radians, a straight run of length s ends up s sin e to one side of where it should. If the heading error itself keeps growing as the robot moves, the sideways error grows faster than linearly.

 Sideways error of a straight run of length s with a constant heading error e:    s sin( e )              A heading error that GROWS steadily, e = k s, gives about  k s^2 / 2   (small angles)

 Worked example: the heading error grows by 1 degree (k = 0.01745 rad) for every metre travelled.
        After 10 m:   about k s^2 / 2 = 0.01745 x 100 / 2 = 0.87 m  (exactly (1 - cos(ks)) / k = 0.870 m)         After 20 m:   0.01745 x 400 / 2 = 3.5 m  (exactly 3.45 m)
 Doubling the distance quadruples the position error (3.45 / 0.87 = 4.0 to within 1 %).  Position error cannot be kept small without keeping the heading error small.

 A fixed heading error of 6 degrees on a straight 5 m run:   5 sin 6 degrees = 0.52 m.         A heading error of 24 degrees:   5 sin 24 degrees = 2.03 m.

Worked example: heading from the wheels against the gyro

A robot drives three laps of a 1 m square (12 turns of 90 degrees, 1080 degrees in total), taking 180 s. The real track width is 150 mm, but the program uses 152 mm. The gyro was calibrated at power-up and is left with a residual bias of 0.05 degrees/s.

 Wheels:  d(theta) = (dR - dL) / L(assumed).  The real turn is (dR - dL) / L(true), so the estimate is the true angle times  150 / 152 = 0.98684.
        After the 1080 degrees of real turning, odometry reads 1080 x 0.98684 = 1065.8 degrees:  heading error = 1080 x ( 1 - 0.98684 ) = 14.2 degrees.
        The wheel error is proportional to how much the robot has TURNED (and to the distance, if wheels slip), and does not depend on how long it takes.
 Gyro:    the residual bias gives an error of  b t = 0.05 x 180 = 9.0 degrees after 180 s.    The gyro error is proportional to TIME, and does not depend on how much the robot turns.

 So over this short run the gyro is better (9.0 degrees against 14.2), and the crossover is at   14.2 / 0.05 = 284 s,  a little under five minutes.
 If the same robot spent 30 minutes (1800 s) doing the same 1080 degrees of turning, the gyro error would be 0.05 x 1800 = 90 degrees against the unchanged 14.2 degrees from the wheels.
 Neither is good enough for a long mission. A robot that fuses them can use the wheels to hold the gyro's bias in check whenever it drives straight (the wheels then say the heading
 is not changing, so any rate the gyro reports is its bias), and the gyro to cover the turns where the wheels slip. And an absolute reference (section 8) resets what is left.

7. GPS and its limits

The Global Positioning System (and its relatives, together called GNSS) is the obvious absolute position sensor for a robot outdoors: a receiver listens to satellites and works out its latitude and longitude. For robots the limits matter more than the principle.

Property	Typical value (approximate)	Consequence for a robot
Accuracy, open sky	About 2 to 5 m for a low-cost receiver	Fine for a lawn mower crossing a field; hopeless for a 1 m wide corridor or driving up to a door
Near buildings, trees	Often much worse, with jumps of 10 m or more	Signals bounce off walls (multipath) and arrive late, and the position moves although the robot has not
Indoors, underground	No fix at all	The satellite signals are very weak and do not pass through roofs and walls, so GPS cannot be the sole answer for an indoor robot
Update rate	1 Hz for many cheap modules; 5 to 10 Hz for better ones	At 1 m/s a robot moves 1 m between fixes; at 1 Hz and 10 m/s, 10 m. Far too slow for steering by itself
Heading	Only from motion: the direction between successive fixes	Useless when the robot is stopped or slow (the noise of metres swamps a movement of centimetres)
Start-up	From seconds to a minute or more to find a first fix	The robot cannot navigate at once
RTK GPS	1 to 2 cm, and 5 to 20 Hz, with a nearby base station or correction service	Used for farm and survey machines; costs more, needs a clear sky and a radio or network link to the corrections, and fails indoors like ordinary GPS

A degree of latitude is about 111 km on the ground, so 0.00001 degrees (the last digit most modules print) is about 1.11 m. The north-south and east-west steps are not equal: a degree of longitude shrinks with the cosine of the latitude, so it is about 111.32 × cos 55 = 63.9 km at 55 degrees north. GPS is best used for what it is good at: a slow, absolute, drift-free position, fused with the fast but drifting estimate of wheel odometry and the IMU (this is the standard job for a Kalman filter).

8. Absolute positioning: landmarks and beacons

Dead reckoning drifts, and an absolute fix resets the drift. Indoors, where there is no GPS, a robot gets its absolute fixes from places and objects whose position it already knows.

Landmarks are features of the environment at known places: a wall (an ultrasonic or laser distance reading tells you the distance from it, which corrects the coordinate perpendicular to the wall), a corner, a door frame, a line crossing a marked junction, a coloured mark, or a printed marker that a camera can read together with its position and orientation. Meeting a landmark whose location is on the robot's map lets the robot replace its drifted estimate with a fresh one, at least in that one direction.
Beacons are active devices at known places that the robot measures its distance or direction to: infrared or radio transmitters, ultra-wideband (UWB) anchors, or acoustic pingers. Distances to three beacons fix a position (trilateration: the position is where three circles meet), and the beacons never drift. The costs are the infrastructure to install and the noise in the range readings.

 error
   ^
   |         /|            /|
   |       /  |          /  |
   |     /    |        /    |
   |   /      |      /      |
   | /        |    /        |
   +----------+---+---------+-----> distance travelled
   start     fix 1         fix 2    (start = known pose)

   Between fixes the error grows; each landmark or beacon fix pulls it back down to the fix's own small error.

Worked example: a robot has driven 10 m along a corridor and its odometry has put it 0.3 m too far along. At the end an ultrasonic sensor measures 0.50 m to the end wall where the map says it should be 0.80 m. The wall reading says the robot is 0.30 m further than it thought, so the robot corrects its position along the corridor by 0.30 m. That fixes the along-corridor coordinate only; the sideways coordinate and the heading need a second landmark (a side wall) to correct. Landmarks used this way, together with maps, are the subject of Robotics Fundamentals 8 (localisation).

9. Reference frames and angle wrapping

Every measurement has a frame of reference. Wheel odometry works in the robot frame (x forward, y to the left, origin at the robot), a lidar reading is in the sensor's frame, and the pose of the robot is in the world frame (fixed to the room, or to the starting point). To use a measurement from one frame in another, rotate it by the robot's heading and shift it by its position. A point (x_r, y_r) in the robot frame is, in the world frame:

 x(world) = x + x_r cos( theta ) - y_r sin( theta )               The robot is at (x, y) in the world with heading theta (counter-clockwise from the world x axis).
 y(world) = y + x_r sin( theta ) + y_r cos( theta )

 Worked example: the robot is at (2, 1) facing 90 degrees (along the world y axis).   An obstacle is 1.0 m ahead and 0.5 m to the left:  (x_r, y_r) = (1.0, 0.5).
        x(world) = 2 + 1.0 x cos 90 - 0.5 x sin 90 = 2 + 0 - 0.5 = 1.5 m
        y(world) = 1 + 1.0 x sin 90 + 0.5 x cos 90 = 1 + 1 + 0 = 2.0 m              So the obstacle is at (1.5, 2.0) in the world frame: 1 m ahead of (2, 1) is (2, 2), and to its left is 0.5 m further toward -x.

 The reverse (world point to robot frame): subtract the robot's position, then rotate by minus theta.

Angle wrapping

An angle of 190 degrees and an angle of −170 degrees point the same way, yet to a computer they are numbers 360 apart. If a robot is facing −170 degrees and wants to face 170 degrees, the subtraction target minus heading gives 340 degrees, and a robot that believed it would turn the long way round, almost a full circle, when 20 degrees the other way would do. The cure is to wrap every angle difference into one standard range, usually −π to +π radians (−180 to +180 degrees).

 wrap( a ) = atan2( sin a, cos a )          or          wrap( a ) = a - 2 pi x floor( ( a + pi ) / ( 2 pi ) )           (result in -pi up to +pi; at exactly 180 degrees the sign is a choice)

 Degrees:     190 -> -170      -190 -> 170      540 -> 180      720 -> 0      -350 -> 10      Radians:    4.0 -> 4.0 - 2 pi = -2.283      -4.0 -> +2.283

 Heading error:   target 170, current -170.   Naive:  170 - (-170) = 340 degrees.   Wrapped:  wrap(340) = -20 degrees:  turn 20 degrees clockwise, not 340 counter-clockwise.

 Averaging angles:   the average of 350 and 10 degrees is clearly 0, but ( 350 + 10 ) / 2 = 180, the exact opposite.
        Average the sines and cosines instead:   atan2( sin 350 + sin 10, cos 350 + cos 10 ) = atan2( 0, 1.970 ) = 0 degrees.
 The same trap catches filters:  a complementary filter or exponential filter that blends two headings (a gyro angle near +179 and a compass reading near -179) must blend the
        wrapped DIFFERENCE, as in  heading = heading + ( 1 - alpha ) x wrap( compass - heading ),  and wrap the result.   Blend the raw numbers and the heading swings through 0.

Make a habit of wrapping differences of angles (errors) and the final angle after every update; it is the same wrap that appeared in the pose update of Robotics Fundamentals 4. Tilt angles in the range −90 to +90 usually do not need it, but headings always do.

10. Why feedback matters

Everything in this chapter is a way of measuring what the robot actually did, as opposed to what it was told to do. That difference is the heart of robot control. Compare two ways of driving 0.6 m.

 Open loop (no measurement):    command --> [ motors and wheels ] --> motion         "Run at 0.30 m/s for 2.0 s."   Whatever happens, happens.

 Closed loop (feedback):       target ---(+)---> [ controller ] --> [ motors and wheels ] --+--> motion
                                       ^ - error                                           |
                                       +-------------- [ encoder: counts ] <---------------+          "Run until the encoders have counted 3878."

 Worked example: the battery has sagged, and the wheels turn at 0.255 m/s instead of the nominal 0.30 m/s (a 15 % drop, illustrative).
        Open loop for 2.0 s:   0.255 x 2.0 = 0.51 m instead of 0.60 m.   An error of 0.09 m (15 %), and different every time the battery, floor or load changes.
        Closed loop:   the target of 0.60 m is 0.6 / 0.0001547 = 3878 counts (the encoder of section 2).   The robot keeps driving until the count reaches that, taking 2.35 s instead.
        The final error is about one count, 0.15 mm, plus whatever slip the wheels had; the battery and the floor no longer matter.

A robot with no feedback is relying on every part being exactly as it was at the factory, and the world never is. With feedback the robot is told its own error, and can correct it. Doing that well is harder than it sounds: how hard should it correct, how quickly, and how can it avoid overcorrecting and swinging back and forth? That is the subject of Robotics Fundamentals 7.

Feedback is only as good as the measurement

A control loop that steers by a drifting, delayed or noisy number will faithfully produce drifting, delayed or noisy motion. The filters and calibration of Robotics Fundamentals 5, the interval choice for speed in section 2, and the fusion in section 5 are not optional extras. They decide how good the feedback loop can ever be.

11. Challenges

Challenge 1: Encoders and speed

A robot has a geared motor with a magnetic quadrature encoder of CPR = 16 on the motor shaft, a gearbox of 20:1 and wheels of diameter 80 mm, decoded at 4×. (a) Find the counts per motor revolution, the counts per wheel revolution, the angular resolution of the wheel in degrees per count, and the distance per count in millimetres. (b) The robot runs at 0.40 m/s. Find the counts per second, the counts in one 25 ms interval, the speed represented by one count, and the quantisation error as a percentage. Also find the motor speed in rpm. (c) What interval is needed to keep the quantisation error at or below 1 % at 0.40 m/s? What is the error with the 25 ms interval at 0.05 m/s, and how long would it take to collect 100 counts at that speed? (d) The decoder sees the states 00, 01, 11, 10, 00 in turn (A and B written AB), where forward is 00, 10, 11, 01. Which way is the wheel turning and what is the net count? What does the sequence 00, 11 mean? (e) Find the resolution of a potentiometer with 270 degrees of travel on a 10-bit ADC, and of 12-bit and 14-bit absolute encoders, in degrees. (f) At a no-load motor speed of 9000 rpm, find the number of counts per second, and the share of the processor taken if each interrupt routine takes 10 microseconds.

View solution

Challenge 2: Gyro, accelerometer, compass and the complementary filter

(a) A 16-bit gyro is set to the ±500 degree/s range. Find its sensitivity in LSB per degree/s. Averaged over a stationary period it reads +33; find the bias in degrees/s and the drift in degrees per minute. While the robot turns, the raw reading is 3277: find the rate before and after subtracting the bias, and the angle turned in 3 s at the corrected rate. (b) An accelerometer on the ±2 g range (16384 LSB per g) reads ax = 4096 and az = 15854 (ay = 0). Find ax and az in g, the pitch with atan2, and |a|. The robot then accelerates forward at 0.2 g with the same tilt: find the new ax, the pitch reported, the error, and |a|. Would a gate of 1.00 g ± 0.05 g reject it? (c) A complementary filter has alpha = 0.95 and dt = 0.02 s. Find the time constant. The robot is still at a true tilt of 10 degrees, the accelerometer reads 10 degrees, the filter starts at 0 and the gyro bias reads 0.5 degrees/s. Calculate the filter output for the first five steps and the final value, and estimate the time to cover 95 % of the way. (d) A compass in the horizontal plane reads (mx, my) = (28, 16) microtesla at a true reading of (20, 20). During a full turn the largest and smallest values were mx from −20 to 36 and my from −32 to 24. Find the heading error and the hard-iron offsets, and the corrected heading.

View solution

Challenge 3: Dead reckoning, frames, wrapping, GPS and feedback

(a) A robot is at (3, 2) with heading −30 degrees. A landmark is seen at (2.0, 1.0) in the robot frame. Find its world position. (b) Wrap these angles into the range −180 to +180 degrees: 200, −270, 450. Wrap 4.0 radians into −π to +π. A robot facing −175 degrees must face +170 degrees: find the naive error, the wrapped error, and the way it should turn (counter-clockwise is positive). Find the correct mean of 355, 5 and 10 degrees and the wrong naive mean. (c) A robot turns 90 degrees twice. The wheels read each turn 3 degrees short of the truth because of slip, and the gyro has a residual bias of 0.05 degrees/s. Compare the heading errors from the wheels and from the gyro for a 60 s mission and for a 600 s mission with the same two turns. At what time do the two errors match? Find the sideways error of a final straight run of 4 m in the 6 degree and the 30 degree case. (d) A GPS fix moves from (55.00000, −3.00000) to (55.00003, −2.99995). Taking 111.32 km per degree of latitude and the longitude scale at 55 degrees north, find the northward and eastward displacement in metres, the distance, and the bearing (clockwise from north). A robot at 1.5 m/s with a 5 Hz GPS: how far does it move between fixes? Would a 3 m accuracy suit a 0.6 m wide corridor? (e) Open loop, a robot is commanded to run at 0.30 m/s for 2 s, but the battery has sagged and it moves at 0.255 m/s. Find the distance and the error. With the encoder of the chapter (0.1547 mm per count), how many counts mark 0.6 m, and how long does the closed-loop run take?

View solution

Quick reference

Relative sensors (encoder, gyro, odometry) are smooth but drift; absolute sensors (compass, GPS, absolute encoder, landmark) do not drift but are noisier or unavailable. Fusion uses one to correct the other.
Quadrature: A and B 90 degrees apart; four states per cycle (forward 00, 10, 11, 01); 4× decoding counts every edge. Counts per wheel revolution = 4 × CPR × gear; resolution = 360 / (CPR × 4 × gear); distance per count = πD / (CPR × 4 × gear). Use interrupts (Basic Electronics 10).
Speed: v = Δcount × d_count / Δt; quantisation error about 1 / N counts per interval; a longer interval is finer but more delayed; time the gap at low speed.
Absolute: n-bit encoder 360 / 2ⁿ; potentiometer T / 2ⁿ over travel T; absolute sensors keep their reading through a power cycle.
Gyro: rate = raw / sensitivity; angle += rate × dt; a bias b gives error b t (0.5 degrees/s is 30 degrees a minute); measure the bias at rest and subtract it.
Accelerometer: tilt = atan2 of the axes (pitch = atan2(a_x, √(a_y² + a_z²))); check |a| = 1 g; wrong while accelerating; no heading.
Compass: heading = atan2(m_y, m_x); hard-iron offset = (max + min) / 2; disturbed by motors and steel; needs tilt compensation.
Complementary filter: angle = α (angle + gyro × dt) + (1 − α) accel_angle; τ = α dt / (1 − α); a gyro bias b leaves an error τ b. Kalman filter: predict with a model, update with a measurement, by a gain set from the uncertainties.
Dead reckoning: distance from the wheels, heading from the gyro; sideways error s sin e, and a growing heading error makes position error grow as s². Wheel heading error scales with turning, gyro error with time.
GPS: metres of error, 1 to 10 Hz, no signal indoors, no heading at low speed; RTK gives centimetres with corrections. 0.00001 degrees of latitude is about 1.11 m.
Frames: x(world) = x + x_r cos θ − y_r sin θ, y(world) = y + x_r sin θ + y_r cos θ. Wrap angle differences to −π..+π with atan2(sin a, cos a); average angles with sines and cosines.
Feedback: open loop trusts the commands; closed loop measures the result and corrects the error.

Next, Robotics Fundamentals 7 uses everything measured here to close the loop. Control: making a robot do what you want. It covers open loop against closed loop, the simple on-off (bang-bang) controller, PID control with a worked example, how to tune it, and why a badly tuned robot oscillates, overshoots or becomes unstable, all driven by the encoder speed and gyro heading from this chapter.