Control Systems & Robotics Advanced 26 min

Discrete Kalman Filter IMU Sensor Fusion for Tilt & Orientation

Fuse high-frequency gyroscope angular rates with noisy low-frequency accelerometer tilt angles using a discrete 2-state Kalman filter.

2. Prerequisites & Background Theory:

Matrix algebra, Gaussian noise covariance, discrete linear state-space, and C++ pointer manipulation.

STEP 1

Goal & Engineering Objectives

Estimate true planar roll and pitch tilt angles in real-time on an embedded microcontroller, eliminating gyro drift and accelerometer vibration artifacts.

STEP 2

Governing Mathematical Formulation & Physics

                  State Vector & Dynamics Matrix:
$$\mathbf{x}_k = \begin{bmatrix} \theta \ \dot{b} \end{bmatrix}, \quad \mathbf{F} = \begin{bmatrix} 1 & -\Delta t \ 0 & 1 \end{bmatrix}, \quad \mathbf{B} = \begin{bmatrix} \Delta t \ 0 \end{bmatrix}$$

Predict Step:
$$\hat{\mathbf{x}}_k^- = \mathbf{F} \hat{\mathbf{x}}_{k-1} + \mathbf{B} \omega_k, \quad \mathbf{P}_k^- = \mathbf{F} \mathbf{P}_{k-1} \mathbf{F}^T + \mathbf{Q}$$

Measurement Update Step:
$$\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}^T (\mathbf{H} \mathbf{P}_k^- \mathbf{H}^T + R)^{-1}$$
$$\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k (z_k - \mathbf{H} \hat{\mathbf{x}}_k^-)$$
$$\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}) \mathbf{P}_k^-$$
Where zk is the raw accelerometer tilt measurement.                

STEP 3

System Architecture & Block Flow Diagram

[MPU-6050 Accelerometer (ax,ay,az)] --(I2C 400kHz)--> [Raw Tilt Calculation]
[MPU-6050 Gyroscope (gx,gy,gz)]     --(I2C 400kHz)--> [Rate Integration]
                                                          |
                                              [Kalman Predict Step]
                                              [Kalman Innovation (y)]
                                              [Kalman Update Step]
                                                          |
                                                          v
                                      [Optimal Filtered Angle (theta)]

STEP 4

Required Hardware Bill of Materials (BOM)

Hardware Component	Qty	Engineering Specification
Arduino Nano Every or STM32 BluePill	1	High-resolution hardware timer, hardware math
MPU-6050 6-Axis MotionTracking IMU	1	16-bit ADCs for Gyro and Accel, I2C interface
0.96 inch I2C OLED Display (SSD1306)	1	128x64 pixels, visual pitch/roll horizon display
Breadboard & Dupont Jumper Wires	1	Short solid-core leads for minimal I2C capacitance

Look up component pinouts in Components Catalog →

STEP 5

Wiring, Pinout & Schematics Mapping

                  • MPU-6050 VCC -> 3.3V (or 5V if board has on-board 3.3V LDO)
• MPU-6050 GND -> MCU GND
• MPU-6050 SCL -> MCU SCL (with 4.7kΩ pullup to 3.3V)
• MPU-6050 SDA -> MCU SDA (with 4.7kΩ pullup to 3.3V)
• MPU-6050 AD0 -> GND (Sets I2C address to 0x68)
• MPU-6050 INT -> MCU D2 (Timer interrupt synchronization)                

STEP 6

Firmware Implementation & Algorithms

// 2-State Discrete Kalman Filter for Tilt Angle Estimation (C++)
class KalmanAngle {
public:
    KalmanAngle() {
        q_angle = 0.001f;
        q_bias  = 0.003f;
        r_measure = 0.03f;
        angle = 0.0f;
        bias = 0.0f;
        P[0][0] = 0.0f; P[0][1] = 0.0f;
        P[1][0] = 0.0f; P[1][1] = 0.0f;
    }

    float getAngle(float newAngle, float newRate, float dt) {
        // Step 1: Predict
        float rate = newRate - bias;
        angle += dt * rate;

        P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + q_angle);
        P[0][1] -= dt * P[1][1];
        P[1][0] -= dt * P[1][1];
        P[1][1] += q_bias * dt;

        // Step 2: Measurement Update
        float S = P[0][0] + r_measure;
        float K[2];
        K[0] = P[0][0] / S;
        K[1] = P[1][0] / S;

        float y = newAngle - angle; // Innovation
        angle += K[0] * y;
        bias  += K[1] * y;

        float P00_temp = P[0][0];
        float P01_temp = P[0][1];

        P[0][0] -= K[0] * P00_temp;
        P[0][1] -= K[0] * P01_temp;
        P[1][0] -= K[1] * P00_temp;
        P[1][1] -= K[1] * P01_temp;

        return angle;
    }

private:
    float q_angle, q_bias, r_measure;
    float angle, bias;
    float P[2][2];
};

Simulate and Edit in Coding Lab

STEP 7

Bench Testing & Waveform Verification

                  1. Place IMU on level bench: Verify filtered angle reads 0.00° ± 0.05°.
2. Vibrate the test bench vigorously by hand: Observe raw accelerometer angle jumps by ±15°, while Kalman output remains completely stable.
3. Tilt the IMU rapidly to 45°: Verify filtered angle reaches 45° in real-time with zero overshoot and no lag.                

STEP 8

Troubleshooting & Common Failure Modes

                  • Problem: Estimated angle slowly drifts away over several minutes.
  Solution: Accelerometer measurement variance R is set too high, causing filter to distrust the accelerometer and rely solely on the drifting gyro. Reduce R from 0.5 to 0.03.
• Problem: Angle estimate jitters whenever the robot vibrates.
  Solution: Process noise Q_angle is set too high. Reduce Q_angle to 0.001.
• Problem: Angle output displays gimbal lock at 90°.
  Solution: Standard Euler angles suffer from singularity at 90 degrees. Upgrade to a 4-component Quaternion Kalman filter.                

STEP 9

Extensions & Undergraduate Research Challenges

                  1. Expand to a 6-DOF Extended Kalman Filter (EKF) incorporating magnetometer measurements for yaw heading tracking.
2. Implement Mahony or Madgwick gradient-descent filter on ARM Cortex-M0 for lower CPU execution overhead.                