Set Up a Kalman Filter on a BNO055 With a Raspberry Pi 5

Introduction

Have you ever wondered how your phone always knows which way is up? Or how a drone manages to stay perfectly level while it's hovering in the wind? Tilt any of them and they react instantly, and somehow they never slowly lose track of where they are.

Well, the funny thing is that the sensors inside them are actually pretty bad on their own. The gyroscope drifts over time, and the accelerometer is super noisy, so neither of them can really be trusted by itself. The trick that makes them work together is called sensor fusion, and one of the most popular ways to do it is with something called a Kalman filter.

Now, most explanations of the Kalman filter start with a whole page of matrices, and that's usually the point where you close the tab 😅. Today I'll show you how to set up a Kalman filter on a BNO055 sensor with a Raspberry Pi 5, and we'll build a live demo that streams straight to your browser so that you can actually see why it matters.

On the left side of the screen we'll have a 3D board that only uses the raw gyroscope, and on the right we'll have the exact same data, but run through a Kalman filter. Put the sensor down on the table, and watch one of them slowly lose its mind 🙃

Supplies

Software:

Embedder

uv (for the Python environment on the Pi)

Components:

Raspberry Pi 5

BNO055 Shuttle Board 3.0

Jumper wires (1.27 mm on the sensor side, I made my own)

Step 1: The Idea

The BNO055 has three sensors inside of it, but for this project we only really care about two, the gyroscope and the accelerometer. And the cool thing is that each one is good at exactly what the other one is bad at.

The gyroscope measures how fast the sensor is rotating. To get an angle out of it, we just add up all of those rotations over time. It's smooth and responds instantly, but every gyroscope has a tiny error built into it called bias, and when you add up a tiny error 100 times a second... the angle slowly starts walking away. This is what's called drift.

The accelerometer on the other hand measures acceleration, and when the sensor is sitting still, the only acceleration it feels is gravity. So it always knows which way is down and it never drifts. The problem is that it's really noisy, and the moment you move the sensor, the acceleration from your hand gets mixed in with gravity.

So the idea is to use both of them together. In the short term we trust the gyro, since it's smooth and fast, and in the long term we trust the accelerometer, since it always knows where down is. The Kalman filter is what decides how much to trust each one every time new data comes in, and the best part is that it can even work out the gyro's bias as it goes and cancel it out.

With that figured out, the plan of execution for this project can be divided into three parts:

Hardware: Getting the BNO055 talking to the Pi.

The Brain: A Python script that reads the sensor and runs two versions of the maths side by side, one with the Kalman filter and one without.

The Display: A web page that shows both of them as 3D boards, so you can watch them from your laptop.

Step 2: Wiring the Electronics

For this project I'm using Bosch's own BNO055 Shuttle Board 3.0 instead of one of the usual breakout boards, and this actually matters a lot. Breakout boards come with extra parts that set the chip up for you, but the shuttle board doesn't, so a few of the pins have to be connected by hand.

The board has two rows of pins, P1 along the top and P2 along the bottom, and they're spaced at a tiny 1.27 mm, which is half the size of normal jumper wires. So I ended up making my own cables, with a small connector on the sensor side and normal pins on the Pi side.

