Multiple IMU fusion

multiple_sensor.cpp

#include <Wire.h>
#include <Arduino.h>

// Define number of IMUs and EKF parameters
#define NUM_IMUS 3  // Number of IMU sensors
#define EKF_N 10    // State vector dimension: [position (3), velocity (3), quaternion (4)]
#define EKF_M 9     // Measurement vector dimension: [accelerometer (3), gyroscope (3), magnetometer (3)]

// Include TinyEKF and set dimensions
#define EKF_N 10
#define EKF_M 9
#include "tinyekf.h"

// Include MPU9250 library
#include "MPU9250.h"

// TCA9548A I2C Multiplexer address
#define TCAADDR 0x70

// MPU9250 instances - one for each sensor
MPU9250 imus[NUM_IMUS];

// IMU addresses - default is 0x68, can be 0x69 if AD0 pin is set high
uint8_t imu_addresses[NUM_IMUS] = {0x68, 0x68, 0x68};

// TCA9548A channels for each IMU
uint8_t imu_channels[NUM_IMUS] = {0, 1, 2};

// Calibration values for each IMU
// These should be determined by running the calibration routine for each IMU independently
float acc_bias[NUM_IMUS][3] = {
    {0.0, 0.0, 0.0},  // IMU 1
    {0.0, 0.0, 0.0},  // IMU 2
    {0.0, 0.0, 0.0}   // IMU 3
};

float gyro_bias[NUM_IMUS][3] = {
    {0.0, 0.0, 0.0},  // IMU 1
    {0.0, 0.0, 0.0},  // IMU 2
    {0.0, 0.0, 0.0}   // IMU 3
};

float mag_bias[NUM_IMUS][3] = {
    {0.0, 0.0, 0.0},  // IMU 1
    {0.0, 0.0, 0.0},  // IMU 2
    {0.0, 0.0, 0.0}   // IMU 3
};

float mag_scale[NUM_IMUS][3] = {
    {1.0, 1.0, 1.0},  // IMU 1
    {1.0, 1.0, 1.0},  // IMU 2
    {1.0, 1.0, 1.0}   // IMU 3
};

// Gravity constant
const float GRAVITY = 9.81;

// Custom EKF implementation for IMU fusion
class ImuEKF : public ekf_t {
public:
    // Initialize the EKF with default settings
    void init() {
        // Initial state covariance diagonal values
        float pval[EKF_N] = {
            1.0, 1.0, 1.0,     // Position variance
            0.1, 0.1, 0.1,     // Velocity variance
            0.1, 0.1, 0.1, 0.1 // Quaternion variance
        };
        
        // Initialize EKF with these variances
        ekf_initialize(this, pval);
    }
    
