Published October 29, 2026

ORBIT-Q: MPU6050 on STM32

Orbit-Q STM32F4 project using a Kalman filter to smooth MPU6050 sensor readings and display accelerometer and gyroscope data on an OLED.

IntermediateProtip9

Things used in this project

Hardware components

ORBIT-Q

Jumper wires (generic)

MPU6050(GY-521) Sensor Module

Software apps and online services

Arduino IDE

STMicroelectronics STM32CUBEPROG

Adafruit SSD1306

Adafruit GFX Library

Wire/TwoWire

Hand tools and fabrication machines

ST-Link debugger

Story

MPU6050 Kalman Filter with ORBIT-Q STM32

Introduction

This project demonstrates how to read and filter accelerometer and gyroscope data from an MPU6050 sensor using the Orbit-Q STM32F405RGT6 development board. The filtered readings are displayed on an SSD1306 OLED display.

The project was developed as part of my embedded systems internship. The development board was provided by Neotrivotx System Private Limited(NTX Systems), and I modified the existing firmware to implement the filtering functionality.

Objectives

1. Read accelerometer and gyroscope measurements from the MPU6050.

2. Apply six independent one-dimensional Kalman filters to smooth the sensor readings.

3. Display the filtered measurements on an OLED screen.

4. Gain practical experience with STM32, I²C communication, embedded C++ and sensor interfacing.

## Hardware Components

1. Orbit-Q STM32F405RGT6 development board

2. MPU6050 (GY-521) accelerometer and gyroscope module

3. SSD1306 OLED display (128 × 32)

4. Jumper wires and breadboard.

Working Principle

The MPU6050 measures acceleration along three axes (X, Y and Z) and angular velocity along these three axes. The STM32 reads these measurements through I²C communication.

The firmware applies six separate one-dimensional Kalman filters: three for acceleration and three for angular velocity. These filters estimate the sensor readings using prediction and measurement updates. The resulting filtered values are then displayed on the OLED.

Note: These are independent filters for individual sensor channels; the firmware does not calculate fused orientation angles such as roll and pitch.

Software and Libraries

1. Arduino IDE with the appropriate STM32 board support

2. Wire/TwoWire for I²C communication

3. Adafruit GFX Library

4. Adafruit SSD1306 Library

Testing and Results

The system was tested using the connected hardware. The OLED displayed the filtered accelerometer readings in m/s² and gyroscope readings in degrees per second (°/s).

This project helped me understand sensor interfacing, firmware modification, digital filtering and embedded systems debugging.

Acknowledgements

The Orbit-Q development board was provided by Neo Vortex as part of my embedded systems internship. The firmware was modified from an existing codebase. Original code and third-party libraries should be credited to their respective authors where applicable.

Code

Using a Kalman Filter on MPU6050

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH  128
#define SCREEN_HEIGHT 32
#define OLED_RESET    -1
#define OLED_ADDRESS  0x3C

#define MPU_ADDR      0x68
#define WHO_AM_I      0x75
#define PWR_MGMT_1    0x6B
#define ACCEL_CONFIG  0x1C
#define GYRO_CONFIG   0x1B
#define ACCEL_XOUT_H  0x3B

#define STATUS_LED    PC13

TwoWire Wire2(PB11, PB10);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

bool ledState = false;

// Independent 1D Kalman filter for smoothing each sensor axis.
class Kalman1D {
public:
  Kalman1D(float processNoise, float measurementNoise)
    : q(processNoise), r(measurementNoise),
      estimate(0.0f), estimateError(1.0f), initialized(false) {}

  float update(float measurement) {
    if (!initialized) {
      estimate = measurement;
      initialized = true;
      return estimate;
    }
    estimateError += q;
    float kalmanGain = estimateError / (estimateError + r);
    estimate += kalmanGain * (measurement - estimate);
    estimateError *= (1.0f - kalmanGain);
    return estimate;
  }

private:
  float q;
  float r;
  float estimate;
  float estimateError;
  bool initialized;
};

