### IMU_EKF Usage Example in C++

Source: https://github.com/hobbeshunter/imu_ekf/blob/master/README.md

Demonstrates how to initialize and use the IMU_EKF filter in a typical Arduino `setup()` and `loop()` structure. It covers sensor reading, filter prediction and correction steps for gyroscope, accelerometer, and magnetometer, and retrieving the estimated attitude.

```C++
#include <Eigen.h>
#include <ESKF.h>
#include <Quaternion.h>

// magnetometer calibration
Eigen::Matrix<float, 3, 3> W; // soft-iron
Eigen::Matrix<float, 3, 1> V; // hard-iron
float incl; // inclination
float B; // geomagnetic field strength

IMU_EKF::ESKF<float> filter;

void setup()
{
    // fill magnetometer calibration stuff
    // Winv << 
    // W << 
    // B = 
    // incl = 

    float ax, ay, az;
    // read accelerometer
    filter.initWithAcc(ax, ay, az);
}

void loop()
{
    float dt;
    bool readMag; // assume magnetometer has lower update rate as acellerometer and gyroscope

    // calculate dt

    // read sensors

    filter.predict(dt);
    filter.correctGyr(gx, gy, gz);
    filter.correctAcc(ax, ay, az);
    if (readMag)
    {
        filter.correctMag(mx, my, mz, incl, B, W, V);
    }
    filter.reset();

    // get attitude as roll, pitch, yaw
    float roll, pitch, yaw;
    filter.getAttitude(roll, pitch, yaw);
    // or quaternion
    IMU_EKF::Quaternion<float> q = filter.getAttitude();
}
```

--------------------------------

### Complete IMU_EKF Usage Example in PlatformIO C++

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

A full example demonstrating the typical usage of the IMU_EKF filter in a PlatformIO embedded project for continuous sensor fusion. It includes sensor reading, filter initialization, prediction, correction steps for gyroscope, accelerometer, and magnetometer, and attitude retrieval. Requires Eigen and ESKF headers.

```cpp
#include <Eigen.h>
#include <ESKF.h>
#include <Quaternion.h>

// Magnetometer calibration (obtain through calibration procedure)
Eigen::Matrix<float, 3, 3> W;
Eigen::Matrix<float, 3, 3> Winv;
Eigen::Matrix<float, 3, 1> V;
float incl = 60.0f * M_PI / 180.0f;  // Local magnetic inclination
float B = 48.0f;                      // Local field strength (uT)

IMU_EKF::ESKF<float> filter;
unsigned long lastTime;
int magUpdateCounter = 0;

void setup() {
    Serial.begin(115200);

    // Initialize magnetometer calibration
    W.setIdentity();
    Winv.setIdentity();
    V.setZero();

    // Initialize IMU sensors here...

    // Read initial accelerometer for tilt initialization
    float ax, ay, az;
    // readAccelerometer(ax, ay, az);  // Your sensor read function
    ax = 0.0f; ay = 0.0f; az = -1.0f;  // Example: level orientation

    filter.initWithAcc(ax, ay, az);
    lastTime = micros();
}

void loop() {
    // Calculate time delta
    unsigned long currentTime = micros();
    float dt = (currentTime - lastTime) / 1000000.0f;
    lastTime = currentTime;

    // Read sensors (replace with actual sensor reads)
    float ax, ay, az;     // Accelerometer (g)
    float gx, gy, gz;     // Gyroscope (deg/s)
    float mx, my, mz;     // Magnetometer (uT)
    // readAccelerometer(ax, ay, az);
    // readGyroscope(gx, gy, gz);
    // readMagnetometer(mx, my, mz);

    // Example sensor values
    ax = 0.01f; ay = -0.02f; az = -0.98f;
    gx = 1.5f; gy = -0.3f; gz = 0.1f;
    mx = 20.0f; my = 5.0f; mz = -42.0f;

    // Run filter
    filter.predict(dt);
    filter.correctGyr(gx, gy, gz);
    filter.correctAcc(ax, ay, az);

    // Magnetometer typically has lower update rate
    magUpdateCounter++;
    if (magUpdateCounter >= 10) {
        filter.correctMag(mx, my, mz, incl, B, W, V);
        magUpdateCounter = 0;
    }

    filter.reset();

    // Get attitude
    float roll, pitch, yaw;
    filter.getAttitude(roll, pitch, yaw);

    // Output results
    Serial.print("Roll: "); Serial.print(roll * 180.0f / M_PI);
    Serial.print(" Pitch: "); Serial.print(pitch * 180.0f / M_PI);
    Serial.print(" Yaw: "); Serial.println(yaw * 180.0f / M_PI);

    delay(10);  // ~100 Hz update rate
}
```