    // Custom model methods - these will be called by tinyekf.h
    void model(float fx[EKF_N], float F[EKF_N*EKF_N], float hx[EKF_M], float H[EKF_M*EKF_N]) {
        // Current state
        float x = this->x[0];  // Position x
        float y = this->x[1];  // Position y
        float z = this->x[2];  // Position z
        
        float vx = this->x[3]; // Velocity x
        float vy = this->x[4]; // Velocity y
        float vz = this->x[5]; // Velocity z
        
        float q0 = this->x[6]; // Quaternion w
        float q1 = this->x[7]; // Quaternion x
        float q2 = this->x[8]; // Quaternion y
        float q3 = this->x[9]; // Quaternion z
        
        // Time delta in seconds
        float dt = 0.01; // Assuming 100Hz update rate, adjust as needed
        
        // State transition function f(x)
        // Position updates with velocity
        fx[0] = x + vx * dt;
        fx[1] = y + vy * dt;
        fx[2] = z + vz * dt;
        
        // Velocity updates (assuming constant velocity model for simplicity)
        fx[3] = vx;
        fx[4] = vy;
        fx[5] = vz;
        
        // Quaternion updates (simplified, assuming small rotations)
        // In a real application, this would use gyro measurements for integration
        fx[6] = q0;
        fx[7] = q1;
        fx[8] = q2;
        fx[9] = q3;
        
        // State transition Jacobian F
        // Initialize to zeros
        for (int i = 0; i < EKF_N * EKF_N; i++) {
            F[i] = 0;
        }
        
        // Position derivatives w.r.t position (identity)
        F[0*EKF_N + 0] = 1;
        F[1*EKF_N + 1] = 1;
        F[2*EKF_N + 2] = 1;
        
        // Position derivatives w.r.t velocity
        F[0*EKF_N + 3] = dt;
        F[1*EKF_N + 4] = dt;
        F[2*EKF_N + 5] = dt;
        
        // Velocity derivatives w.r.t velocity (identity)
        F[3*EKF_N + 3] = 1;
        F[4*EKF_N + 4] = 1;
        F[5*EKF_N + 5] = 1;
        
        // Quaternion derivatives w.r.t quaternion (identity for now)
        F[6*EKF_N + 6] = 1;
        F[7*EKF_N + 7] = 1;
        F[8*EKF_N + 8] = 1;
        F[9*EKF_N + 9] = 1;
        
        // Measurement function h(x) - what we expect to measure given the state
        // This will be filled with predicted accelerometer, gyroscope, and magnetometer readings
        
        // Convert quaternion to rotation matrix
        float R[9];
        quaternionToRotationMatrix(q0, q1, q2, q3, R);
        
        // Predicted acceleration (accounting for gravity)
        // In body frame, when not accelerating, we should measure [0, 0, GRAVITY]
        float gravity[3] = {0, 0, GRAVITY};
        float acc_predicted[3];
        
        // Transform gravity from inertial to body frame
        rotateVector(R, gravity, acc_predicted, true);
        
        // Add linear acceleration (simple model)
        // For more accuracy, you would account for centripetal acceleration as well
        hx[0] = acc_predicted[0];
        hx[1] = acc_predicted[1]; 
        hx[2] = acc_predicted[2];
        
        // Predicted gyroscope readings
        // In a perfect model, if we're not rotating, gyro should read 0
        // For simplicity, we'll assume zero for now
        hx[3] = 0;
        hx[4] = 0;
        hx[5] = 0;
        
        // Predicted magnetometer readings
        // Using Earth's magnetic field, typically points north and down in northern hemisphere
        float mag_field[3] = {0.2, 0, 0.5}; // Example values, adjust for your location
        float mag_predicted[3];
        
        // Transform magnetic field from inertial to body frame
        rotateVector(R, mag_field, mag_predicted, true);
        
        hx[6] = mag_predicted[0];
        hx[7] = mag_predicted[1];
        hx[8] = mag_predicted[2];
        
        // Measurement Jacobian H
        // This is a complex matrix that relates how each state affects each measurement
        // For simplicity, we're going to use a numerical approximation method
        // In a real implementation, you would compute this analytically
        
        // Initialize H to zeros
        for (int i = 0; i < EKF_M * EKF_N; i++) {
            H[i] = 0;
        }
        
        // For simplicity, we'll just set some basic relationships
        // In a full implementation, these would be computed properly
        
        // Accelerometer affected by orientation (quaternion)
        H[0*EKF_N + 6] = 1;
        H[0*EKF_N + 7] = 1;
        H[0*EKF_N + 8] = 1;
        H[0*EKF_N + 9] = 1;
        
        H[1*EKF_N + 6] = 1;
        H[1*EKF_N + 7] = 1; 
        H[1*EKF_N + 8] = 1;
        H[1*EKF_N + 9] = 1;
        
        H[2*EKF_N + 6] = 1;
        H[2*EKF_N + 7] = 1;
        H[2*EKF_N + 8] = 1;
        H[2*EKF_N + 9] = 1;
        
        // Gyroscope measurements are not directly related to any state
        // In a real model, gyro would relate to rate of change of quaternion
        
        // Magnetometer affected by orientation (quaternion)
        H[6*EKF_N + 6] = 1;
        H[6*EKF_N + 7] = 1;
        H[6*EKF_N + 8] = 1;
        H[6*EKF_N + 9] = 1;
        
        H[7*EKF_N + 6] = 1;
        H[7*EKF_N + 7] = 1;
        H[7*EKF_N + 8] = 1;
        H[7*EKF_N + 9] = 1;
        
        H[8*EKF_N + 6] = 1;
        H[8*EKF_N + 7] = 1;
        H[8*EKF_N + 8] = 1;
        H[8*EKF_N + 9] = 1;
    }
    
