Complementary Filter for IMU Sensor Fusion | Lesson 11

The Madgwick filter sits between complementary and common filters in terms of complexity. It combines information from three sensors (accelerometer, gyroscope, and magnetometer) while remaining efficient enough to run on tiny embedded systems. This efficiency makes it particularly popular in drones, robotics, and hobby electronic projects where computational resources are limited. It provides a good balance between accuracy and computational cost.

Gyroscope data configuration involves setting low pass filters to remove high-frequency noise (typically 50 Hz for GoPro). Axis orientation determines how the software interprets camera movement direction. Rotation offset correction allows adjusting for misaligned loggers. Integration methods (Integral, Complementary, Mahony, Madgwick) determine how gyroscope data is interpreted. For GoPro 8+, Integral method is used; for other cameras, Complementary or Madgwick work best for cinematic stabilization.

Sensor fusion combines data from multiple inertial sensors (gyroscopes, accelerometers, and magnetometers) to achieve accurate and reliable orientation estimation for drones, as each sensor has complementary strengths: gyroscopes provide responsive short-term angular velocity measurements but suffer from drift, accelerometers offer stable long-term gravity references but are noisy during motion, and magnetometers provide absolute heading references for yaw correction; common fusion algorithms include the complementary filter (blending high-frequency gyroscope data with low-frequency accelerometer/magnetometer data), Kalman-based filters (IMU filter, AHRS filter), and gradient descent methods (Madgwick and Mahony algorithms), all of which can be implemented in MATLAB and Simulink for real-time orientation estimation in autonomous systems.

Madgwick's Filter is a complementary filter algorithm that fuses data from a 6DOF (gyroscope and accelerometer) or 9DOF (adding magnetometer) sensor module to provide accurate orientation estimation for balancing robots; the filter requires only two configuration parameters—the sample frequency and the gain (beta)—and outputs a quaternion that can be converted to Euler angles like pitch to determine if the robot is tipping over, enabling basic balancing control through simple proportional motor speed adjustments based on the pitch angle.

Steerable filters enable efficient computation of filter responses at arbitrary orientations by leveraging the singular value decomposition (SVD) of a matrix formed from rotated versions of a basis filter. The process involves synthesizing multiple rotated filter kernels, vectorizing them into matrix columns, performing SVD to obtain orthogonal basis filters, and then using these basis filters to interpolate responses at any desired orientation through linear combination. This approach provides significant computational savings compared to computing each rotated filter separately, as the number of required basis filters corresponds to the rank of the SVD, which is typically much smaller than the total number of orientations needed. The right singular vectors correspond to Fourier basis functions due to the rotational periodicity of the problem, enabling efficient synthesis of filter responses at arbitrary angles.

An Inertial Measurement Unit (IMU) combines accelerometer, gyroscope, and magnetometer data through sensor fusion algorithms to determine device orientation (pitch, yaw, roll) and provide absolute acceleration values in North-East-Down coordinates; the implementation requires calibrating the magnetometer by taking median values during rotation, using an AHRS algorithm like the Mahony Quaternion Update to fuse sensor data into quaternions, converting quaternions to rotation matrices, transforming acceleration vectors to absolute coordinates, and applying magnetic declination correction for accurate heading.

Converting raw magnetometer data to usable yaw requires multiple steps. Basic yaw calculation uses arctangent of Y/X axes but fails when pitch/roll change due to assuming level orientation. Earth's magnetic field has vertical components (10-60° downward depending on location), which simple compasses ignore. True north requires combining magnetometer with accelerometer via Direction Cosine Matrix (DCM). The DCM uses cross products to derive north, east, and down vectors, accounting for full 3D orientation. Pitch and roll estimates from complementary filters enable complex trigonometry to transform readings, providing stable yaw even during banking maneuvers.

A 9-degree-of-freedom inertial measurement unit (IMU) combining a 3-axis accelerometer, 3-axis gyroscope, and 3-axis magnetometer enables accurate orientation tracking by fusing sensor data to eliminate drift and noise; the accelerometer provides stable baseline readings with no drift but is noisy, the gyroscope offers clean dynamic signals but suffers from drift, and the magnetometer provides absolute heading reference, together creating a robust system for measuring roll, pitch, and yaw without the limitations of using any single sensor type.

An Inertial Measurement Unit (IMU) combines sensors to measure orientation and motion relative to an inertial reference frame, specifically the Earth Centered Inertial (ECI) frame located at Earth's center. IMUs come in two configurations: 6DOF (gyroscope + accelerometer) and 9DOF (adding magnetometer). Gyroscopes measure angular rotation rates in radians per second, enabling orientation angle estimation through integration. However, all gyroscopes exhibit inherent biases—constant offsets in rate measurements even when stationary—which cause cumulative drift in integrated angle estimates over time. This fundamental limitation necessitates advanced correction methods for practical applications.

The BNO055 is a 9-degree-of-freedom Inertial Measurement Unit (IMU) that combines an accelerometer, gyroscope, and magnetometer to measure orientation in three dimensions. To use it, connect the sensor to a microcontroller via I²C, verify the I²C address (hex 29), switch from config mode to NDOF mode by writing hex 0C to register 3D, and read yaw, roll, and pitch from registers 1A-1F (combining two 8-bit registers into a 16-bit signed word and dividing by 16 for 1-degree resolution). The magnetometer calibration status can be checked in register 35, and if not fully calibrated (all ones), perform figure-eight motions and 5-second flat/upside-down positions to assist calibration.