--------------------------------

### Initialize ESKF Filter with Accelerometer (C++)

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Initializes the ESKF filter using accelerometer readings to estimate initial roll and pitch angles. This method is useful when the device is stationary, providing a better starting orientation than the default identity. Yaw is initialized to zero.

```cpp
IMU_EKF::ESKF<float> filter;

// Read initial accelerometer values (in g units)
float ax = 0.02f;   // x-axis acceleration
float ay = -0.01f;  // y-axis acceleration
float az = -0.99f;  // z-axis acceleration (pointing down when level)

// Initialize filter with accelerometer to get initial tilt
filter.initWithAcc(ax, ay, az);
// Filter now has initial roll and pitch estimated from gravity vector
// Yaw is initialized to 0
```

--------------------------------

### Get Linear Acceleration from ESKF

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Retrieves the current estimated linear acceleration from the filter's state vector. These acceleration values are provided in meters per second squared (m/s^2). It's important to note that these estimates are considered experimental and may not be sufficiently reliable for critical navigation tasks.

```cpp
IMU_EKF::ESKF<float> filter;

// After running filter updates...
float accX, accY, accZ;
filter.getAcceleration(accX, accY, accZ);
// Acceleration values are in m/s^2
// Note: Linear acceleration estimates are experimental
```

--------------------------------

### Get Attitude as Quaternion from ESKF

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Retrieves the current estimated attitude from the filter as a quaternion object. This representation is beneficial for avoiding gimbal lock issues and is commonly used in 3D graphics and advanced motion tracking. The quaternion can be converted to a rotation matrix for further transformations.

```cpp
IMU_EKF::ESKF<float> filter;

// After running filter updates...
IMU_EKF::Quaternion<float> q = filter.getAttitude();

// Access quaternion components
float qx = q[IMU_EKF::v1];  // Vector part x
float qy = q[IMU_EKF::v2];  // Vector part y
float qz = q[IMU_EKF::v3];  // Vector part z
float qw = q[IMU_EKF::w];   // Scalar part

// Convert to rotation matrix for 3D transforms
Eigen::Matrix<float, 3, 3> R = q.toRotationMatrix();
```

--------------------------------

### Get Attitude as Euler Angles from ESKF

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Retrieves the current estimated attitude from the filter in the form of Euler angles (roll, pitch, and yaw) in radians. The output can then be converted to degrees for display or other applications. This function is called after the filter has processed sensor data.

```cpp
IMU_EKF::ESKF<float> filter;

// After running filter updates...
float roll, pitch, yaw;
filter.getAttitude(roll, pitch, yaw);

// Convert to degrees for display
float rollDeg = roll * 180.0f / M_PI;
float pitchDeg = pitch * 180.0f / M_PI;
float yawDeg = yaw * 180.0f / M_PI;

Serial.print("Roll: "); Serial.print(rollDeg);
Serial.print(" Pitch: "); Serial.print(pitchDeg);
Serial.print(" Yaw: "); Serial.println(yawDeg);
```

--------------------------------

### Get Full State Vector from ESKF

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Retrieves the complete 15-dimensional state vector estimated by the filter. This vector encompasses position, velocity, acceleration, attitude error (using small-angle approximation), and angular velocity. The state vector is returned as an Eigen matrix, allowing direct access to its components.