    // Helper function to convert quaternion to rotation matrix
    void quaternionToRotationMatrix(float q0, float q1, float q2, float q3, float* R) {
        // Normalize quaternion
        float norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 /= norm;
        q1 /= norm;
        q2 /= norm;
        q3 /= norm;
        
        // Compute rotation matrix from quaternion
        R[0] = 1 - 2*q2*q2 - 2*q3*q3;  // R11
        R[1] = 2*q1*q2 - 2*q0*q3;      // R12
        R[2] = 2*q1*q3 + 2*q0*q2;      // R13
        
        R[3] = 2*q1*q2 + 2*q0*q3;      // R21
        R[4] = 1 - 2*q1*q1 - 2*q3*q3;  // R22
        R[5] = 2*q2*q3 - 2*q0*q1;      // R23
        
        R[6] = 2*q1*q3 - 2*q0*q2;      // R31
        R[7] = 2*q2*q3 + 2*q0*q1;      // R32
        R[8] = 1 - 2*q1*q1 - 2*q2*q2;  // R33
    }
    
    // Helper function to rotate a vector using a rotation matrix
    void rotateVector(float* R, float* v, float* result, bool inverse = false) {
        if (!inverse) {
            // v_rotated = R * v
            result[0] = R[0]*v[0] + R[1]*v[1] + R[2]*v[2];
            result[1] = R[3]*v[0] + R[4]*v[1] + R[5]*v[2];
            result[2] = R[6]*v[0] + R[7]*v[1] + R[8]*v[2];
        } else {
            // v_rotated = R^T * v (transpose of R)
            result[0] = R[0]*v[0] + R[3]*v[1] + R[6]*v[2];
            result[1] = R[1]*v[0] + R[4]*v[1] + R[7]*v[2];
            result[2] = R[2]*v[0] + R[5]*v[1] + R[8]*v[2];
        }
    }
};

// Instance of our EKF
ImuEKF ekf;

// Function to select a specific channel on the TCA9548A multiplexer
void tcaSelect(uint8_t channel) {
    if (channel > 7) return;
    
    Wire.beginTransmission(TCAADDR);
    Wire.write(1 << channel);
    Wire.endTransmission();
}