Quaternions provide an efficient four-parameter representation of 3D rotations that avoids the gimbal lock problem inherent in Euler angle representations; a unit quaternion (with norm 1) can represent any 3D rotation about an arbitrary axis by an angle α using the formula q = cos(α/2) + sin(α/2)(a_x i + a_y j + a_z k), where (a_x, a_y, a_z) is the unit vector axis of rotation, and the sandwich product qvq* rotates a vector v, with the key property that both q and -q represent the same rotation (double cover property).

Quaternions are a 4-dimensional number system that solve fundamental problems in representing 3D orientations. Unlike Euler angles, which suffer from gimbal lock when rotation axes align, quaternions provide a robust alternative. They enable seamless interpolation between orientations without ambiguities or numerical precision issues that plague rotation matrices. This makes quaternions essential for computer graphics, robotics, and virtual reality applications, including smartphone orientation tracking. The mathematical structure allows complex rotations to be computed efficiently through multiplication operations, extending the elegant principles of complex numbers for 2D rotation into three dimensions.

Euler angles represent 3D orientation using three ordered values (X, Y, Z) that suffer from gimbal lock—a phenomenon where two rotation axes align, causing loss of a degree of freedom—making them easier to visualize but harder to work with in complex animations; Quaternions represent orientation using four values (W, X, Y, Z) in four-dimensional space, which avoids gimbal lock and enables smooth spherical interpolation (SLERP) between orientations, though they are more abstract to understand and require each keyframe to be less than 180 degrees apart for proper animation.

Quaternions provide an alternative representation for 3D rotations that addresses many limitations of matrices and Euler angles. They require only four values (one scalar and three imaginary components), making them more compact than matrices. Quaternions are immune to gimbal lock (assuming they are not constructed from Euler angles) and support smooth spherical linear interpolation (slerp) between orientations. While quaternions are mathematically challenging to understand intuitively, they are the preferred choice for representing rotations in game engines due to their computational efficiency and stability during interpolation.

Euler angles represent 3D rotations as three sequential rotations around X, Y, and Z axes (yaw, pitch, roll). However, they suffer from gimbal lock (loss of a degree of freedom at certain orientations) and produce biased distributions when sampling randomly. Quaternions, which use four real numbers (w, x, y, z) representing coefficients of i, j, k imaginary units, provide a smoother representation without singularities. They correspond to points on a 4D hypersphere and avoid the problems of Euler angles.

The PID controller maintains robot balance by calculating error between desired angle (0 degrees) and actual angle from MPU6050. It uses three parameters: KP (Proportional) responds to current error magnitude, KI (Integral) eliminates steady-state error over time, KD (Derivative) predicts future error based on rate of change. Tuning process: (1) Start with KP - increase from 5 to 20 until robot oscillates but doesn't fall; (2) Add KI (e.g., 0.5-1.5) to eliminate oscillation and achieve stable balance; (3) Adjust KD (e.g., 140) to speed up settling. Optimal values: KP=22, KI=1.5, KD=140 may provide stable balance.

This section covers two fundamental control concepts: inverse kinematics determines motor angles needed to achieve desired platform orientations, derived from geometric relationships of the robot structure; PID controllers automatically compare target values (platform center) with actual values (ball position) to compute corrective actions. The error is a vector quantity with both magnitude and direction. The PID algorithm computes proportional, integral, and derivative terms using constants KP, KI, and KD, which must be tuned for stable operation. These concepts form the theoretical foundation for balancing and positioning control systems.

This extensive section details the complete PID control implementation for balancing robots. The PID architecture computes motor commands based on orientation error between target and estimated angles. The proportional term responds to current error, the integral term eliminates steady-state error through accumulated past errors, and the derivative term predicts future error to reduce overshoot. The error calculation involves subtracting estimated angle from target angle. Motor speed commands are generated by applying PID gains to the error signal, producing values that drive left and right motors to generate corrective torque. Target angle selection considers robot geometry, with slight forward tilt (4-5 degrees) often necessary for stable balancing. PID gain tuning significantly affects performance: increasing P gain increases response speed but may cause oscillation, while I gain eliminates steady-state error but can cause overshoot. D gain reduces overshoot and improves stability. The final implementation demonstrates practical tuning to achieve stable balancing behavior.

The Odometry message (nav_msgs/Odometry) provides robot position and orientation information. It contains a Pose message with position (x, y, z) and orientation represented as a quaternion (x, y, z, w). Quaternions avoid gimbal lock problems that occur with Euler angles. The orientation must be converted to Euler angles (roll, pitch, yaw) for easier interpretation using quaternion-to-Euler conversion functions. To control robot orientation, calculate the desired orientation angle using the arctangent function: φ_desired = arctan2(y_desired - y_current, x_desired - x_current). This gives the angle from the robot's current position to the target position. The orientation error is the difference between desired and actual orientation angles. The angular velocity command is proportional to this error. The arctangent function properly handles angle wrapping, which is a common problem when working with angles where values can jump from π to -π. A PID controller can be implemented as a Python class with parameters for proportional gain (Kp), integral gain (Ki), and derivative gain (Kd). The controller maintains an integral term that accumulates past errors and an error_prev variable for the derivative calculation. The controller output is calculated as: u = Kp*error + Ki*integral + Kd*(error - error_prev)/dt. The controller must be called at each time step with the current error. Discrete implementation uses the time step (dt) for integral and derivative calculations. PID parameter tuning is critical: increasing Kp reduces steady-state error but causes oscillations, increasing Ki eliminates steady-state error but increases overshoot, and increasing Kd reduces oscillations but increases noise sensitivity.

PID controllers use proportional, integral, and derivative terms to maintain robot balance. The P term responds to tilt angle, I eliminates steady-state error, and D prevents overshooting. Successful implementation requires iterative tuning of coefficient values to achieve stable, smooth balancing behavior.