// Starting tuning values; each axis has its own filter.
Kalman1D filterAx(0.08f, 1.0f);
Kalman1D filterAy(0.08f, 1.0f);
Kalman1D filterAz(0.08f, 1.0f);
Kalman1D filterGx(0.5f, 4.0f);
Kalman1D filterGy(0.5f, 4.0f);
Kalman1D filterGz(0.5f, 4.0f);

bool writeRegister(uint8_t reg, uint8_t value) {
  Wire2.beginTransmission(MPU_ADDR);
  Wire2.write(reg);
  Wire2.write(value);
  return Wire2.endTransmission() == 0;
}

bool readRegisters(uint8_t reg, uint8_t *data, uint8_t length) {
  Wire2.beginTransmission(MPU_ADDR);
  Wire2.write(reg);
  if (Wire2.endTransmission(false) != 0) return false;

  uint8_t received = Wire2.requestFrom(MPU_ADDR, length);
  if (received != length) {
    while (Wire2.available()) Wire2.read();
    return false;
  }
  for (uint8_t i = 0; i < length; i++) data[i] = Wire2.read();
  return true;
}

int16_t readInt16(uint8_t highByte, uint8_t lowByte) {
  return (int16_t)((uint16_t)highByte << 8 | lowByte);
}

void showError(const char *message) {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("MPU-6050 ERROR");
  display.println(message);
  display.display();
}

void setup() {
  Serial.begin(115200);
  pinMode(STATUS_LED, OUTPUT);
  digitalWrite(STATUS_LED, HIGH); // Active-low LED OFF

  Wire.setSCL(PB6);
  Wire.setSDA(PB7);
  Wire.begin();

  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Starting MPU-6050");
  display.display();

  Wire2.begin();
  Wire2.setClock(100000);

  uint8_t identity;
  if (!readRegisters(WHO_AM_I, &identity, 1) || identity != 0x68) {
    showError("Check sensor/I2C");
    while (true) delay(500);
  }
  if (!writeRegister(PWR_MGMT_1, 0x00)) {
    showError("Wake-up failed");
    while (true) delay(500);
  }

  writeRegister(ACCEL_CONFIG, 0x00); // +/- 2g
  writeRegister(GYRO_CONFIG, 0x00);  // +/- 250 degrees/second
  delay(100);
}

void loop() {
  uint8_t data[14];
  if (!readRegisters(ACCEL_XOUT_H, data, 14)) {
    showError("I2C read failed");
    delay(500);
    return;
  }

  int16_t rawAx = readInt16(data[0], data[1]);
  int16_t rawAy = readInt16(data[2], data[3]);
  int16_t rawAz = readInt16(data[4], data[5]);
  int16_t rawGx = readInt16(data[8], data[9]);
  int16_t rawGy = readInt16(data[10], data[11]);
  int16_t rawGz = readInt16(data[12], data[13]);

  float ax = rawAx / 16384.0f * 9.80665f;
  float ay = rawAy / 16384.0f * 9.80665f;
  float az = rawAz / 16384.0f * 9.80665f;
  float gx = rawGx / 131.0f;
  float gy = rawGy / 131.0f;
  float gz = rawGz / 131.0f;

  // Apply Kalman filters only; preserve output names, units and format.
  ax = filterAx.update(ax);
  ay = filterAy.update(ay);
  az = filterAz.update(az);
  gx = filterGx.update(gx);
  gy = filterGy.update(gy);
  gz = filterGz.update(gz);

  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Ax:"); display.print(ax, 1);
  display.print(" Ay:"); display.println(ay, 1);
  display.print("Az:"); display.println(az, 1);
  display.print("Gx:"); display.print(gx, 1);
  display.print(" Gy:"); display.println(gy, 1);
  display.print("Gz:"); display.println(gz, 1);
  display.display();

  ledState = !ledState;
  digitalWrite(STATUS_LED, ledState ? LOW : HIGH);

  Serial.print("Accel m/s2: ");
  Serial.print(ax); Serial.print(", ");
  Serial.print(ay); Serial.print(", ");
  Serial.println(az);

  Serial.print("Gyro deg/s: ");
  Serial.print(gx); Serial.print(", ");
  Serial.print(gy); Serial.print(", ");
  Serial.println(gz);

  delay(200);
}