// Function to initialize all IMUs
bool initializeIMUs() {
    bool allInitialized = true;
    MPU9250Setting settings;
    
    // Configure settings for all IMUs
    settings.accel_fs_sel = ACCEL_FS_SEL::A16G;
    settings.gyro_fs_sel = GYRO_FS_SEL::G2000DPS;
    settings.mag_output_bits = MAG_OUTPUT_BITS::M16BITS;
    settings.fifo_sample_rate = FIFO_SAMPLE_RATE::SMPL_200HZ;
    settings.gyro_fchoice = 0x03;
    settings.gyro_dlpf_cfg = GYRO_DLPF_CFG::DLPF_41HZ;
    settings.accel_fchoice = 0x01;
    settings.accel_dlpf_cfg = ACCEL_DLPF_CFG::DLPF_45HZ;
    
    for (int i = 0; i < NUM_IMUS; i++) {
        // Select the appropriate channel on the multiplexer
        tcaSelect(imu_channels[i]);
        
        // Initialize IMU
        if (!imus[i].setup(imu_addresses[i], settings)) {
            Serial.print("Failed to initialize IMU ");
            Serial.println(i);
            allInitialized = false;
            continue;
        }
        
        // Set calibration values
        imus[i].setAccBias(acc_bias[i][0], acc_bias[i][1], acc_bias[i][2]);
        imus[i].setGyroBias(gyro_bias[i][0], gyro_bias[i][1], gyro_bias[i][2]);
        imus[i].setMagBias(mag_bias[i][0], mag_bias[i][1], mag_bias[i][2]);
        imus[i].setMagScale(mag_scale[i][0], mag_scale[i][1], mag_scale[i][2]);
        
        // Set magnetic declination for your location
        imus[i].setMagneticDeclination(-7.51); // Adjust for your location
        
        // Use MADGWICK filter for sensor fusion within each IMU
        imus[i].selectFilter(QuatFilterSel::MADGWICK);
        
        Serial.print("IMU ");
        Serial.print(i);
        Serial.println(" initialized successfully");
    }
    
    return allInitialized;
}

// Function to read data from all IMUs
void readAllIMUs(float acc_data[][3], float gyro_data[][3], float mag_data[][3], float quat_data[][4]) {
    for (int i = 0; i < NUM_IMUS; i++) {
        // Select the appropriate channel on the multiplexer
        tcaSelect(imu_channels[i]);
        
        // Update the IMU data (reads from sensor and runs internal fusion)
        if (imus[i].update()) {
            // Read accelerometer data (in G's)
            acc_data[i][0] = imus[i].getAccX();
            acc_data[i][1] = imus[i].getAccY();
            acc_data[i][2] = imus[i].getAccZ();
            
            // Read gyroscope data (in degrees/sec)
            gyro_data[i][0] = imus[i].getGyroX();
            gyro_data[i][1] = imus[i].getGyroY();
            gyro_data[i][2] = imus[i].getGyroZ();
            
            // Read magnetometer data (in μT)
            mag_data[i][0] = imus[i].getMagX();
            mag_data[i][1] = imus[i].getMagY();
            mag_data[i][2] = imus[i].getMagZ();
            
            // Read quaternion data
            quat_data[i][0] = imus[i].getQuaternionW();
            quat_data[i][1] = imus[i].getQuaternionX();
            quat_data[i][2] = imus[i].getQuaternionY();
            quat_data[i][3] = imus[i].getQuaternionZ();
        } else {
            Serial.print("Failed to read IMU ");
            Serial.println(i);
        }
    }
}

// Function to fuse IMU data using weighted average
// This is a simple approach before feeding into the EKF
void fuseIMUData(float acc_data[][3], float gyro_data[][3], float mag_data[][3], float quat_data[][4], 
                float fused_acc[3], float fused_gyro[3], float fused_mag[3], float fused_quat[4]) {
    
    // Weights for each IMU (can be adjusted based on sensor reliability)
    float weights[NUM_IMUS] = {1.0/NUM_IMUS, 1.0/NUM_IMUS, 1.0/NUM_IMUS};
    
    // Initialize fused data to zero
    for (int j = 0; j < 3; j++) {
        fused_acc[j] = 0;
        fused_gyro[j] = 0;
        fused_mag[j] = 0;
    }
    for (int j = 0; j < 4; j++) {
        fused_quat[j] = 0;
    }
    
    // Compute weighted average
    for (int i = 0; i < NUM_IMUS; i++) {
        for (int j = 0; j < 3; j++) {
            fused_acc[j] += weights[i] * acc_data[i][j];
            fused_gyro[j] += weights[i] * gyro_data[i][j];
            fused_mag[j] += weights[i] * mag_data[i][j];
        }
        for (int j = 0; j < 4; j++) {
            fused_quat[j] += weights[i] * quat_data[i][j];
        }
    }
    
    // Normalize quaternion
    float qnorm = sqrt(fused_quat[0]*fused_quat[0] + fused_quat[1]*fused_quat[1] + 
                       fused_quat[2]*fused_quat[2] + fused_quat[3]*fused_quat[3]);
    
    for (int j = 0; j < 4; j++) {
        fused_quat[j] /= qnorm;
    }
}