```cpp
IMU_EKF::ESKF<float> filter;

// Get full state vector
Eigen::Matrix<float, 15, 1> state = filter.getState();

// State vector layout:
// [0-2]:   Position (x, y, z) in meters
// [3-5]:   Velocity (x, y, z) in m/s
// [6-8]:   Acceleration (x, y, z) in m/s^2
// [9-11]:  Attitude error (small angle approximation)
// [12-14]: Angular velocity (x, y, z) in rad/s

float angularVelX = state(12);
float angularVelY = state(13);
float angularVelZ = state(14);
```

--------------------------------

### Quaternion Math Operations in C++

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Demonstrates quaternion operations including creation, multiplication, normalization, and conversion to rotation matrices and Euler angles. Requires the Eigen library for matrix operations.

```cpp
// Create identity quaternion (no rotation)
IMU_EKF::Quaternion<float> q1;

// Create quaternion with components (v1, v2, v3, w)
IMU_EKF::Quaternion<float> q2(0.0f, 0.0f, 0.3827f, 0.9239f);  // 45 deg around z-axis

// Quaternion multiplication (combine rotations)
IMU_EKF::Quaternion<float> q3 = q1 * q2;

// Normalize quaternion
q3.normalize();

// Convert to rotation matrix
Eigen::Matrix<float, 3, 3> R = q3.toRotationMatrix();

// Convert to Euler angles (roll, pitch, yaw in radians)
Eigen::Matrix<float, 3, 1> euler = q3.toEulerAngles();
float roll = euler(0);
float pitch = euler(1);
float yaw = euler(2);

// Access quaternion components
float v1 = q3[IMU_EKF::v1];
float v2 = q3[IMU_EKF::v2];
float v3 = q3[IMU_EKF::v3];
float w = q3[IMU_EKF::w];
```

--------------------------------

### Instantiate ESKF Filter (C++)

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Demonstrates how to create instances of the ESKF filter class for both single-precision (float) and double-precision (double) types. The filter is templated to support different floating-point precisions.

```cpp
#include <Eigen.h>
#include <ESKF.h>
#include <Quaternion.h>

// Create a single-precision ESKF filter instance
IMU_EKF::ESKF<float> filter;

// Or use double precision for higher accuracy
IMU_EKF::ESKF<double> filterDouble;
```

--------------------------------

### Initialize ESKF Filter with Accelerometer and Magnetometer (C++)

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Initializes the ESKF filter using both accelerometer and magnetometer readings to estimate the complete initial orientation, including yaw. This method requires magnetometer calibration parameters (soft-iron and hard-iron compensation) for accurate results.

```cpp
IMU_EKF::ESKF<float> filter;

// Magnetometer calibration matrices
Eigen::Matrix<float, 3, 3> Winv;  // Inverse soft-iron compensation matrix
Winv << 1.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 1.0f;

Eigen::Matrix<float, 3, 1> V;     // Hard-iron offset vector
V << 0.0f, 0.0f, 0.0f;

// Initial sensor readings
float ax = 0.02f, ay = -0.01f, az = -0.99f;  // Accelerometer (g)
float mx = 0.2f, my = 0.1f, mz = -0.4f;       // Magnetometer (normalized)

// Initialize with full orientation including yaw
filter.initWithAccAndMag(ax, ay, az, mx, my, mz, Winv, V);
// Filter now has complete initial orientation (roll, pitch, yaw)
```

--------------------------------

### Initialize ESKF Filter to Default (C++)

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Shows how to reinitialize the ESKF filter to its default state. This resets the reference quaternion to identity, the state vector to zeros, and initializes covariance matrices with default noise values. This is typically called automatically by the constructor.

```cpp
IMU_EKF::ESKF<float> filter;

// Explicitly reinitialize if needed
filter.init();
// State vector is now zeros
// Reference quaternion is identity (no rotation)
// Covariance matrices are initialized with default noise values
```

--------------------------------

### Apply Magnetometer Correction with ESKF

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Performs a Kalman filter correction step using magnetometer measurements to adjust the yaw estimate. This requires pre-calibrated magnetometer data (soft-iron and hard-iron compensation) and local magnetic field parameters like inclination and field strength. The function takes raw magnetometer readings and calibration data as input.

