IMU Attitude Estimation: Complementary vs Kalman Filter

The problem

Vehicle safety functions such as fall detection, hill hold and lean-angle traction control need a reliable attitude estimate. Gyros drift and accelerometers are disturbed by vehicle acceleration and vibration, so the fusion method matters, and it must run cheaply on a small MCU.

Objectives

• Implement a complementary filter and a two-state Kalman filter (angle and gyro bias) for roll and pitch
• Build a reference jig to measure static and dynamic angle error
• Quantify RMS error, drift, response time and sensitivity to linear acceleration
• Measure execution time in floating point and fixed point

What you need

Key concepts

Complementary filter

Angle = alpha times (previous angle plus gyro rate times dt) plus (1 − alpha) times accelerometer angle; alpha sets the crossover between gyro and accelerometer.

Kalman filter with bias state

The state holds angle and gyro bias; the gyro drives the prediction and the accelerometer angle corrects it, with noise covariances setting the trust in each.

Linear acceleration disturbance

When the vehicle accelerates, the accelerometer no longer measures only gravity, so accelerometer angle is wrong for the duration.

DWT cycle counter

The Cortex-M3 Data Watchpoint and Trace unit has a cycle counter that measures code execution time to the cycle.

Step-by-step guide

1. 1

   Questions and metrics

   Write down exactly what you will compare: static RMS error at known angles, drift over a 10-minute hold, response time to an angle step, error while the board is linearly accelerated, and cycles per update on the STM32. Choose sample rates of 100 Hz and 200 Hz so you can see the effect of rate on both filters. List the test cases, the number of repeats, and how ground truth is obtained for each case.

   Checkpoint: A written experiment plan with metrics and test cases.

2. 2

   Board and sensor setup

   Use the DIP switches to enable the MPU6050 and UART. Configure I2C at 400 kHz, set the MPU6050 accelerometer and gyro ranges, and choose the digital low-pass filter setting through register 0x1A, because it adds delay that affects both filters. Sample from a timer interrupt at a fixed rate and include a sample counter in the stream so you can verify the rate and detect dropped samples.

   Checkpoint: Samples arrive at the exact rate, verified by a timestamp counter.

3. 3

   Calibration

   Calibrate the accelerometer offset and scale on each axis using six static orientations, and measure the gyro bias at rest. Also log the MPU6050's internal temperature during calibration and later tests, because gyro bias changes with temperature and this explains slow drift. After calibration, check each static orientation reads 1 g on its axis within a small error, and record the residual error.

   Checkpoint: Each static orientation reads 1 g within a small error after calibration.

4. 4

   Raw data logging

   Stream raw calibrated samples over UART during every test, at a baud rate high enough for the chosen sample rate. Save each run to a file with the test name and settings. Replaying the same data through both filters in Python means differences come only from the algorithms, not from the motion. Check every file for dropped samples using the sample counter before analysing it.

   Checkpoint: A logged dataset of static, step and motion tests.

5. 5

   Complementary filter

   Implement the complementary filter for roll and pitch. Choose alpha from a desired crossover time constant and the sample period, then tune it on logged data in Python by minimising error on the jig tests. Port the tuned filter to the MCU and run it on the same logged data fed over UART, or compare live output with offline output, to confirm the two implementations agree.

   Checkpoint: MCU and Python outputs on the same data agree.

6. 6

   Kalman filter

   Implement a two-state Kalman filter per axis with angle and gyro bias as states. Set the process and measurement noise from your measured sensor noise, then tune. Add adaptive gating that increases the accelerometer measurement noise when the measured acceleration magnitude departs from 1 g, so vehicle acceleration is trusted less. Check that the bias estimate converges to the calibrated bias within a few seconds of start-up.

   Checkpoint: Estimated gyro bias converges to the calibrated bias within a few seconds.

7. 7

   Reference jig tests

   On the hinged jig, test static angles every 10 degrees, steps between angles and a 10-minute hold at a fixed angle. For linear acceleration, slide the jig along the table at a fixed angle and measure the angle error during the motion. Repeat every test several times and compute error statistics for both filters at both sample rates. Note the jig's own reading uncertainty.

   Checkpoint: Error statistics for both filters at each test.

8. 8

   Execution cost

   Measure cycles per update with the DWT cycle counter for both filters in software floating point, and in a fixed-point version of each. Run long enough to record the worst case as well as the mean. Express the results as microseconds and as a percentage of the sample period at 100 and 200 Hz, so a reader can see what each filter leaves for the rest of a vehicle controller.

   Checkpoint: A table of cycles per update for each implementation.

9. 9

   Analysis and recommendation

   Bring the results together in one table: accuracy, drift, response time, robustness to linear acceleration and execution cost for each filter at each sample rate. Recommend a filter for each vehicle function, such as fall detection, hill hold and lean-angle traction control, and explain the trade-offs behind each choice. Point out where differences are within measurement uncertainty and should not be over-interpreted, and what extra test would settle them.

   Checkpoint: A summary table and recommendation.

10. 10

    Report

    Write up the theory of both filters, the calibration, the tuning process, the experiments and the results with figures. Include the dataset and scripts so the results can be reproduced, and describe the jig so others can build one. Discuss what you would change for a real vehicle, such as vibration isolation, mounting alignment and temperature compensation of gyro bias.

    Checkpoint: Report with figures and data.

Testing & validation

• Static RMS error against the jig reference
• Drift over a 10-minute hold
• Step response time and error during linear acceleration
• Cycles per update for each implementation

Expected results

A quantified comparison of complementary and Kalman attitude estimation on the same MPU6050 data, with costs on a Cortex-M3 and a recommendation for vehicle safety functions.

Take it further

• Add Mahony or Madgwick quaternion filters to the comparison
• Use the BMP280 to study vertical motion
• Apply the chosen filter to a lean-angle-aware traction controller

Viva and review questions