// Function to convert quaternion to Euler angles (roll, pitch, yaw)
void quaternionToEuler(float q[4], float euler[3]) {
    // Roll (x-axis rotation)
    float sinr_cosp = 2.0 * (q[0] * q[1] + q[2] * q[3]);
    float cosr_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
    euler[0] = atan2(sinr_cosp, cosr_cosp);
    
    // Pitch (y-axis rotation)
    float sinp = 2.0 * (q[0] * q[2] - q[3] * q[1]);
    if (abs(sinp) >= 1)
        euler[1] = copysign(M_PI / 2, sinp); // Use 90 degrees if out of range
    else
        euler[1] = asin(sinp);
    
    // Yaw (z-axis rotation)
    float siny_cosp = 2.0 * (q[0] * q[3] + q[1] * q[2]);
    float cosy_cosp = 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]);
    euler[2] = atan2(siny_cosp, cosy_cosp);
    
    // Convert to degrees
    euler[0] *= 180.0 / M_PI;
    euler[1] *= 180.0 / M_PI;
    euler[2] *= 180.0 / M_PI;
}

void setup() {
    Serial.begin(115200);
    delay(1000); // Give serial monitor time to open
    
    Wire.begin();
    Wire.setClock(400000); // Set I2C to 400kHz
    
    Serial.println("Initializing IMUs...");
    if (initializeIMUs()) {
        Serial.println("All IMUs initialized successfully");
    } else {
        Serial.println("Error initializing one or more IMUs");
    }
    
    // Initialize the EKF
    ekf.init();
    Serial.println("EKF initialized");
    
    delay(1000);
}