Here are all of the connections (pin names are from Bosch's shuttle board flyer):

P1-1 (VDD) → Pi pin 1 (3.3V)

P1-2 (VDDIO) → Pi pin 17 (3.3V)

P1-3 (GND) → Pi pin 6 (GND)

P1-6 (PS0) → GND

P1-7 (PS1) → GND

P2-2 (SCL) → Pi pin 5 (SCL)

P2-3 (SDO) → GND

P2-4 (SDA) → Pi pin 3 (SDA)

Everything else can be left unconnected.

Disclaimer: The BNO055 runs on 3.3V, so make sure you don't connect it to the Pi's 5V pins (pins 2 and 4), otherwise you might fry it.

Pro tip: PS0, PS1 and SDO are the sneaky ones on this board. PS0 and PS1 tell the chip to use I2C, and SDO sets its address to 0x28. If you leave them floating, the chip might start up in the wrong mode and the Pi just gets silence. I also nearly forgot VDDIO, which powers the chip's communication pins, so double check that one too.

Pinout diagram: Raspberry Pi Ltd.

Step 3: Setting Up the Pi

My Pi runs headless (no screen or keyboard), so I did all of this over SSH from my laptop.

First: Turn on I2C. It's off by default, and until you turn it on, the Pi can't talk to the sensor at all.

sudo raspi-config nonint do_i2c 0
sudo reboot

(Yes, 0 means "on" here, don't ask me why.)

Next: Check that the Pi can actually see the sensor.

sudo apt install i2c-tools
i2cdetect -y 1

If you see 28 in the grid, that's the BNO055 saying hello 👋 If the grid is empty, head back to the wiring step.

Then: Install uv and set up a virtual environment. My Pi didn't even come with pip, but uv doesn't need it, so it's actually easier this way.

curl -LsSf https://astral.sh/uv/install.sh | sh
source $HOME/.local/bin/env

mkdir -p ~/imu-cube && cd ~/imu-cube
uv venv
source .venv/bin/activate
uv pip install smbus2 aiohttp

Finally: Copy the code over from your laptop.

scp bno055_bringup.py imu_fusion_server.py index.html <user>@<pi-ip>:~/imu-cube/

Note: The destination has to go last, written as user@host:path. I got this backwards the first time and spent way too long wondering why nothing was copying.

Step 4: Testing the Sensor

Before we get to any of the filtering, we first need to make sure the sensor is actually alive and talking to the Pi. Instead of spending an evening digging through the datasheet for register addresses, I used Embedder to write a quick bring-up script for it.

If you haven’t come across it before, Embedder is an AI coding agent made specifically for firmware and embedded systems. What I like about it is that it works off the actual datasheet for your part, so all the register addresses come straight from Bosch’s documentation instead of a guess, which is exactly what you want when you’re setting up a new sensor:

#!/usr/bin/env python3
"""BNO055 setup for Raspberry Pi 5 (I2C bus 1, addr 0x28)."""
import time
from smbus2 import SMBus

ADDR = 0x28
REG_CHIP_ID = 0x00      # expect 0xA0
REG_CALIB_STAT = 0x35
REG_OPR_MODE = 0x3D
REG_EULER_H = 0x1A      # 6 bytes: heading, roll, pitch (1 deg = 16 LSB)
MODE_CONFIG = 0x00
MODE_NDOF = 0x0C

with SMBus(1) as bus:
    # Identity check
    chip_id = bus.read_byte_data(ADDR, REG_CHIP_ID)
    print(f"CHIP_ID: 0x{chip_id:02X} (want 0xA0)")
    if chip_id != 0xA0:
        raise SystemExit("Not a BNO055, check PS0/PS1 grounding and address.")

    # Enter NDOF fusion mode
    bus.write_byte_data(ADDR, REG_OPR_MODE, MODE_CONFIG)
    time.sleep(0.025)
    bus.write_byte_data(ADDR, REG_OPR_MODE, MODE_NDOF)
    time.sleep(0.02)
    print("NDOF mode active. Streaming Euler angles (Ctrl+C to stop)...")

    def s16(lo, hi):
        v = lo | (hi << 8)
        return v - 65536 if v & 0x8000 else v

    while True:
        d = bus.read_i2c_block_data(ADDR, REG_EULER_H, 6)
        h = s16(d[0], d[1]) / 16.0
        r = s16(d[2], d[3]) / 16.0
        p = s16(d[4], d[5]) / 16.0
        cal = bus.read_byte_data(ADDR, REG_CALIB_STAT)
        print(f"H: {h:7.2f}  R: {r:7.2f}  P: {p:7.2f} | "
              f"cal S{(cal >> 6) & 3} G{(cal >> 4) & 3} "
              f"A{(cal >> 2) & 3} M{cal & 3}")
        time.sleep(0.1)

Note: This script uses NDOF mode, which is where the BNO055 does its own sensor fusion and gives you finished angles. That's great for normal projects, but since the whole point here is to do the fusion ourselves, the main script later switches the chip into a raw data mode instead. This script is just to prove the hardware works.

Run it with python bno055_bringup.py, and if you see CHIP_ID: 0xA0 and the angles change when you tilt the board, you're good to go ✅

Step 5: Building the Brain

So now we get to the part that does all of the thinking. Everything lives in one file, imu_fusion_server.py, which reads the sensor 100 times a second, runs both versions of the maths, and streams the results to your browser.

Remember how the bring-up script used NDOF mode, where the chip does the fusion for you? Here we switch it into AMG mode instead, which just hands over the raw accelerometer, magnetometer and gyroscope readings. Otherwise there would be nothing left for our filter to do...

All of the sensors get read in one go, so the readings all line up in time:

# one 18-byte block: acc(6) mag(6) gyr(6), single I2C transaction
d = bus.read_i2c_block_data(ADDR, REG_ACC_X_LSB, 18)

ax = s16(d[0], d[1]) * ACC_SCALE     # m/s^2
ay = s16(d[2], d[3]) * ACC_SCALE
az = s16(d[4], d[5]) * ACC_SCALE
gx = s16(d[12], d[13]) * GYR_SCALE   # degrees per second
gy = s16(d[14], d[15]) * GYR_SCALE
gz = s16(d[16], d[17]) * GYR_SCALE

And here's the Kalman filter itself. Honestly, it's a lot shorter than you would expect. There's one of these for roll and another one for pitch:

class Kalman1D:
    """2-state Kalman filter: state = [angle, gyro_bias]."""

    def __init__(self, q_angle=0.001, q_bias=0.003, r_measure=0.03):
        self.q_angle = q_angle      # how much the angle wanders between updates
        self.q_bias = q_bias        # how fast the gyro bias wanders
        self.r_measure = r_measure  # how noisy the accelerometer is
        self.angle = 0.0
        self.bias = 0.0
        self.p00 = self.p01 = self.p10 = self.p11 = 0.0  # uncertainty

    def predict(self, rate, dt):
        # gyro step: move the angle by (rate - bias), grow the uncertainty
        self.angle += dt * (rate - self.bias)
        self.p00 += dt * (dt * self.p11 - self.p01 - self.p10 + self.q_angle)
        self.p01 -= dt * self.p11
        self.p10 -= dt * self.p11
        self.p11 += self.q_bias * dt

    def update(self, measurement, r_scale=1.0):
        # accel step: pull toward the gravity angle, learn the bias
        s = self.p00 + self.r_measure * r_scale
        k0 = self.p00 / s          # Kalman gain for the angle
        k1 = self.p10 / s          # Kalman gain for the bias
        y = measurement - self.angle
        self.angle += k0 * y
        self.bias += k1 * y
        p00, p01 = self.p00, self.p01
        self.p00 -= k0 * p00
        self.p01 -= k0 * p01
        self.p10 -= k1 * p00
        self.p11 -= k1 * p01

The line to look out for is self.bias += k1 * y. Every time the accelerometer disagrees with the gyro in the same direction, the bias guess moves a little closer to the gyro's real error, and you can actually watch this number settle in the display later.

Right next to the filter, the script also runs a "dumb" version that takes the exact same gyro readings and just adds them up, with no bias correction and no accelerometer. This is what drives the left side of the screen.

Note: If the sensor isn't answering, the script tells you what to check and shuts down, instead of leaving the web page sitting there with frozen numbers.

Before trying it on the real sensor, I tested the filter on some fake data with a really bad gyro, held perfectly still for 60 seconds. The dumb version drifted a whole 90 degrees, while the Kalman filter stayed within 0.1 degrees. The real BNO055 is a lot better than that fake gyro, so the drift is slower, but you'll still clearly see it within a minute or two.

The full code is available here: https://github.com/Iloke-Alusala/BNO055-Kalman_Filter

Step 6: The Display

Since the Pi doesn’t have a screen, it hosts a little web page instead, so you can open it from any laptop on the same Wi-Fi and watch the data stream in.

The left board is the raw gyro, and the right board is the Kalman filter. Both get the exact same data, so the only difference you’re seeing is the maths behind them.

The Align raw button snaps the red board back onto the teal one so the drift starts again from zero, and Zero yaw resets the heading on both.

Step 7: Time to Shine

Once everything is copied over, run this on the Pi:

cd ~/imu-cube
source .venv/bin/activate
python imu_fusion_server.py

Then find the Pi's IP address with hostname -I, and open http://<pi-ip>:8080 on your laptop.

Note: Make sure the bring-up script isn't running at the same time, otherwise the two will fight over the sensor.

Now for the fun part. Tilt the sensor around and both boards should follow your hand. Then hit Align raw, put the sensor flat on the table and leave it alone. Within a minute or two, the red board starts tilting all on its own while the teal one stays perfectly still, even though nothing is moving 🤯

Keep an eye on the bias number too, it slowly settles as the filter works out your gyro's error. And if you give the sensor a good shake, the accelerometer readings go crazy, but the teal board stays calm.

Step 8: Troubleshooting

I ran into quite a few issues getting this working, so here they are, so that you don't have to go through the same pain I did 😅

scp says "no such file or directory": The destination has to go last, like user@pi-ip:~/imu-cube/.

/dev/i2c-1 not found: I2C is still off. Run sudo raspi-config nonint do_i2c 0 and reboot.

Remote I/O error: The Pi is talking, but nothing is answering. Run i2cdetect -y 1, and if there's no 28, check that VDDIO has power and that PS0, PS1 and SDO are connected to GND.

i2cdetect shows 29 instead of 28: SDO isn't connected to GND, so the chip picked the other address.

Permission denied: Run sudo usermod -aG i2c $USER, then log out and back in.

A board turns the wrong way: Tick the invert box for that axis.

Pro tip: Keep your wires short and stick the sensor down to whatever you're rotating. If it's dangling on loose wires, it wobbles on its own and the boards will show that wobble as real movement.

Final Reflections

I've read about Kalman filters so many times, and it honestly never fully clicked until I watched the red board slowly tip over on a table where nothing was moving. The maths is exactly the same as in the textbooks, but seeing two boards get the same data and end up in completely different places makes it really obvious why every drone and robot out there does this.

The hardest part of this project actually wasn't the code, it was the wiring. The shuttle board leaves a couple of pins floating, and "Remote I/O error" doesn't exactly tell you much about why 😅

As for what's next, this filter only fixes roll and pitch, so the next step would be adding the magnetometer to fix the heading as well. After that, I want to try fusing the IMU with a camera, which is how a lot of robots keep track of where they are without GPS. So stay tuned for that one 👀

If you end up building this, I'd love to see your version, so drop it in the comments!

Until next time 🙌

Disclosure: This project was sponsored by Embedder. I used Embedder to write the BNO055 bring-up script from the datasheet, and built the Kalman filter and display on top of that. All opinions are my own.

~ Iloke