Polaris' IMU

Overview

Polaris is UBC Sailbot's completely autonomous sailboat. It's planned mission is to sail from the coast of Vancouver Island to Alaska and collect oceanic data on the way in collaboration with research groups at UBC.

I was in charge of designing and implementing a solution that allowed for precise heading (or yaw) measurement that fit within our budget constraints.

Using an off the shelf 9-dof MEMS IMU, and fusing it with the data from a dual antenna GNSS kit. With this technique we achieved +/-0.15 degree heading accuracy.

The heading accuracy is so critical as the power our sail receives is very sensitive to the angle at which wind hits the sail.

Created a PCB, CI Pipeline, and boat simulator to de-risk as much of the filter as possible before getting it on the boat experiencing real world failures.

I focused deeply on:

Motivation

Sailbot's autonomous boat Polaris is a research vessel designed to sail for months completely autonomously.

Polaris in the water

To achieve this goal, the boat needs to be able to monitor it's current state very accurately. A cheap, off-the-shelf, hobby IMU wasn't up to the task, while commercial solutions were slightly outside of our budget.

Architecture

The system is made up of three parts:

  1. A Raspberry Pi Pico on a custom PCB
  2. An Adafruit BNO085 9-dof IMU
  3. A Septentrio mosaic-go H dual antenna GNSS kit

BNO085 breakout (left) and mosaic-go H (right)

BNO085 and mosaic-go H

Neither sensor is good enough on its own. The GNSS gets a very accurate heading from its two antennas, but it drops out whenever it loses a fix. The IMU updates much faster, but the gyro drifts over time and the magnetometer gets thrown off by anything metal on the boat. Fusing the two lets the GNSS correct the IMU, and lets the IMU fill in while the GNSS is out.

The filter sends its heading to the rudder controller, and if it isn't confident in the heading the rudder falls back to its own logic.

I focused on the simulator first, before most of the firmware, for a few reasons:

  1. Testing on the boat is extremely slow, and the boat is too heavy to put through all the possible scenarios on land, like heavy heel or big waves.
  2. In the simulator I know the true heading, so I can see exactly how wrong the filter is. On the boat the only thing to compare against is the GNSS, which is also one of the filter's inputs.
  3. A Kalman filter can look fine most of the time and then fall apart in one specific situation. Sweeping through heel, trim, and gyro drift in simulation finds those situations before the boat does.
  4. The simulator runs the exact same filter code as the firmware, so I'm never testing a different version of the filter than the one on the boat.

I also set up a CI pipeline that runs the filter through the simulator on every change, so I'd know right away if something I changed made the heading worse.

Implementation

EKF

A Kalman filter is a way of combining sensors that are each wrong in different ways. It keeps a best guess of where the boat is pointing, along with how sure it is about that guess. It does this in two steps:

  1. Predict: the gyro measures how fast the boat is turning, so the filter uses it to move the guess forward. The gyro is never perfect, so every step the filter gets a little less sure.
  2. Correct: when another sensor gives a reading, the filter moves its guess toward it. How far depends on how sure it is of each. If the filter is unsure and the GNSS reading is clean, it makes a big correction. If the filter is sure and the reading is noisy, it only nudges a bit.

On Polaris the gyro does the predicting 100 times a second, and the corrections come from:

A reading that disagrees too much with the guess gets thrown out, so one bad reading can't yank the heading around.

The filter can also learn things no sensor measures directly. If the GNSS keeps pulling the heading the same way, the filter figures out the gyro is drifting and starts correcting for it. Ours learns the drift on all three gyro axes, and the difference between the magnetometer's heading and the real heading.

The "extended" part means it still works when the math isn't a straight line. Heading, roll, and pitch are full of sines and cosines (see the heel math below), so the filter approximates them as straight lines around its current guess, and redoes that every step.

FreeRTOS

I used FreeRTOS to split the firmware into separate tasks for reading the IMU, reading the GNSS, running the filter, sending to the rudder, and saving calibration to flash. The rudder task always sends the newest estimate, so it never falls behind if the filter runs slow.

Simulator

The simulator can recreate:

Each run plots the true heading against what the filter estimates, so it's easy to see exactly where it goes wrong.

20° heel with a GNSS outage from 32 to 57 s

Heel outage

PCB

The PCB is a two layer board I designed in KiCad. It holds the Pico and the IMU, has connectors for the GNSS and the rudder controller, and has overvoltage protection on the power input.

The board was originally designed around an Xsens MTi-3 IMU, and we later switched to the BNO085.

PCB front

PCB front

PCB back

PCB back

Debugging

  1. Heeling the boat would make the heading drift

    • Solution: The first version of the filter integrated the body-Z gyro straight into heading and only estimated a Z gyro bias. That only works when the boat is level. When the boat heels, a turn shows up split across two gyro axes:

      gyro_y = turn_rate * sin(roll)
gyro_z = turn_rate * cos(roll)

      At 20° of heel the Z gyro only sees 94% of the turn, and any bias on the Y gyro leaks into heading. To fix this I added roll, pitch, and all three gyro biases to the state. The filter subtracts the bias on each axis, then maps the gyro rates into heading rate:

      heading_rate = (gyro_y * sin(roll) + gyro_z * cos(roll)) / cos(pitch)

      The cos(pitch) in the denominator blows up as pitch goes to 90°. You can see this in the sweep below, where heel doesn't matter but trim past 70° does. That's why the filter stops trusting its heading past 80° of pitch.

  2. Indoors with no GNSS, the heading would drift about 15 times faster than the gyro said it should

    • Solution: The magnetometer heading indoors was moving around a lot, and the filter was trusting it too much. It pushed that movement into the heading instead of into the magnetometer offset. I changed the filter to throw out magnetometer readings that disagree too much with the gyro, and let the offset move more freely. In the simulator, drift during an outage went from 69° to 3.5°.

Peak heading error in a 30 s GNSS outage across boat attitudes

Drift sweep

BNO085 magnetometer calibration spin

Mag cal spin

Repositories