void loop() {
    // Arrays to store data from all IMUs
    float acc_data[NUM_IMUS][3];
    float gyro_data[NUM_IMUS][3];
    float mag_data[NUM_IMUS][3];
    float quat_data[NUM_IMUS][4];
    
    // Fused data (simple weighted average)
    float fused_acc[3];
    float fused_gyro[3];
    float fused_mag[3];
    float fused_quat[4];
    
    // Read data from all IMUs
    readAllIMUs(acc_data, gyro_data, mag_data, quat_data);
    
    // Simple fusion step (weighted average)
    fuseIMUData(acc_data, gyro_data, mag_data, quat_data, fused_acc, fused_gyro, fused_mag, fused_quat);
    
    // Create measurement vector for EKF
    float z[EKF_M];
    
    // Convert accelerometer data from G's to m/s² and fill measurement vector
    z[0] = fused_acc[0] * GRAVITY;
    z[1] = fused_acc[1] * GRAVITY;
    z[2] = fused_acc[2] * GRAVITY;
    
    // Convert gyro data from deg/s to rad/s
    z[3] = fused_gyro[0] * M_PI / 180.0;
    z[4] = fused_gyro[1] * M_PI / 180.0;
    z[5] = fused_gyro[2] * M_PI / 180.0;
    
    // Magnetometer data (already in correct units)
    z[6] = fused_mag[0];
    z[7] = fused_mag[1];
    z[8] = fused_mag[2];
    
    // Process noise matrix
    float Q[EKF_N * EKF_N] = {0};
    // Fill diagonal elements with process noise values
    for (int i = 0; i < EKF_N; i++) {
        Q[i * EKF_N + i] = (i < 3) ? 0.01 : (i < 6) ? 0.1 : 0.01;
    }
    
    // Measurement noise matrix
    float R[EKF_M * EKF_M] = {0};
    // Fill diagonal elements with measurement noise values
    for (int i = 0; i < EKF_M; i++) {
        R[i * EKF_M + i] = (i < 3) ? 0.1 : (i < 6) ? 0.01 : 0.1;
    }
    
    // Create arrays for EKF prediction
    float fx[EKF_N];
    float F[EKF_N * EKF_N];
    float hx[EKF_M];
    float H[EKF_M * EKF_N];
    
    // Run EKF prediction step
    ekf.model(fx, F, hx, H);
    ekf_predict(&ekf, fx, F, Q);
    
    // Run EKF update step with our measurements
    bool update_successful = ekf_update(&ekf, z, hx, H, R);
    
    if (update_successful) {
        // Extract the estimated state
        float position[3] = {ekf.x[0], ekf.x[1], ekf.x[2]};
        float velocity[3] = {ekf.x[3], ekf.x[4], ekf.x[5]};
        float quaternion[4] = {ekf.x[6], ekf.x[7], ekf.x[8], ekf.x[9]};
        
        // Convert quaternion to Euler angles for easier interpretation
        float euler[3];
        quaternionToEuler(quaternion, euler);
        
        // Print results
        Serial.println("EKF Estimate:");
        
        Serial.print("Position (m): X=");
        Serial.print(position[0], 3);
        Serial.print(" Y=");
        Serial.print(position[1], 3);
        Serial.print(" Z=");
        Serial.println(position[2], 3);
        
        Serial.print("Velocity (m/s): X=");
        Serial.print(velocity[0], 3);
        Serial.print(" Y=");
        Serial.print(velocity[1], 3);
        Serial.print(" Z=");
        Serial.println(velocity[2], 3);
        
        Serial.print("Orientation (deg): Roll=");
        Serial.print(euler[0], 1);
        Serial.print(" Pitch=");
        Serial.print(euler[1], 1);
        Serial.print(" Yaw=");
        Serial.println(euler[2], 1);
        
        Serial.println();
    } else {
        Serial.println("EKF update failed");
    }
    
    delay(10); // 100Hz update rate
}

This implementation provides a comprehensive solution for fusing data from multiple MPU9250 IMU sensors using an Extended Kalman Filter. Here's how it works:
Key Components:

I2C Multiplexer Handling:

The code uses a TCA9548A I2C multiplexer to connect to multiple MPU9250 sensors
The tcaSelect() function switches between different I2C channels

IMU Configuration:

Each IMU is initialized with appropriate settings (sample rates, filter configurations)
Individual calibration values can be set for each sensor

Data Fusion Approach:

First Layer: Each MPU9250 has internal sensor fusion (using Madgwick filter)
Second Layer: Simple weighted averaging of data from all IMUs
Third Layer: Extended Kalman Filter for optimal state estimation

State Representation:

10-dimensional state vector: [position (3), velocity (3), quaternion (4)]
The quaternion represents orientation without gimbal lock issues

EKF Implementation:

Customized prediction and update steps for IMU fusion
Includes quaternion-to-rotation matrix conversion for proper sensor modeling
Carefully tuned process and measurement noise matrices

Additional Implementation Notes:

Calibration:
You should first calibrate each IMU individually (using their built-in calibration functions) and save those values in the acc_bias, gyro_bias, mag_bias, and mag_scale arrays.
Tuning the Filter:
The process noise (Q) and measurement noise (R) matrices will need tuning based on your specific setup. As implemented, the filter assumes:

Position has low process noise
Velocity has higher process noise
Acceleration measurements have higher noise than gyroscope measurements

Magnetic Declination:
Set the appropriate magnetic declination for your geographic location to get accurate heading information.
Gravity Handling:
The filter accounts for gravity in the accelerometer measurements to properly estimate linear acceleration.
Future Improvements:

More sophisticated outlier rejection techniques
Better quaternion dynamics modeling using gyroscope data
Integration of additional sensors like barometers for height estimation