```cpp
IMU_EKF::ESKF<float> filter;

// Magnetometer calibration
Eigen::Matrix<float, 3, 3> W;  // Soft-iron compensation matrix
W << 1.0f, 0.0f, 0.0f,
     0.0f, 1.0f, 0.0f,
     0.0f, 0.0f, 1.0f;

Eigen::Matrix<float, 3, 1> V;  // Hard-iron offset
V << 0.0f, 0.0f, 0.0f;

// Local magnetic field parameters (location dependent)
float incl = 60.0f * M_PI / 180.0f;  // Inclination angle (radians)
float B = 48.0f;                      // Field strength (microtesla)

// Read magnetometer values
float mx = 20.0f, my = 5.0f, mz = -42.0f;

// Apply magnetometer correction
filter.correctMag(mx, my, mz, incl, B, W, V);
// Yaw estimate is now corrected using earth's magnetic field reference
```

--------------------------------

### Apply Accelerometer Correction with ESKF

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Applies the Kalman filter correction step using accelerometer measurements to refine tilt estimates (roll and pitch) based on gravity. If the device is not undergoing significant acceleration, linear acceleration states are also updated. This function takes raw accelerometer readings as input.

```cpp
IMU_EKF::ESKF<float> filter;

// Read accelerometer values (in g units)
float ax = 0.01f;   // x-axis acceleration (g)
float ay = -0.02f;  // y-axis acceleration (g)
float az = -0.98f;  // z-axis acceleration (g)

// Apply accelerometer correction
filter.correctAcc(ax, ay, az);
// Tilt estimates (roll, pitch) are now corrected using gravity reference
// If |acceleration| is close to 1g, linear acceleration states are also updated
```

--------------------------------

### Predict ESKF State with Time Delta (C++)

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Performs the prediction step of the Kalman filter using the current angular velocity estimate and a time delta. This function propagates the filter's state forward in time and updates the covariance matrix.

```cpp
IMU_EKF::ESKF<float> filter;
filter.initWithAcc(0.0f, 0.0f, -1.0f);

// In your main loop
unsigned long lastTime = micros();

void loop() {
    unsigned long currentTime = micros();
    float dt = (currentTime - lastTime) / 1000000.0f;  // Convert to seconds
    lastTime = currentTime;

    // Perform prediction step with time delta
    filter.predict(dt);
    // State and covariance are now propagated forward by dt seconds
}
```

--------------------------------

### Correct ESKF State with Gyroscope Measurement (C++)

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Applies a Kalman filter correction step using gyroscope measurements. This function updates the angular velocity estimate within the filter's state vector. Gyroscope values should be provided in degrees per second.

```cpp
IMU_EKF::ESKF<float> filter;

// Read gyroscope values (in degrees per second)
float gx = 1.5f;   // Roll rate (deg/s)
float gy = -0.3f;  // Pitch rate (deg/s)
float gz = 0.8f;   // Yaw rate (deg/s)

// Apply gyroscope correction
filter.correctGyr(gx, gy, gz);
// Angular velocity state is now updated with gyroscope measurement
```

--------------------------------

### Reset ESKF Error State

Source: https://context7.com/hobbeshunter/imu_ekf/llms.txt

Resets the error state of the Extended Kalman Filter by incorporating the attitude error into the reference quaternion. This operation is crucial and must be performed after all correction steps within each iteration to ensure filter stability and prevent error accumulation. It takes the time delta for prediction as input.

```cpp
IMU_EKF::ESKF<float> filter;

// After all corrections in the loop
filter.predict(dt);
filter.correctGyr(gx, gy, gz);
filter.correctAcc(ax, ay, az);
filter.correctMag(mx, my, mz, incl, B, W, V);

// Reset error state - MUST be called after corrections
filter.reset();
// Attitude error is now folded into reference quaternion
// Error state is reset to zero
```

=== COMPLETE CONTENT === This response contains all available snippets from this library. No additional content exists. Do not make further requests.